#include "main.h"

#define DRIVE_SPEED 120
#define TURN_SPEED 105
#define SWING_SPEED 90
// R1 -> WINGS, L1 -> CATA, L2 -> PTO, R2 -> INTAKE
// Renu's control preferences
#define RENU_PTO_TOGGLE DIGITAL_R1
#define RENU_CATA_CONTROL DIGITAL_A
#define RENU_INTAKE_CONTROL_INTAKE DIGITAL_L1
#define RENU_INTAKE_CONTROL_OUTTAKE DIGITAL_L2
#define RENU_WING_CONTROL DIGITAL_R2

// Ria's control preferences
#define RIA_PTO_TOGGLE DIGITAL_LEFT
#define RIA_CATA_CONTROL DIGITAL_A
#define RIA_INTAKE_CONTROL DIGITAL_R1
#define RIA_WINGS_CONTROL DIGITAL_L1


namespace superstruct {
    /* Configs */
    void chassisInit();
    void opControlInit();
    void configureExitConditions();
    void configureConstants();
    void autonomousResets();
    void motorsCoast();
    void motorsHold();
    void motorsBrake();

    void disableActiveBrake();
    
    /* Motion */
    void driveAsync(double dist, bool useHeadingCorrection);
    void driveSync(double dist, bool useHeadingCorrection);
    void driveWithMD(double dist, bool useHeadingCorrection, double waitUntilDist);

    void turnSync(double theta);
    void turnAsync(double theta);

    void leftSwing(double theta);
    void rightSwing(double theta);
    
    void setDriveScale(double val);
    void setTurnScale(double val);
    void setSwingScale(double val);

    /* Structure */
    void togglePto(bool toggle);
    void runCata(double inpt);
    void runIntake(double inpt);
    void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton, pros::controller_digital_e_t intakeButton, pros::controller_digital_e_t outtakeButton);
    void wingsControl(pros::controller_digital_e_t wingControlButton);

    /* Controls */
    void renu_control();
    void ria_control();
    void chris_control();  
}