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

//------------------------------------------------
// This is The Car Unit
//------------------------------------------------
// RC Car Remote MAC Address: 64:E8:33:B5:D8:40
// RC Car MAC Address: 50:78:7D:43:14:00
//------------------------------------------------

Servo leftServoA, leftServoB, rightServoA, rightServoB;
int leftPins[2] = {0, 1};
int rightPins[2] = {2, 3};

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

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

CommandPacket command;
TelemetryPacket telemetry;

unsigned long lastTelemetrySent = 0;

// RC Car Remote MAC Address: 64:E8:33:B5:D8:40
uint8_t remoteAddr[] = {0x64, 0xE8, 0x33, 0xB5, 0xD8, 0x40}; // MAC of remote

void onCommandRecv(const esp_now_recv_info_t *recv_info, 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;
}

// Function to ensure peer is always registered
void ensurePeerConnected() {
  if (!esp_now_is_peer_exist(remoteAddr)) {
    esp_now_peer_info_t peerInfo = {};
    memcpy(peerInfo.peer_addr, remoteAddr, 6);
    peerInfo.channel = 0;
    peerInfo.encrypt = false;
    esp_err_t res = esp_now_add_peer(&peerInfo);
    if (res == ESP_OK) {
      Serial.println("Re-added remote peer");
    } else {
      Serial.printf("Failed to add peer: %d\n", res);
    }
  }
}

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);
  PrintMacAddress();

  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);

}

void loop() {

  ensurePeerConnected();

  if (millis() - lastTelemetrySent > 500) {
    telemetry.battery = analogRead(36); // Adjust this as needed
    esp_now_send(remoteAddr, (uint8_t*)&telemetry, sizeof(telemetry));
    lastTelemetrySent = millis();
  }

}

void PrintMacAddress() {
  delay(100); // Let hardware settle
  uint8_t mac[6];
  esp_wifi_get_mac(WIFI_IF_STA, mac); // Get MAC address for STA
  Serial.print("Car MAC Address: ");
  Serial.printf("%02X:%02X:%02X:%02X:%02X:%02X\n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
}