#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include <fstream>

#include "vex.h"

using namespace vex;

// Brain should be defined by default
brain Brain;


// START V5 MACROS
#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

#define repeat(iterations)                                                     \
  for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS


// Robot configuration code.
controller vexController = controller(primary); // Instance of the VEX controller
motor leftMotorA = motor(PORT19, ratio6_1, false);
motor leftMotorB = motor(PORT20, ratio6_1, false);
motor_group LeftDriveSmart = motor_group(leftMotorA, leftMotorB);
motor rightMotorA = motor(PORT9, ratio6_1, true);
motor rightMotorB = motor(PORT10, ratio6_1, true);
motor_group RightDriveSmart = motor_group(rightMotorA, rightMotorB);
drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 319.19, 295, 40, mm, 1);

motor WingsMotorA = motor(PORT15, ratio18_1, true);
motor WingsMotorB = motor(PORT5, ratio18_1, false);
motor_group Wings = motor_group(WingsMotorA, WingsMotorB);





// Forward declaration to resolve circular dependency





// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
  printf("VEXPlaySound:%s\n", soundName);
  wait(5, msec);
}

class CustomController {
private:
    
    bool recording = false;

    struct ControllerState {
      uint32_t timestamp = 0;
      int8_t ButtonA = false;
      int8_t ButtonB = false;
      int8_t ButtonX = false;
      int8_t ButtonY = false;
      int8_t ButtonUp = false;
      int8_t ButtonDown = false;
      int8_t ButtonLeft = false;
      int8_t ButtonRight = false;
      int8_t ButtonL1 = false;
      int8_t ButtonL2 = false;
      int8_t ButtonR1 = false;
      int8_t ButtonR2 = false;
      int16_t Axis1 = 0;
      int16_t Axis2 = 0;
      int16_t Axis3 = 0;
      int16_t Axis4 = 0;
    };
    struct ControllerState state;
    struct ControllerState playbackstate;

    struct RecordingData {
      uint32_t recordinglength = 0;
      struct ControllerState recording[100000];
    };
    struct RecordingData recordingtmp;
    timer recordingtimer; 
    uint32_t recordingidx = 0;
    int filenum = 1;
    bool playback = false;

    void saveRecording(const RecordingData& myStruct, const std::string& filename) {
        //std::remove(filename.c_str());
        std::ofstream file(filename, std::ios::binary);
        if (file.is_open()) {
            file.write(reinterpret_cast<const char*>(&myStruct), sizeof(RecordingData));
            file.close();
        }
    }

    void loadRecording(RecordingData& myStruct, const std::string& filename) {
        std::ifstream file(filename, std::ios::binary);
        if (file.is_open()) {
            file.read(reinterpret_cast<char*>(&myStruct), sizeof(RecordingData));
            file.close(); 
        }
    }


  public:
    // Constructor
    
    CustomController() {
    }

    bool startRecording() {
      if(!recording && !playback) {
        recording = true;
        recordingidx = 0;
        recordingtimer.clear();
        return true;
      }
      else {
        return false; // already recording
      }
    }

    bool stopRecording() {
      if(recording) {
        recording = false;
        recordingtmp.recordinglength = recordingidx + 1;
        char filename[1] = "";
        snprintf(filename, 2, "%d", filenum);
        std::string fn = filename;
        fn += ".txt";
        saveRecording(recordingtmp, fn);
        return true;
      }
      else {
        return false; // not recording
      }
    }

    bool startPlayback() {
      if(!playback && !recording) {
        char filename[1] = "";
        snprintf(filename, 2, "%d", filenum);
        std::string fn = filename;
        fn += ".txt";
        recordingidx = 0;
        loadRecording(recordingtmp, fn);
        recordingtimer.clear();
        playback = true;
        return true;
      }
      else {
        return false;
      }
    }
    inline int32_t record(int32_t value) {
      if(!recording) return value;
      state.timestamp = recordingtimer.time();
      recordingtmp.recording[recordingidx] = state;
      recordingidx++;
      wait(1,msec);
      return value;
    }

    bool __update_struct() {
      if(!playback) return false;

      if (recordingidx >= recordingtmp.recordinglength) {
        stopPlayback();
        return true;
      }
      else if ( recordingtmp.recording[recordingidx].timestamp <= recordingtimer.time() ) {
        recordingidx++;
        //playbackstate = recordingtmp.recording[recordingidx];
        
        //wait(1,msec);
        return true;
      } else {
        playbackstate = recordingtmp.recording[recordingidx];
        return false;
      }
    }


