#include "main.h"
#include "EZ-Template/api.hpp"
#include "api.h"

#pragma once

#ifndef CONFIG_H
#define CONFIG_H

extern bool gotRed;
extern bool gotBlue;
extern double currentColor;

extern Drive chassis;

// Declares the left side motors
extern pros::Motor FrontLeft;
extern pros::Motor MiddleLeft;
extern pros::Motor BackLeft;

// Declares the right side motors
extern pros::Motor FrontRight;
extern pros::Motor MiddleRight;
extern pros::Motor BackRight;

// Declares the left and right  drive motor groups.
extern pros::MotorGroup RightSide;
extern pros::MotorGroup LeftSide;

// Declares all the drive motors as a group.
extern pros::MotorGroup FullDrive;

// Declaring the Intake motors.
inline pros::Motor stage1({14});
inline pros::Motor stage2({16});
inline pros::Motor stage3({12});

//
inline ez::Piston ramp({'H',true});

// Declaring the piston used to guide the balls into the basket.
inline ez::Piston deScorer({'G',false});

// Declaring the piston used to get the balls out of the tube.
inline ez::Piston matchLoader({'F',false});

//
inline pros::Optical colorSensorOne({20});
inline pros::Optical colorSensorTwo({11});
inline pros::Optical colorSensorThree({18});

//
inline pros::Distance frontDistance({5});

//inline pros::MotorGroup fillingGroup({20, 17, 18});
//inline pros::MotorGroup scoringGroup({20, 17, 18, 19});

// Function used for the Intake control
void IntakePresets();
void ToggleButtons();

// Function used for the color sorter
void ColorDisplay();
void CheckRed(std::string Sensor, std::string controlPeriod, bool loop);
void CheckBlue(std::string Sensor, std::string controlPeriod, bool loop);
void ColorDetection(std::string wantedColor, bool checkForColor);

// Auton voids
void leftMain(std::string side);
void rightMain(std::string side);
void Skills();
void BlueRight();
void BlueLeft();
void RedRight();
void RedLeft();
void testMain(std::string side);
void Test();

#endif