/*

   ___  _             _                       _       ___             _                        
  / (_)| |           | |                     | |     / (_)           | | o     o               
 |     | |  __   __  | |           __   ,_   | |    |      __   __,  | |   _|_     __   _  _   
 |     |/  /  \_/    |/_) |  |  |_/  \_/  |  |/_)   |     /  \_/  |  |/  |  |  |  /  \_/ |/ |  
  \___/|__/\__/ \___/| \_/ \/ \/  \__/    |_/| \_/   \___/\__/ \_/|_/|__/|_/|_/|_/\__/   |  |_/
                                                                                               
                                                                                               
Current up to date code for the Clockwork Coalition Robot -- Last Updated April 23 2024
R1 : Catapult Toggle
R3 : Drive Both Motors With Left Stick
Right DPad : Toggle Ball Grip
L1 : Mini Golf Putter
Triangle : Air Compressors Toggle
Square + Up/Down : Vertical Lift Tower
Cross + Up/Down : Extra Stepper - Mos Likely For Spinning Nut
Circle + Up/Down : Putter Flow Control Stepper
Options : HuskyLens Auto Color Tracking


*/

#include 
#include 
#include "HUSKYLENS.h"
#include "Wire.h"

// Required for automated line following. HuskyLens is required to correctly pull data from the camera
HUSKYLENS huskylens;
HUSKYLENSResult result;
int linePosition = 0;
int leftwheelauto = 2000;
int rightwheelauto = 1000;

// Set Servo Names
Servo leftwheel;
Servo rightwheel;

// Relay Pins
int relay1 = 19;
int relay2 = 18;
int relay3 = 5;
int relay4 = 17;
int relay5 = 16;
int relay6 = 4;
int relay7 = 0;
int relay8 = 2;

// Stepper Pins
int Direction = 27;
int Move = 15;
int Enable1 = 25;
int Enable2 = 33;
int Enable3 = 32;

// Stick Positions
int leftstick;
int rightstick;

// Booleans - Used To Toggle Outputs On A Single Button Click
bool launcher;    //Toggles basketball launcher cylinders
bool compressor;  //Toggles compressor solenoids
bool ballgrip;    //Toggles basketball gripper
bool gripper;     //Toggles pick and place gripper

// Time Stuff - Used To Debounce Toggle Buttons
unsigned long lastmilli = 0;
unsigned long currentmilli = 0;

void setup() {
  Serial.begin(115200);
  // Connect to PS4 Controller
  PS4.begin("A0:B7:65:4D:B5:88");
  // Attach output pins for Servos
  leftwheel.attach(12);
  rightwheel.attach(14);
  // Verify Connection To HuskyLens
  Wire.begin();
  Serial.println("w");
  while (!huskylens.begin(Wire)) {
    Serial.println(F("Begin failed!"));
    delay(100);
  }
  // Write Color Recognition Algorithim To HuskyLens
  huskylens.writeAlgorithm(ALGORITHM_COLOR_RECOGNITION);

  // Set Outputs As Outputs
  pinMode(relay1, OUTPUT);
  pinMode(relay2, OUTPUT);
  pinMode(relay3, OUTPUT);
  pinMode(relay4, OUTPUT);
  pinMode(relay5, OUTPUT);
  pinMode(relay6, OUTPUT);
  pinMode(relay7, OUTPUT);
  pinMode(relay8, OUTPUT);
  pinMode(Direction, OUTPUT);
  pinMode(Move, OUTPUT);
  pinMode(Enable1, OUTPUT);
  pinMode(Enable2, OUTPUT);
  pinMode(Enable3, OUTPUT);

  // Set All Relays To High (Off)
  digitalWrite(relay1, HIGH);
  digitalWrite(relay2, HIGH);
  digitalWrite(relay3, HIGH);
  digitalWrite(relay4, HIGH);
  digitalWrite(relay5, HIGH);
  digitalWrite(relay6, HIGH);
  digitalWrite(relay7, HIGH);
  digitalWrite(relay8, HIGH);
}

