#include "superstructure.hpp"

using namespace ary;
using namespace globals;

bool ptoEnabled = false;
bool wingsOpen = false;
bool intakeEngaged = false;

/*
  SCALE SPEEDS: Determines what percentage speeds of autonomous movements should move at
    speedScale -> The scale of how fast the drivetrain goes forward and backwards
    turnScale -> The scale of how fast the drivetrain turns
    swingScale -> The scale of fast one side of the chassis moves
*/

double speedScale = 1.0;
double turnScale = 1.0;
double swingScale = 1.0;
namespace superstruct {
  void chassisInit() {
    /*
      When the robot first starts up we want to do a couple things:
      - Adjust the drivetrain curve bottons so it does not interfere with any of the driver controls.
      - Enable the joystick curve
      - Enable active break on the drive
        - Active break is a P controller applied to the drivetrain in order to help it maintain it's position, resisting external forces.
        - 
    */

    chassis.set_curve_buttons(pros::E_CONTROLLER_DIGITAL_LEFT, pros::E_CONTROLLER_DIGITAL_RIGHT);
    chassis.toggle_modify_curve_with_controller(true);
    chassis.set_active_brake(0.1);
    chassis.set_curve_default(0.375, 0.375);

    /* Adjust the adjust the factor by which the drive velocity is adjusted */ 
    chassis.set_joystick_drivescale(1.0);
    chassis.set_joystick_turnscale(1.0); 

    // chassis_odom.calibrate();
    // chassis_odom.setPose(0, 0, 0);
  }

  void telemetry() {
    while (true) {
      lemlib::Pose bot_pose = chassis_odom.getPose();
      pros::lcd::print(0, "x: %f", bot_pose.x); // print the x position
        pros::lcd::print(1, "y: %f", bot_pose.y); // print the y position
        pros::lcd::print(2, "heading: %f", bot_pose.theta); // print the heading
        pros::delay(10);
    }
  }

  void opControlInit() {
    motorsCoast();
	  disableActiveBrake();
  }