    void stopPlayback() {
        playback = false;
        vexController.Screen.setCursor(3,1);
        vexController.Screen.clearLine();
        vexController.Screen.clearScreen();
        vexController.Screen.print(filenum);
    }
    bool update_struct() {
      while (playback && __update_struct()) {
        wait(0, msec);
      }
      
      /*vexController.Screen.clearLine();
      vexController.Screen.clearScreen();
      vexController.Screen.setCursor(2,1);
      vexController.Screen.print(recordingidx);
      vexController.Screen.setCursor(3,1);
      vexController.Screen.print(playbackstate.Axis1);*/
      
      return true;
    }

    void recordingloop() {
      if(vexController.ButtonLeft.pressing() && !recording && !playback) {
        wait(250, msec);
        startRecording();
        vexController.Screen.setCursor(3,1);
        vexController.Screen.clearLine();
        vexController.Screen.clearScreen();
        vexController.Screen.print("Recording");
      }
      if(vexController.ButtonLeft.pressing() && recording && !playback) {
        stopRecording();
        vexController.Screen.setCursor(3,1);
        vexController.Screen.clearLine();
        vexController.Screen.clearScreen();
        vexController.Screen.print(filenum);
        wait(250, msec);
      }
      if(vexController.ButtonRight.pressing() && !recording && !playback) {
        wait(250, msec);
        startPlayback();
        vexController.Screen.setCursor(3,1);
        vexController.Screen.clearLine();
        vexController.Screen.clearScreen();
        vexController.Screen.print("Playing");
      }
      if (vexController.ButtonUp.pressing()) {
        filenum ++;
        vexController.Screen.setCursor(3, 1);
        vexController.Screen.clearLine();
        vexController.Screen.clearScreen();
        vexController.Screen.print(filenum);
        wait(250, msec);
      }
      if (vexController.ButtonDown.pressing()) {
        filenum --;
        vexController.Screen.setCursor(3, 1);
        vexController.Screen.clearLine();
        vexController.Screen.clearScreen();
        vexController.Screen.print(filenum);
        wait(250, msec);
      }
      
    }
    // Check if a button is pressed
    bool ButtonA() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonA;
      }
      return record(state.ButtonA = vexController.ButtonA.pressing());
    }

    bool ButtonB() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonB;
      }
      return record(state.ButtonB = vexController.ButtonB.pressing());
    }

    bool ButtonX() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonX;
      }
      return record(state.ButtonX = vexController.ButtonX.pressing());
    }

    bool ButtonY() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonY;
      }
      return record(state.ButtonY = vexController.ButtonY.pressing());
    }

    bool ButtonUp() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonUp;
      }
      return record(state.ButtonUp = vexController.ButtonUp.pressing());
    }

    bool ButtonDown() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonDown;
      }
      return record(state.ButtonDown = vexController.ButtonDown.pressing());
    }

    bool ButtonLeft() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonLeft;
      }
      return record(state.ButtonLeft = vexController.ButtonLeft.pressing());
    }

    bool ButtonRight() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonRight;
      }
      return record(state.ButtonRight = vexController.ButtonRight.pressing());
    }

    bool ButtonL1() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonL1;
      }
      return record(state.ButtonL1 = vexController.ButtonL1.pressing());
    }

    bool ButtonL2() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonL2;
      }
      return record(state.ButtonL2 = vexController.ButtonL2.pressing());
    }

    bool ButtonR1() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonR1;
      }
      return record(state.ButtonR1 = vexController.ButtonR1.pressing());
    }

    bool ButtonR2() {
      if(playback) {
        update_struct();
        return playbackstate.ButtonR2;
      }
      return record(state.ButtonR2 = vexController.ButtonR2.pressing());
    }

    // Getters for Axis values
    int Axis1() {
      if(playback) {
        update_struct();
        return playbackstate.Axis1;
      }
      return record(state.Axis1 = vexController.Axis1.position(percent));
    }

    int Axis2() {
      if(playback) {
        update_struct();
        return playbackstate.Axis2;
      }
      return record(state.Axis2 = vexController.Axis2.position(percent));
    }

    int Axis3() {
      if(playback) {
        update_struct();
        return playbackstate.Axis3;
      }
      return record(state.Axis3 = vexController.Axis3.position(percent));
    }

    int Axis4() {
      if(playback) {
        update_struct();
        return playbackstate.Axis4;
      }
      return record(state.Axis4 = vexController.Axis4.position(percent));
    }

    // Add more functionalities as needed
};

