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

Servo leftServo1, leftServo2, rightServo1, rightServo2;
int leftServoPin1 = 12, leftServoPin2 = 13, rightServoPin1 = 14, rightServoPin2 = 15;

typedef struct {
  int speed;
  int turn;
} CommandPacket;

typedef struct {
  int leftServoSpeed;
  int rightServoSpeed;
  int batteryLevel;
} TelemetryPacket;

CommandPacket command;
uint8_t controllerAddr[] = {0x24, 0x6F, 0x28, 0x33, 0xDE, 0xB4}; // Remote MAC

void onCommandRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  memcpy(&command, incomingData, sizeof(command));
  int leftSpeed  = constrain(command.speed + command.turn, -255, 255);
  int rightSpeed = constrain(command.speed - command.turn, -255, 255);

  // Map speed value to servo position (0-180 for continuous rotation speed control)
  int leftPWM  = map(leftSpeed, -255, 255, 0, 180);
  int rightPWM = map(rightSpeed, -255, 255, 0, 180);

  leftServo1.write(leftPWM);
  leftServo2.write(leftPWM);
  rightServo1.write(rightPWM);
  rightServo2.write(rightPWM);

  // Store for telemetry
  telemetry.leftServoSpeed = leftSpeed;
  telemetry.rightServoSpeed = rightSpeed;
}

TelemetryPacket telemetry;

void setup() {
  Serial.begin(115200);
  leftServo1.attach(leftServoPin1);
  leftServo2.attach(leftServoPin2);
  rightServo1.attach(rightServoPin1);
  rightServo2.attach(rightServoPin2);
  WiFi.mode(WIFI_STA);
  esp_now_init();
  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, controllerAddr, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  esp_now_add_peer(&peerInfo);
  esp_now_register_recv_cb(onCommandRecv);
}

unsigned long lastTelemetrySent = 0;
void loop() {
  // Telemetry every 500 ms
  if (millis() - lastTelemetrySent > 500) {
    telemetry.batteryLevel = analogRead(36); // Replace with battery calculation if needed
    esp_now_send(controllerAddr, (uint8_t*)&telemetry, sizeof(telemetry));
    lastTelemetrySent = millis();
  }
}