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

typedef struct {
  int speed;     // -255 to 255
  int direction; // -100 to 100
} CommandPacket;

typedef struct {
  int currentSpeed;
  int batteryVoltage;
  int temperature;
} TelemetryPacket;

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

void onCommandRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  memcpy(&command, incomingData, sizeof(command));
  // Control motors using command.speed and command.direction
  // Example: use PWM with analogWrite to motor driver pins
  Serial.print("Received Speed: "); Serial.println(command.speed);
  Serial.print("Received Direction: "); Serial.println(command.direction);

  // Add full motor control logic here for variable speed/direction
}

void setup() {
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW init error");
    return;
  }
  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() {
  // Telemetry every 500 ms
  if (millis() - lastTelemetrySent > 500) {
    TelemetryPacket telemetry;
    telemetry.currentSpeed = command.speed;           // You could read actual motor speed via encoder
    telemetry.batteryVoltage = analogRead(36) * 3.3;  // Example: Replace with real voltage calculation
    telemetry.temperature = 25;                       // Example: Replace with real temperature sensor value

    esp_now_send(remoteAddr, (uint8_t*)&telemetry, sizeof(telemetry));
    lastTelemetrySent = millis();
  }
}