// if, else-if are being used such that at a time only one component controls the motor.

#include <esp_now.h>
#include <WiFi.h>

// Motor driver pins
#define F_L PWM_1  23     // For Robot's Forward (Local Y) 
#define F_L PWM_2  19     // Motor 1A and 1B are responsible for Left/ Right movements
#define F_R PWM_1  22
#define F_R PWM_2  18

#define B_L PWM_1 16     // For Robot's Right (Local X)
#define B_L PWM_2  10     // Motor 2A and 2B are responsible for forward/ backward movements
#define B_R PWM_1 17
#define B_R PWM_2  4

// Structures for data
// Joysticks
typedef struct {
  int xValue;
  int yValue;
  int switchvalue;
} JoystickData;

// Push Buttons
typedef struct {
  int button1State;
  int button2State;
} ButtonData;

// Potentiometer
typedef struct {
  int potValue;
} PotData;

// On-On Toggle Switches
typedef struct {
  int leftState;
  int rightState;
} ToggleData;

// Combined data
typedef struct {
  JoystickData joystick1;
  JoystickData joystick2;
  ButtonData buttons;
  PotData pot;
  ToggleData toggle1;
  ToggleData toggle2;
  ToggleData toggle3;
} ControllerData;

ControllerData receivedData;

void setup() {
  // Initialize Serial Monitor
  Serial.begin(115200);

  // Initialize WiFi
  WiFi.mode(WIFI_STA);

  // Initialize ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW initialization failed!");
    return;
  }

  // Register callback function to receive data
  esp_now_register_recv_cb(onDataReceive);

  // Initialize motor driver pins
  pinMode(F_L PWM_1, OUTPUT);
  pinMode(F_L PWM_2, OUTPUT);
  pinMode(F_R PWM_1, OUTPUT);
  pinMode(F_R PWM_2, OUTPUT);
  
  pinMode(B_L PWM_1, OUTPUT);
  pinMode(B_L PWM_2, OUTPUT);
  pinMode(B_R PWM_1, OUTPUT);
  pinMode(B_R PWM_2, OUTPUT);
}

void loop() {
  // Main loop can be empty as data is processed in the callback
}

// Callback function to handle received data

void Drive_ctrl_jystck_1 (int pwm_x, int pwm_y, int xvalue_new_1, int yvalue_new_1 ) 
{
  if (xvalue_new_1 > 0) {                        // X Value from joystick is for Left and Right movements
      if (yvalue_new_1 > 0){                      //  Y Value from joystick is for reverse and straight movements
        digitalWrite(motor1A, HIGH);  
        digitalWrite(motor1B, LOW);   
        analogWrite(pwm1A, pwm_x);     
        analogWrite(pwm1B, pwm_x);     

        digitalWrite(motor2A, HIGH);    
        digitalWrite(motor2B, LOW);   
        analogWrite(pwm2A, pwm_y);    
        analogWrite(pwm2B, pwm_y);    
      }
      
      else if (yvalue_new_1 < 0){
        digitalWrite(motor1A, HIGH);  
        digitalWrite(motor1B, LOW);   
        analogWrite(pwm1A, pwm_x);    
        analogWrite(pwm1B, pwm_x);    
    
        digitalWrite(motor2A, LOW);  
        digitalWrite(motor2B, HIGH); 
        analogWrite(pwm2A, pwm_y);   
        analogWrite(pwm2B, pwm_y);   
      }
      
      else {
        digitalWrite(motor1A, HIGH);  
        digitalWrite(motor1B, LOW);   
        analogWrite(pwm1A, pwm_x);    
        analogWrite(pwm1B, pwm_x);    
  
        analogWrite(pwm2A, 0);     
        analogWrite(pwm2B, 0);     
      }
    } 
    
    
    else if (xvalue_new_1 < 0) {
        
      if (yvalue_new_1 > 0)  {
          digitalWrite(motor1A, LOW);   
          digitalWrite(motor1B, HIGH);  
          analogWrite(pwm1A, pwm_x);    
          analogWrite(pwm1B, pwm_x);    
  
          digitalWrite(motor2A, HIGH);  
          digitalWrite(motor2B, LOW);   
          analogWrite(pwm2A, pwm_y);    
          analogWrite(pwm2B, pwm_y);    
        } 
      
      else if(yvalue_new_1 < 0){
            digitalWrite(motor1A, LOW); 
            digitalWrite(motor1B, HIGH);
            analogWrite(pwm1A, pwm_x);  
            analogWrite(pwm1B, pwm_x);  
    
            digitalWrite(motor2A, LOW); 
            digitalWrite(motor2B, HIGH);
            analogWrite(pwm2A, pwm_y);  
            analogWrite(pwm2B, pwm_y);  
        }
      
      else {
            digitalWrite(motor1A, LOW); 
            digitalWrite(motor1B, HIGH);
            analogWrite(pwm1A, pwm_x);  
            analogWrite(pwm1B, pwm_x);     
    
            analogWrite(pwm2A, 0);     
            analogWrite(pwm2B, 0);     
        }
    } 
    
    
    else {
      
      if (yvalue_new_1 > 0){
          analogWrite(pwm1A, 0);     
          analogWrite(pwm1B, 0);     
        
          digitalWrite(motor2A, HIGH);  
          digitalWrite(motor2B, LOW);   
          analogWrite(pwm2A, pwm_y);    
          analogWrite(pwm2B, pwm_y);    
      }
      
      else if (yvalue_new_1 < 0){
          analogWrite(pwm1A, 0);     
          analogWrite(pwm1B, 0);     
        
          digitalWrite(motor2A, LOW);
          digitalWrite(motor2B, HIGH);   
          analogWrite(pwm2A, pwm_y);     
          analogWrite(pwm2B, pwm_y);     
      }
      
      else {
          analogWrite(pwm1A, 0);     
          analogWrite(pwm1B, 0);     
        
          analogWrite(pwm2A, 0);     
          analogWrite(pwm2B, 0);     
      }
    }
  
  // Joystick output
    Serial.print("X: "); Serial.print(xvalue_new_1);
    Serial.print(" Y: "); Serial.println(yvalue_new_1);
  
  }

