/*
 * SENDER  — XIAO ESP32-C3 + GY-521 (MPU6050)
 * Reads board TILT (from the accelerometer, not the gyro), reduces it to ONE
 * tilt angle, maps that to a musical pitch, and broadcasts the frequency over
 * ESP-NOW to the buzzer board.
 *
 * Wiring (XIAO ESP32-C3):
 *   MPU6050 SDA -> D1  (GPIO3)
 *   MPU6050 SCL -> D0  (GPIO2)
 *   MPU6050 VCC -> 3V3
 *   MPU6050 GND -> GND
 *
 * Built for ESP32 Arduino core 3.x (current). 
 */

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

// ---------- Pins ----------
static const int PIN_SDA = 3;   // D1
static const int PIN_SCL = 2;   // D0

// ---------- MPU6050 ----------
static const uint8_t MPU_ADDR        = 0x68;
static const uint8_t REG_PWR_MGMT_1  = 0x6B;
static const uint8_t REG_WHO_AM_I    = 0x75;
static const uint8_t REG_ACCEL_XOUT_H= 0x3B;

// ---------- ESP-NOW ----------
uint8_t broadcastAddress[] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
static const uint8_t WIFI_CHANNEL = 1;   // both boards must match

typedef struct __attribute__((packed)) {
  uint16_t freq;     // Hz (0 = silent)
} message_t;
message_t msg;

esp_now_peer_info_t peerInfo;

// ---------- Tilt -> pitch mapping ----------
static const float ANGLE_MIN = -75.0f;   // deg (tilt one way)
static const float ANGLE_MAX =  75.0f;   // deg (tilt the other way)
static const float FREQ_MIN  = 150.0f;   // Hz at full tilt one way
static const float FREQ_MAX  = 1500.0f;  // Hz at full tilt the other way

// ---------- Noise control ----------
static const float ALPHA   = 0.15f;      // low-pass: lower = smoother/slower
static const float HYST    = 0.6f;       // semitones of slack before pitch changes
float  angleFilt = 0.0f;                 // filtered tilt angle
float  curNote   = 0.0f;                 // current quantized semitone (rel. A4)

void readAccel(int16_t &ax, int16_t &ay, int16_t &az) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(REG_ACCEL_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom((int)MPU_ADDR, 6, (int)true);
  ax = (int16_t)((Wire.read() << 8) | Wire.read());
  ay = (int16_t)((Wire.read() << 8) | Wire.read());
  az = (int16_t)((Wire.read() << 8) | Wire.read());
}

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

  // --- I2C + wake MPU6050 ---
  Wire.begin(PIN_SDA, PIN_SCL);
  Wire.setClock(400000);

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(REG_PWR_MGMT_1);
  Wire.write(0x00);                  // clear sleep bit
  Wire.endTransmission(true);
  delay(100);

  // quick presence check
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(REG_WHO_AM_I);
  Wire.endTransmission(false);
  Wire.requestFrom((int)MPU_ADDR, 1, (int)true);
  uint8_t who = Wire.available() ? Wire.read() : 0xFF;
  if (who != 0x68) Serial.printf("WARNING: MPU6050 not found (WHO_AM_I=0x%02X)\n", who);

  // --- WiFi / ESP-NOW ---
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  esp_wifi_set_channel(WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE);

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

  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = WIFI_CHANNEL;
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) != ESP_OK)
    Serial.println("Failed to add broadcast peer");

  // seed filter with first real reading so it doesn't ramp from 0
  int16_t ax, ay, az;
  readAccel(ax, ay, az);
  angleFilt = atan2f((float)ax, sqrtf((float)ay*ay + (float)az*az)) * 180.0f / PI;
  curNote   = 12.0f * log2f((FREQ_MIN + FREQ_MAX) * 0.5f / 440.0f);

  Serial.println("Sender ready");
}

void loop() {
  int16_t ax, ay, az;
  readAccel(ax, ay, az);

  // ONE tilt axis: angle of gravity vector (flat = 0, +/- = tilt each way)
  float angle = atan2f((float)ax, sqrtf((float)ay*ay + (float)az*az)) * 180.0f / PI;

  // low-pass filter (kills accelerometer jitter)
  angleFilt = ALPHA * angle + (1.0f - ALPHA) * angleFilt;

  // clamp + linear map to a continuous frequency
  float a = angleFilt;
  if (a < ANGLE_MIN) a = ANGLE_MIN;
  if (a > ANGLE_MAX) a = ANGLE_MAX;
  float freq = FREQ_MIN + (a - ANGLE_MIN) * (FREQ_MAX - FREQ_MIN) / (ANGLE_MAX - ANGLE_MIN);

  // snap to nearest musical semitone, with hysteresis so it won't flutter
  float semis = 12.0f * log2f(freq / 440.0f);     // continuous semitone index
  if (fabsf(semis - curNote) > HYST) curNote = roundf(semis);
  float snapped = 440.0f * powf(2.0f, curNote / 12.0f);

  msg.freq = (uint16_t)(snapped + 0.5f);
  esp_now_send(broadcastAddress, (uint8_t*)&msg, sizeof(msg));

  Serial.printf("angle=%6.1f  freq=%u Hz\n", angleFilt, msg.freq);
  delay(30);   // ~33 updates/sec
}