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

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

uint8_t carAddr[] = {0x50, 0x78, 0x7D, 0x43, 0x14, 0x00};

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

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

TelemetryPacket telemetry;

// The callback function now takes esp_now_recv_info_t*
void onTelemetry(const esp_now_recv_info_t *recv_info, const uint8_t *incomingData, int len) {
  memcpy(&telemetry, incomingData, sizeof(telemetry));
  Serial.print("Left: "); Serial.println(telemetry.leftSpeed);
  Serial.print("Right: "); Serial.println(telemetry.rightSpeed);
  Serial.print("Battery: "); Serial.println(telemetry.battery);

  // Optionally, to get source MAC from recv_info:
  // Serial.printf("From MAC: %02X:%02X:%02X:%02X:%02X:%02X\n",
  //    recv_info->src_addr[0], recv_info->src_addr[1], recv_info->src_addr[2], recv_info->src_addr[3], recv_info->src_addr[4], recv_info->src_addr[5]);
}

void setup() {
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);
  PrintMacAddress();

  esp_now_init();
  esp_now_peer_info_t peerInfo = {};
  memcpy(peerInfo.peer_addr, carAddr, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  esp_now_add_peer(&peerInfo);
  esp_now_register_recv_cb(onTelemetry); // Now matches new signature
}

void loop() {
  CommandPacket cmd;
  cmd.speed = map(analogRead(0), 0, 4095, -255, 255);
  cmd.turn  = map(analogRead(1), 0, 4095, -255, 255);
  esp_now_send(carAddr, (uint8_t*)&cmd, sizeof(cmd));
  delay(100);
}

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