void Yaw_Pitch_motion_ctrl (int joystick_2_Switch_state, int pwm_x, int pwm_y, int xvalue_new_2) {

    bool mode = 1; //  Mode 1 (default)
    bool prevButtonState = LOW; 
    
    if (joystick_2_Switch_state == HIGH && prevButtonState == LOW) { 
      mode = !mode; 
      delay(200);   
    }
    prevButtonState = joystick_2_Switch_state; 
  
    if (mode == 1) {
      
      if (xvalue_new_2 < 0){
          digitalWrite(motor1A, LOW);   
          digitalWrite(motor1B, LOW);  
          analogWrite(pwm1A, pwm_x);    
          analogWrite(pwm1B, pwm_x);    
  
          digitalWrite(motor2A, HIGH);  
          digitalWrite(motor2B, HIGH);   
          analogWrite(pwm2A, pwm_x);    
          analogWrite(pwm2B, pwm_x);
      }
      
      else if (xvalue_new_2 > 0){
          digitalWrite(motor1A, HIGH);   
          digitalWrite(motor1B, HIGH);  
          analogWrite(pwm1A, pwm_x);    
          analogWrite(pwm1B, pwm_x);    
  
          digitalWrite(motor2A, LOW);  
          digitalWrite(motor2B, LOW);   
          analogWrite(pwm2A, pwm_x);    
          analogWrite(pwm2B, pwm_x);
      }
      
      else {
          analogWrite(pwm1A, 0);    
          analogWrite(pwm1B, 0); 
          
          analogWrite(pwm2A, 0);    
          analogWrite(pwm2B, 0);
      }
    } 
    
    else {
      // Code for Top-Spin or Back-Spin.
    }
  }
  

void onDataReceive(const esp_now_recv_info *info, const uint8_t *incomingData, int len) {
  memcpy(&receivedData, incomingData, sizeof(receivedData));

  // FOR JOYSTICK-1
  int xvalue_1 = receivedData.joystick1.xValue;
  int xvalue_new_1;

  if (xvalue_1 <= 2047) {
    xvalue_new_1 = map(xvalue_1, 0, 2047, -255, 0);
  } else {
    xvalue_new_1 = map(xvalue_1, 2047, 4095, 0, 255);
  }

  int yvalue_1 = receivedData.joystick1.yValue;
  int yvalue_new_1;

  if (yvalue_1 <= 2047) {
    yvalue_new_1 = map(yvalue_1, 0, 2047, -255, 0);
  } else {
    yvalue_new_1 = map(yvalue_1, 2047, 4095, 0, 255);
  }

  int pwm_x = abs(xvalue_new_1); 
  int pwm_y = abs(yvalue_new_1); 

  // FOR JOYSTICK-2
  int joystick_2_Switch_state = receivedData.joystick2.switchvalue ;
  
  // Mapping X values to PWM range
  
  int xvalue_2= receivedData.joystick2.xValue ;
  int xvalue_new_2 ;
  
  if (xvalue_2 <= 2047) {
    xvalue_new_2 = map(xvalue_2, 0, 2047, -255, 0);
  } else {
    xvalue_new_2 = map(xvalue_2, 2047, 4095, 0, 255);
  }

  // Mapping Y values to PWM range

  int yvalue_2= receivedData.joystick2.yValue ;
  int yvalue_new_2 ;

  if (yvalue_2 <= 2047) {
    yvalue_new_2 = map(yvalue_2, 0, 2047, -255, 0);
  } else {
    yvalue_new_2 = map(yvalue_2, 2047, 4095, 0, 255);
  }


  // Control For Joystick- 1
  if (xvalue_new_1 !=0 || yvalue_new_1 !=0 ) {

    Drive_ctrl_jystck_1 (pwm_x, pwm_y, xvalue_new_1, yvalue_new_1 ) ;
  }
  
  // Control For Joystick- 2
  else if (xvalue_new_2 != 0 || yvalue_new_2 != 0) {  
    
    Yaw_Pitch_motion_ctrl (joystick_2_Switch_state, pwm_x, pwm_y, xvalue_new_2) ;
    
  }
}