#include  <Arduino.h>
#include  "L298N_ESP32.h"
#include  "Servo_ESP32.h"

// Motor 1 Pins
#define   MOTOR1_PIN1     27
#define   MOTOR1_PIN2     26
#define   MOTOR1_ENABLE   14

// Motor 2 Pins
#define   MOTOR2_PIN1     25
#define   MOTOR2_PIN2     33
#define   MOTOR2_ENABLE   32

// Servo Pins
#define   SERVO1_PIN      13
#define   SERVO2_PIN      12

// Motor Objects
L298N_ESP32   motor1(MOTOR1_PIN1, MOTOR1_PIN2, MOTOR1_ENABLE, 0);
L298N_ESP32   motor2(MOTOR2_PIN1, MOTOR2_PIN2, MOTOR2_ENABLE, 1);

// Servo Objects
Servo_ESP32   servo1(SERVO1_PIN, 2, 1000, 2000);
Servo_ESP32   servo2(SERVO2_PIN, 3, 1000, 2000);

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

// Packet Data Structure
struct PacketData {
    uint8_t X;  // Steering (left/right)
    uint8_t Y;  // Throttle (forward/backward)
    bool    B;  // Button
};

// Data Received From Remote
PacketData data;

// Constants
const unsigned long MOTOR_ACTION_DELAY = 5000; // Delay between motor actions
const int SERVO1_START = 10;
const int SERVO1_END = 170;
const int SERVO2_START = 20;
const int SERVO2_END = 160;

enum MotorState {
    STOPPED,
    FORWARD_M1,
    BACKWARD_M1,
    FORWARD_M2,
    BACKWARD_M2
};

MotorState currentMotorState = STOPPED;
unsigned long motorStateStartTime = 0;

void setup() {

  Serial.begin(115200);

  // Initialize Motors
  motor1.begin();
  motor2.begin();

  // Initialize Servos
  servo1.begin();
  servo2.begin();

  Serial.println("Initialization complete.");

  // ESP-NOW setup
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW init failed");
    return;
  }

  // When Receive Data Execute OnDataRecv Function
  esp_now_register_recv_cb(OnDataRecv);

}

void loop() {

  // Non Blocking Motor control
  if(millis() - motorStateStartTime >= MOTOR_ACTION_DELAY) {

    motorStateStartTime = millis();

    switch (currentMotorState) {

      case STOPPED:
        Serial.println("Moving motor 1 forward...");
        motor1.forward();
        motor1.setPercentage(75); //PWM set percentage
        motor2.stop();
        currentMotorState = FORWARD_M1;
        break;
      case FORWARD_M1:
        Serial.println("Moving motor 1 backward...");
        motor1.backward();
        motor1.setPercentage(50); //PWM set percentage
        motor2.stop();
        currentMotorState = BACKWARD_M1;
        break;
      case BACKWARD_M1:
        Serial.println("Moving motor 2 forward...");
        motor1.stop();
        motor2.forward();
        motor2.setPercentage(25); //PWM set percentage
        currentMotorState = FORWARD_M2;
        break;
      case FORWARD_M2:
        Serial.println("Moving motor 2 backward...");
        motor1.stop();
        motor2.backward();
        motor2.setPercentage(100); //PWM set percentage
        currentMotorState = BACKWARD_M2;
        break;
      case BACKWARD_M2:
        Serial.println("Stopping motors ...");
        motor1.stop();
        motor2.stop();
        currentMotorState = STOPPED;
        break;

    }

  }

  //Non Blocking Servos
  static unsigned long servoTimer = 0;
  static int servo1Pos = SERVO1_START;
  static int servo2Pos = SERVO2_START;
  static bool servoForward = true;
  unsigned long currentTime = millis();

  if(currentTime - servoTimer >= 20) { // Adjust for speed

    servoTimer = currentTime;

    if (servoForward) {
      servo1Pos++;
      if (servo1Pos >= SERVO1_END) {
        servoForward = false;
      }
    } else {
      servo1Pos--;
      if (servo1Pos <= SERVO1_START) {
        servoForward = true;
      }
    }

    servo1.write(servo1Pos);

  }

  //Non Blocking Servos
  static unsigned long servoTimer2 = 0;
  static int servo2Pos2 = SERVO2_START;
  static bool servoForward2 = true;
  unsigned long currentTime2 = millis();

  if(currentTime2 - servoTimer2 >= 20) { // Adjust for speed

    servoTimer2 = currentTime2;

    if (servoForward2) {
      servo2Pos2++;
      if (servo2Pos2 >= SERVO2_END) {
        servoForward2 = false;
      }
    } else {
      servo2Pos2--;
      if (servo2Pos2 <= SERVO2_START) {
        servoForward2 = true;
      }
    }

    servo2.write(servo2Pos2);

  }

}

// ESP-NOW callback
void OnDataRecv(const esp_now_recv_info *info, const uint8_t *data, int len) {
  if (len == sizeof(PacketData)) {
    memcpy(&data, data, sizeof(PacketData));
  }
}