void loop() {
  // Check if PS4 Controller is connected. Stop All Movement If Not Connected
  while (PS4.isConnected()) {
    Serial.println(PS4.R2Value());
    currentmilli = millis();
    // Read Joystick Values, Map, And Write To Motor Controllers.
    leftstick = PS4.LStickY();
    rightstick = PS4.RStickY();
    leftstick = map(leftstick, -127, 127, 1200, 1800);
    rightstick = map(rightstick, -127, 127, 1800, 1200);
    // Set motors to idle if joysticks are in the middle floating area
    if (rightstick < 1575 && rightstick > 1425) {
      rightstick = 1500;
    }
    if (leftstick < 1575 && leftstick > 1425) {
      leftstick = 1500;
    }
    // Debugging Printout To Show Output Being Written To Motor Controllers
    //Serial.print(leftstick);
    //Serial.print("_____");
    //Serial.println(rightstick);
    // Output PWM Based On Stick Position
    if (PS4.R3() == 1) {
      leftwheel.writeMicroseconds(leftstick);
      rightstick = map(leftstick, 1200, 1800, 1800, 1200);
      rightwheel.writeMicroseconds(rightstick);
    }
    else {
    leftwheel.writeMicroseconds(leftstick);
    rightwheel.writeMicroseconds(rightstick);
    }

    // HuskyLens Line Follow Auto Mode
    while (PS4.Options() == 1) {
      // Grab Data From HuskyLens For Only Color ID 1
      huskylens.request(1);
      result = huskylens.read();
      linePosition = result.xCenter;
      // Print Where Color Block Center Is On Screen
      //Serial.println(linePosition);
      // Map values based off of R2
      leftwheelauto = map(PS4.R2Value(), 0, 400, 1500, 1000);
      rightwheelauto = map(PS4.R2Value(), 0, 400, 1500, 2000);
      Serial.print(leftwheelauto);
      Serial.print("____");
      Serial.println(rightwheelauto);
      // If Color Block Is On Left Third Of Screen, Turn Left
      if (linePosition <= 131 && linePosition > 0) {  // Was 106 and 0
        Serial.println("Left");
        leftwheel.writeMicroseconds(leftwheelauto); // Was 1500
        rightwheel.writeMicroseconds(1500); //Was 1350
      }
      // If Color Block Is In Center Of Screen, Go Straight
      else if (linePosition <= 187 && linePosition > 131) {  //Was 187 and 106
        Serial.println("Straight");
        leftwheel.writeMicroseconds(leftwheelauto);    // Was 1650
        rightwheel.writeMicroseconds(rightwheelauto);   // Was 1350
      }
      // If Color Block Is On Right Third Of Screen, Turn Right
      else if (linePosition <= 320 && linePosition > 187) {   //Was 320 and 212
        Serial.println("Right");
        leftwheel.writeMicroseconds(1500);    // Was 1650
        rightwheel.writeMicroseconds(rightwheelauto); // Was 1500
      }
      // If Color Is Not Found On Screen, Do Nothing
      else {
        Serial.println("Spin");
        leftwheel.writeMicroseconds(1500);
        rightwheel.writeMicroseconds(1500);
      }
      delay(100); // This delay is required to keep the HuskyLens from slowing down the line following.
    }


    // Putter Flow Control Stepper Movement
    if (PS4.Circle() && PS4.Up()) {
      digitalWrite(Enable1, LOW);
      digitalWrite(Direction, LOW);
      analogWrite(Move, 120);
      Serial.println("Step 1 DirHi");
    } else if (PS4.Circle() && PS4.Down()) {
      digitalWrite(Enable1, LOW);
      digitalWrite(Direction, HIGH);
      analogWrite(Move, 120);
      Serial.println("Step 1 DirLo");
    } else {
      digitalWrite(Enable1, HIGH);
    }

    // Nut Rotation Stepper
    if (PS4.Cross() && PS4.Up()) {
      digitalWrite(Enable2, LOW);
      digitalWrite(Direction, HIGH);
      analogWrite(Move, 130);
      Serial.println("Step2 DirHI");
    } else if (PS4.Cross() && PS4.Down()) {
      digitalWrite(Enable2, LOW);
      digitalWrite(Direction, LOW);
      analogWrite(Move, 130);
      Serial.println("Step2 DirLo");
    } else {
      digitalWrite(Enable2, HIGH);
    }

    // Vertical Lift Tower Stepper
    if (PS4.Square() && PS4.Up()) {
      digitalWrite(Enable3, LOW);
      digitalWrite(Direction, HIGH);
      analogWrite(Move, 130);
      Serial.println("Step3 DirHi");
    } else if (PS4.Square() && PS4.Down()) {
      digitalWrite(Enable3, LOW);
      digitalWrite(Direction, LOW);
      analogWrite(Move, 130);
      Serial.println("Step3 DirLo");
    } else {
      digitalWrite(Enable3, HIGH);
    }
    // If No Steppers Are In Use, Cease Writing PWM
    if (!PS4.Circle() && !PS4.Cross() && !PS4.Square()) {
      analogWrite(Move, 0);
    }

    // Compressor Toggle
    if (PS4.Triangle() && currentmilli - lastmilli > 400) {
      currentmilli = millis();
      lastmilli = currentmilli;
      if (compressor == 1) {
        digitalWrite(relay6, HIGH);
        compressor = 0;
      }
      else {
        digitalWrite(relay6, LOW);
        compressor = 1;
      }
    }

    // Putter
    if (PS4.L1()) {
      digitalWrite(relay1, LOW);
    } else {
      digitalWrite(relay1, HIGH);
    }

    // Basketball Launcher Toggle
    if (PS4.R1() && currentmilli - lastmilli > 400) {
      currentmilli = millis();
      lastmilli = currentmilli;
      if (launcher == 1) {
        digitalWrite(relay4, LOW);
        digitalWrite(relay7, LOW);
        launcher = 0;
      }
      else {
      digitalWrite(relay4, HIGH);
      digitalWrite(relay7, HIGH);
      launcher = 1;
      }
    }

    // Basketball Gripper Toggle
    if (PS4.Right() == 1 && currentmilli - lastmilli > 400) {
      currentmilli = millis();
      lastmilli = currentmilli;
      if (ballgrip == 1) {
        digitalWrite(relay5, LOW);
        ballgrip = 0;
      }
      else {
        digitalWrite(relay5, HIGH);
        ballgrip = 1;
      }
    }

    // Pick And Place Gripper Toggle
    if (PS4.Left() && currentmilli - lastmilli > 400) {
      currentmilli = millis();
      lastmilli = currentmilli;
      if (gripper == 1) {
        digitalWrite(relay3, LOW);
        gripper = 0;
      }
      else {
        digitalWrite(relay3, HIGH);
        gripper = 1;
      }
    }
  }


  // If PS4 Controller Is Not Connected Set Motors To Neutral.
  // This helps avoid Mustang Syndrome
  while (!PS4.isConnected()) {
    leftstick = 1500;
    rightstick = 1500;
    leftwheel.writeMicroseconds(leftstick);
    rightwheel.writeMicroseconds(rightstick);
    digitalWrite(Enable1, HIGH);
    digitalWrite(Enable2, HIGH);
    digitalWrite(Enable3, HIGH);
    analogWrite(Move, 0);
    Serial.println("MY LEEGGGGGG!!!!!");
  }
}