CustomController Controller1;

// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;
// define variables used for controlling motors based on controller inputs
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
bool DrivetrainRNeedsToBeStopped_Controller1 = true;

// define a task that will handle monitoring inputs from Controller1
int rc_auto_loop_function_Controller1() {
  // process the controller input every 20 milliseconds
  // update the motors based on the input values
  while(true) {
    

    if(RemoteControlCodeEnabled) {
      
    }
    wait(5, msec);
  }
  return 0;
}

task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);

#pragma endregion VEXcode Generated Robot Configuration
// Include the V5 Library

  
// Allows for easier use of the VEX Library
using namespace vex;

competition Competition;

float myVariable;

// "when started" hat block
int whenStarted1() {
  return 0;
}

void drive() {
  while (true) {
      //Controller1.recordingloop();
      // calculate the drivetrain motor velocities from the controller joystick axies
      // left = Axis3
      // right = Axis2
      int drivetrainLeftSideSpeed = Controller1.Axis3();
      int drivetrainRightSideSpeed = Controller1.Axis2();
      
      // check if the value is inside of the deadband range
      if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
        // check if the left motor has already been stopped
        if (DrivetrainLNeedsToBeStopped_Controller1) {
          // stop the left drive motor
          LeftDriveSmart.stop();
          // tell the code that the left motor has been stopped
          DrivetrainLNeedsToBeStopped_Controller1 = false;
        }
      } else {
        // reset the toggle so that the deadband code knows to stop the left motor nexttime the input is in the deadband range
        DrivetrainLNeedsToBeStopped_Controller1 = true;
      }
      // check if the value is inside of the deadband range
      if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
        // check if the right motor has already been stopped
        if (DrivetrainRNeedsToBeStopped_Controller1) {
          // stop the right drive motor
          RightDriveSmart.stop();
          // tell the code that the right motor has been stopped
          DrivetrainRNeedsToBeStopped_Controller1 = false;
        }
      } else {
        // reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
        DrivetrainRNeedsToBeStopped_Controller1 = true;
      }
      
      // only tell the left drive motor to spin if the values are not in the deadband range
      if (DrivetrainLNeedsToBeStopped_Controller1) {
        LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent);
        LeftDriveSmart.spin(forward);
      }
      // only tell the right drive motor to spin if the values are not in the deadband range
      if (DrivetrainRNeedsToBeStopped_Controller1) {
        RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
        RightDriveSmart.spin(forward);
      }
    
    if (Controller1.ButtonR1()) {
      Wings.setStopping(coast);
      Wings.setMaxTorque(100.0, percent);
      Wings.setVelocity(100.0, percent);
      Wings.spin(forward);
      //waitUntil((!Controller1.ButtonR1()));
    }
    else if (Controller1.ButtonR2()) {
      Wings.setStopping(coast);
      Wings.setMaxTorque(100.0, percent);
      Wings.setVelocity(100.0, percent);
      Wings.spin(reverse);
      //waitUntil((!Controller1.ButtonR2()));
    }
    else if(Controller1.ButtonX()) {
      Wings.stop(); 
    }
    wait(5,msec);
    // wait before repeating the process
    
  }
}
// "when autonomous" hat block
int onauton_autonomous_0() {
  Controller1.startPlayback();
  drive();
  
  
  return 0;
}

// "when driver control" hat block
int ondriver_drivercontrol_0() {
  Controller1.stopPlayback();
  drive();

  return 0;
}

void VEXcode_driver_task() {
  // Start the driver control tasks....
  vex::task drive0(ondriver_drivercontrol_0);
  while(Competition.isDriverControl() && Competition.isEnabled()) {this_thread::sleep_for(10);}
  drive0.stop();
  return;
}

void VEXcode_auton_task() {
  // Start the auton control tasks....
  vex::task auto0(onauton_autonomous_0);
  while(Competition.isAutonomous() && Competition.isEnabled()) {this_thread::sleep_for(10);}
  auto0.stop();
  return;
}



int main() {
  vex::competition::bStopTasksBetweenModes = false;
  Competition.autonomous(VEXcode_auton_task);
  Competition.drivercontrol(VEXcode_driver_task);

  // register event handlers

  wait(15, msec);
  // post event registration

  // set default print color to black
  printf("\033[30m");

  // wait for rotation sensor to fully initialize
  wait(30, msec);

  whenStarted1();
}