// ENGR Final Project
// Colin and William
//6/5/2024

#include <Ps3Controller.h>

// Motor control pins
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

// Slowest speed for the motors
#define SLOWEST_SPEED 100


// Motor control function
void motor(uint8_t left, uint8_t right) {
  if ((left < 138)&&(left > 118)) { // Set left to stop
    ledcWrite(3, 0); // Set M2B to LOW
    ledcWrite(2, 0); // Set M2A to LOW
  } else if (left > 138) { // Set left to forward
    left = map(left, 138, 255, SLOWEST_SPEED, 255);
    ledcWrite(3, 0); // Set M2B to LOW
    ledcWrite(2, left); // Set M2A to PWM
  } else { // Set left to reverse
    left = map(left, 138, 0, SLOWEST_SPEED, 255);
    ledcWrite(3, left); // Set M2B to PWM
    ledcWrite(2, 0); // Set M2A to LOW
  }

  if ((right < 138)&&(right > 118)) { // Set right to stop
    ledcWrite(1, 0); // Set M2B to LOW
    ledcWrite(0, 0); // Set M2A to LOW
  } else if (right > 138) { // Set right to forward
    right = map(right, 138, 255, SLOWEST_SPEED, 255);
    ledcWrite(1, 0); // Set M2B to LOW
    ledcWrite(0, right); // Set M2A to PWM
  } else { // Set right to reverse
    right = map(right, 138, 0, SLOWEST_SPEED, 255);
    ledcWrite(1, right); // Set M2B to PWM
    ledcWrite(0, 0); // Set M2A to LOW
  }
}

void setup() {

  Serial.begin(9600);
  Ps3.attach(notify);
  Ps3.begin("00:00:00:00:00:00");

  // Set the 4 motor control lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  // Connect 4 PWM channels to the 4 motor control Pins
  ledcSetup(0, 30000, 8);
  ledcAttachPin(M1A, 0);
  ledcSetup(1, 30000, 8);
  ledcAttachPin(M1B, 1);
  ledcSetup(2, 30000, 8);
  ledcAttachPin(M2A, 2);
  ledcSetup(3, 30000, 8);
  ledcAttachPin(M2B, 3);
}

void notify() {
  //testing x and y values
  if( abs(Ps3.event.analog_changed.stick.lx) + abs(Ps3.event.analog_changed.stick.ly) > 2 ){
       Serial.print("Moved the left stick:");
       Serial.print(" x="); Serial.print(Ps3.data.analog.stick.lx, DEC);
       Serial.print(" y="); Serial.print(Ps3.data.analog.stick.ly, DEC);
       Serial.println();
    }
  if( Ps3.event.button_up.cross ){
    Serial.print("Up was pressed");
    for(int i = 1; i < 2; i++){
      delay(2000);
    }
  }
    int leftStickY = Ps3.data.analog.stick.ly;
    int leftStickX = Ps3.data.analog.stick.lx; // X-axis of left joystick
    int turnAdjustment;
    int leftSpeed;
    int rightSpeed;
    // Convert joystick values to motor speed and direction
   

    //straight
    if ((leftStickX < 110) && (leftStickX > -110)){
    Serial.println("straight");
    leftSpeed = map(leftStickY, 0, 255, 0, 255);
    rightSpeed = map(leftStickY, 0, 255, 0, 255);
    turnAdjustment = map(leftStickX, 0, 255, -127, 127);
    leftSpeed = constrain(leftSpeed - turnAdjustment, 0, 255);
    rightSpeed = constrain(rightSpeed - turnAdjustment, 0, 255);
    motor(leftSpeed, rightSpeed);
    }
    //turn right
    if(leftStickX > 110){ 
    Serial.println("right");
    leftSpeed = map(leftStickY, 0, 255, 0, 255);
    rightSpeed = map(leftStickY, 0, 255, 0, 255);
    motor(leftSpeed, (rightSpeed -50));

    }
    //turn left
    if(leftStickX < -110){ 
    Serial.println("Left");
    leftSpeed = map(leftStickY, 0, 255, 0, 255);
    rightSpeed = map(leftStickY, 0, 255, 0, 255);
    motor((leftSpeed -50), rightSpeed);

   
    }
    
  
}

void loop(){

}