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

const int ledRedPin = 25;
const int ledBluePin = 26;
const int ledGreenPin = 27;
const int MPU_addr=0x68;  // I2C address of the MPU-6050

#define WAIT 0
#define WRITE 1
#define READ 2

//uint8_t broadcastAddress[] = {0x4, 0xDD, 0x57, 0xD4, 0x43, 0xB8};
uint8_t broadcastAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

float accx, accy, accz;
uint8_t state = WAIT;
float rolling_accx[50];
int x_ind = 0;
int imu_samples = 0;
float total_accx = 0;

typedef struct struct_message {
  int gesture;
} struct_message;

struct_message gestureNum;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(115200);

  pinMode(ledRedPin, OUTPUT);
  pinMode(ledGreenPin, OUTPUT);
  pinMode(ledBluePin, OUTPUT);

  // start LED off
  setColor(HIGH, HIGH, HIGH);

  // set all x values to something high
  for (int i=0; i<50; i++) {
    rolling_accx[i] = 1;
  }

  // Set device as a Wi-Fi Station
  WiFi.mode(WIFI_STA);

  // Init ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  // Once ESPNow is successfully Init, we will register for Send CB to get the status of Trasmitted packet
  esp_now_register_send_cb(onDataSent);
  // Register peer
  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  // Add peer        
  if (esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("Failed to add peer");
    return;
  }
}

void loop(){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  accx = (Wire.read() << 8 | Wire.read()) / 16384.0;  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  accy = (Wire.read() << 8 | Wire.read()) / 16384.0;  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  accz = (Wire.read() << 8 | Wire.read()) / 16384.0;  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  
  switch(state) {
    case WAIT:
      // update rolling average
      rolling_accx[x_ind] = accx;
      total_accx = 0;
      for (int i=0; i<50; i++) {
        total_accx += rolling_accx[i];
      }
      x_ind = x_ind == 49 ? 0 : x_ind + 1;

      // when you detect the "ready to begin state", turn LED on and switch to write state
      if (total_accx / 50 < 0.5) {
        setColor(LOW, LOW, LOW);
        state = WRITE;
        // set all x values to something high
        for (int i=0; i<50; i++) {
          rolling_accx[i] = 1;
        }
       
      } else {
        setColor(HIGH, HIGH, HIGH);
        delay(20);
      }
      break;
      
    case WRITE:
      // write acceleration data to serial which will be picked up by Python code     
      Serial.print(accx);
      Serial.print(" ");
      Serial.print(accy);
      Serial.print(" ");
      Serial.println(accz);

      imu_samples = imu_samples + 1;

      // send 300 samples and then wait for Python code to send back the gesture detected (if any)
      if (imu_samples == 300) {
        imu_samples = 0;
        state = READ;
        Serial.println("DONE");
      }
      delay(10);
      break;
    case READ:
      int gesture = Serial.readString().toInt();

      if (gesture == -1) {
        // if no gesture detected, turn led red and go back to wait state
        setColor(LOW, HIGH, HIGH);
        state = WAIT;
        delay(1000);
        
      } else {
        // if gesture was detected, turn the LED blue
        setColor(HIGH, HIGH, LOW);

        // send gesture to other ESP32 
        gestureNum.gesture = gesture;
        // Send message via ESP-NOW
        esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &gestureNum, sizeof(gestureNum));
        if (result == ESP_OK) {
          Serial.println("Sent with success");
        } else {
          Serial.println("Error sending the data");
        }  
        delay(5000);
        
        // go back to wait state so you can detect another gesture
        state = WAIT;
      }
      break;
  }
}

// Callback when data is sent
void onDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}

void setColor(int r, int g, int b) {
  digitalWrite(ledRedPin, r);
  digitalWrite(ledGreenPin, g);
  digitalWrite(ledBluePin, b);
}