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

Servo leftServoA, leftServoB, rightServoA, rightServoB;
int leftPins[2] = {12, 13};
int rightPins[2] = {14, 15};

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

typedef struct {
  int leftSpeed;
  int rightSpeed;
  int battery;
} TelemetryPacket;

CommandPacket command;
TelemetryPacket telemetry;

uint8_t remoteAddr[] = {0x24, 0x6F, 0x28, 0x33, 0xDE, 0xB4}; // MAC of remote

void onCommandRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  memcpy(&command, incomingData, sizeof(command));

  int left = constrain(command.speed + command.turn, -255, 255);
  int right = constrain(command.speed - command.turn, -255, 255);

  int pwmLeft = map(left, -255, 255, 0, 180);
  int pwmRight = map(right, -255, 255, 0, 180);

  leftServoA.write(pwmLeft);
  leftServoB.write(pwmLeft);
  rightServoA.write(pwmRight);
  rightServoB.write(pwmRight);

  telemetry.leftSpeed = left;
  telemetry.rightSpeed = right;
}

void setup() {
  Serial.begin(115200);
  leftServoA.attach(leftPins[0]);
  leftServoB.attach(leftPins[1]);
  rightServoA.attach(rightPins[0]);
  rightServoB.attach(rightPins[1]);
  WiFi.mode(WIFI_STA);
  esp_now_init();
  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, remoteAddr, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  esp_now_add_peer(&peerInfo);
  esp_now_register_recv_cb(onCommandRecv);
}

unsigned long lastTelemetrySent = 0;
void loop() {
  if (millis() - lastTelemetrySent > 500) {
    telemetry.battery = analogRead(36); // Replace with proper battery voltage calculation
    esp_now_send(remoteAddr, (uint8_t*)&telemetry, sizeof(telemetry));
    lastTelemetrySent = millis();
  }
}