/*
 * BoardB_Receiver.ino — XIAO ESP32-S3 + Servo
 * ESP32 Arduino Core v3.x
 *
 * Wiring:
 *   Servo Signal → D8
 *   Servo VCC    → 5V
 *   Servo GND    → GND
 */

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

const int SERVO_PIN = D8;

Servo myServo;

// Must match Board A struct exactly
typedef struct Message {
  float distanceCm;
} Message;

// Distance → servo angle: 5 cm (close) → 180°, 50 cm (far) → 0°
int distanceToAngle(float cm) {
  if (cm < 5)  cm = 5;
  if (cm > 50) cm = 50;
  return map((int)cm, 5, 50, 180, 0);
}

// v3.x recv callback — first argument is esp_now_recv_info*
void onDataRecv(const esp_now_recv_info *info, const uint8_t *data, int len) {
  Message incoming;
  memcpy(&incoming, data, sizeof(incoming));

  int angle = distanceToAngle(incoming.distanceCm);
  myServo.write(angle);

  Serial.printf("From: %02X:%02X:%02X:%02X:%02X:%02X  ",
                info->src_addr[0], info->src_addr[1], info->src_addr[2],
                info->src_addr[3], info->src_addr[4], info->src_addr[5]);
  Serial.print("Distance: ");
  Serial.print(incoming.distanceCm, 1);
  Serial.print(" cm → Servo: ");
  Serial.print(angle);
  Serial.println("°");
}

void setup() {
  Serial.begin(115200);
  delay(1000);

  myServo.attach(SERVO_PIN);
  myServo.write(0);

  WiFi.mode(WIFI_STA);
  WiFi.STA.begin();

  Serial.print("Board B MAC: ");
  Serial.println(WiFi.macAddress());

  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW init failed!");
    return;
  }

  esp_now_register_recv_cb(onDataRecv);

  Serial.println("Board B (Receiver) ready");
  Serial.println("Waiting for data from Board A...");
}

void loop() {
  // All work is done in onDataRecv callback
}