  // Adjust exit conditions to allow for quick movements
  void configureExitConditions() {
    chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 500, 500);
    chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);
    chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 500, 500);
  }

  // Adjust PID constants for accurate movements
  void configureConstants() {
    chassis.set_slew_min_power(80, 80);
    chassis.set_slew_distance(7, 7);
    chassis.set_pid_constants(&chassis.headingPID, 16, 0, 32, 0);
    chassis.set_pid_constants(&chassis.forward_drivePID, 0.5, 0, 5, 0);
    chassis.set_pid_constants(&chassis.backward_drivePID, 0.5, 0, 5, 0);
    chassis.set_pid_constants(&chassis.turnPID, 6.25, 0.003, 57, 15);
    chassis.set_pid_constants(&chassis.swingPID, 8.5, 0, 50, 0);
  }

  // Prepare the bot for the autonomous period of a match
  void autonomousResets() {
    chassis.reset_pid_targets();
  	chassis.reset_gyro();
  	chassis.reset_drive_sensor();
    configureConstants();
    configureExitConditions();
    motorsBrake();
    
  }

  void motorsCoast() {
    chassis.set_drive_brake(MOTOR_BRAKE_COAST);
  }

  void motorsHold() {
    chassis.set_drive_brake(MOTOR_BRAKE_HOLD);
  }

  void motorsBrake() {
    chassis.set_drive_brake(MOTOR_BRAKE_BRAKE);
  }

  // The chassis will not apply a constant voltage to prevent it from being moved
  void disableActiveBrake() {
    chassis.set_active_brake(0.0);
  }


  // Drives forward, runs next commands WITHOUT waiting for the drive to complete 
  void driveAsync(double dist, bool useHeadingCorrection) {
    //chassis.set_mode(ary::DRIVE);
    chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection); 
  }

  // Drives forward, runs next commands AFTER waiting for the drive to complete 
  void driveSync(double dist, bool useHeadingCorrection) {
    //chassis.set_mode(ary::DRIVE);
    chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection); 
    chassis.wait_drive();
  }

  // Drives forward, runs next commands AFTER reaching a certain measurement/error along the path
  void driveWithMD(double dist, bool useHeadingCorrection, double waitUntilDist) {
    //chassis.set_mode(ary::DRIVE);
    chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection); 
    chassis.wait_until(waitUntilDist);
  }

  // Turns the chassis, runs other commands after it has run.
  void turnSync(double theta) {
    //chassis.set_mode(ary::TURN);
    chassis.set_turn(theta, TURN_SPEED * turnScale);
    chassis.wait_drive();
  }

  // Turns the chassis, runs other commands immediately after call
  void turnAsync(double theta) {
    //chassis.set_mode(ary::TURN);
    chassis.set_turn(theta, TURN_SPEED * turnScale);
  }

  // Moves only the right side of the chassis so it can make a left turn
  void leftSwing(double theta) {
    //chassis.set_mode(SWING);
    chassis.set_swing(LEFT_SWING, theta, SWING_SPEED * swingScale);
  }

  // Moves only the left side of the chassis so it can make a right turn
  void rightSwing(double theta) {
    //chassis.set_mode(SWING);
    chassis.set_swing(RIGHT_SWING, theta, SWING_SPEED * swingScale);
  }

  /*
    Each of the scale values must be clamed between 0.1 - 1 (10% to 100%) to avoid saturation of motors.
  */

  void setDriveScale(double val) {
    speedScale = std::clamp(val, 0.1, 1.0);
  }

  void setTurnScale(double val) {
    turnScale = std::clamp(val, 0.1, 1.0);
  }

  void setSwingScale(double val) {
    swingScale = std::clamp(val, 0.1, 1.0); // 
  }

  // Structure methods
  void intakeControl(pros::controller_digital_e_t intakeButton) {
    if (globals::master.get_digital_new_press(intakeButton)) {
      if (intakeEngaged == false) {
        intake_piston.set_value(1);
        intakeEngaged = true;
      } else if (intakeEngaged == true) {
        intake_piston.set_value(0);
        intakeEngaged = false;
      }
    }
  }

  void togglePto(bool toggle) {
    ptoEnabled = toggle;
    chassis.pto_toggle({cata_left, cata_right}, toggle); // Configure the listed PTO motors to whatever value toggle is.
    pto_piston.set_value(toggle);

    if (toggle) {
      cata_left.set_brake_mode(MOTOR_BRAKE_COAST);
      cata_right.set_brake_mode(MOTOR_BRAKE_COAST);
    }
  }

  void runCata(double inpt) {
    if (!ptoEnabled) return;
    //cata_left = inpt;
    cata_right = inpt;
  }

  void runAntiBlock(double inpt) {
    if (!ptoEnabled) return;
    cata_left = inpt;
  }

  /*
    TODO:
      - Make anti block passable button
  */

  int lock = 0;
  void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton) {
    if (globals::master.get_digital(ptoToggleButton) && lock == 0) { // If the PTO button has been pressed and the PTO is not engaged
      togglePto(!ptoEnabled); // Toggle the PTO so that cataput is useable
      lock = 1;
    } else if(!globals::master.get_digital(ptoToggleButton)) {
      lock = 0;
    }

    if (globals::master.get_digital(cataRunButton)) {
			runCata(-12000);
		} else {
			runCata(0);
		}

    if (globals::master.get_digital(DIGITAL_UP)) {
      runAntiBlock(-12000);
    } else if (globals::master.get_digital(DIGITAL_LEFT)) {
      runAntiBlock(12000);
    } else {
      runAntiBlock(0);
    }
  }

  int climb_state = 0;
  void climbControl(pros::controller_digital_e_t climbButton) {
    if (globals::master.get_digital_new_press(climbButton)) {
      if (climb_state == 0) {
        climb_piston.set_value(1);
        climb_state = 1;
      } else if (climb_state == 1) {
        climb_piston.set_value(0);
        climb_state = 0;
      }
    }
  }

  void wingsControlSingle(pros::controller_digital_e_t wingControlButton) {
    if (globals::master.get_digital_new_press(wingControlButton)) {
      if (wings.getState() == 0) // A value of 0 indicates that both wings are closed
        wings.open();
      else if (wings.getState() == 3) // A value of 3 indicates that both wings are open
        wings.close();
    }
  }

  void toggleIntake(bool val) {
    int valTo = (val == true) ? 1 : 0;
    intake_piston.set_value(valTo);
  }

  // void toggleRemovalMech(bool val) {
  //   int valTo = (val == true) ? 1 : 0;
  //   doinker_piston.set_value(valTo);
  // }

  /*
    Handle respective controls
  */

  void renu_control() {
    subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL);
    wingsControlSingle(RENU_WING_CONTROL);
    intakeControl(RENU_INTAKE_CONTROL);
    climbControl(RENU_CLIMB_CONTROL);
  }
  
  void ria_control() {
    subsysControl(RIA_PTO_TOGGLE, RIA_CATA_CONTROL);
    wingsControlSingle(RIA_WINGS_CONTROL);
    intakeControl(RIA_INTAKE_CONTROL);
  }

  void chris_control() {
    subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL);
    wingsControlSingle(RENU_WING_CONTROL);
    intakeControl(RENU_INTAKE_CONTROL);
  }
}