#include <Wire.h>

#define I2C_ADDR 0x42

#define ESC1_PIN 3
#define ESC2_PIN 4
#define ESC3_PIN 9
#define ESC4_PIN 8

#define FRAME_US 20000   // 50Hz frame
#define FAILSAFE 500     // ms

volatile uint16_t esc_rx[4] = {0,0,0,0}; // start with "no signal"
volatile bool newData = false;

uint32_t frameStart = 0;
uint32_t lastI2C = 0;
bool armed = false;
bool mOn[4];

void onI2CReceive(int len) {
  if (len != 8) return; // Expect exactly 8 bytes

  for (int i = 0; i < 4; i++) {
    if (Wire.available() >= 2) {
      uint8_t lo = (uint8_t)Wire.read();
      uint8_t hi = (uint8_t)Wire.read();
      esc_rx[i] = ((uint16_t)hi << 8) | lo; // little-endian
    }
  }

  newData = true;
  lastI2C = millis();
}

void setup() {
  pinMode(ESC1_PIN, OUTPUT);
  pinMode(ESC2_PIN, OUTPUT);
  pinMode(ESC3_PIN, OUTPUT);
  pinMode(ESC4_PIN, OUTPUT);

  digitalWrite(ESC1_PIN, LOW);
  digitalWrite(ESC2_PIN, LOW);
  digitalWrite(ESC3_PIN, LOW);
  digitalWrite(ESC4_PIN, LOW);

  Wire.begin(I2C_ADDR);          
  Wire.onReceive(onI2CReceive);  

//  SerialUSB.begin(115200);
//  SerialUSB.println("READY");

  Serial1.begin(115200);
  Serial1.println("READY");  

  frameStart = micros();
}

void loop() {
  uint32_t now = micros();

  // Only generate pulses if we have received at least one packet
  static uint16_t esc[4] = {0,0,0,0};

  if (newData) {
    noInterrupts();
    for (int i = 0; i < 4; i++) esc[i] = esc_rx[i];
    newData = false;
    interrupts();

    // Debug print
    //SerialUSB.print("Received ESC values: ");
//    Serial1.print("Received ESC values: ");    
    /*for (int i = 0; i < 4; i++) {
      SerialUSB.print(esc[i]);
      SerialUSB.print(" ");
      Serial1.print(esc[i]);
      Serial1.print(" ");
    }*/
//    SerialUSB.println();
//    Serial1.println();
  }

  // If no data yet, keep ESC pins low (ESCs will beep)
  if (esc[0] == 0 && esc[1] == 0 && esc[2] == 0 && esc[3] == 0) {
    return; // skip PWM frame until master sends something
  }

  /* ===== START PWM FRAME ===== */
  if ((int32_t)(now - frameStart) >= FRAME_US) {
    frameStart += FRAME_US;

    digitalWrite(ESC1_PIN, HIGH);
    digitalWrite(ESC2_PIN, HIGH);
    digitalWrite(ESC3_PIN, HIGH);
    digitalWrite(ESC4_PIN, HIGH);

    mOn[0] = mOn[1] = mOn[2] = mOn[3] = true;
  }

  uint32_t elapsed = now - frameStart;

  // Failsafe: if no update for too long, hold 1000 µs
  if (millis() - lastI2C > FAILSAFE) {
    esc[0] = esc[1] = esc[2] = esc[3] = 1000;
  }

  /* ===== TURN OFF PULSES ===== */
  if (mOn[0] && elapsed >= esc[0]) { digitalWrite(ESC1_PIN, LOW); mOn[0] = false; }
  if (mOn[1] && elapsed >= esc[1]) { digitalWrite(ESC2_PIN, LOW); mOn[1] = false; }
  if (mOn[2] && elapsed >= esc[2]) { digitalWrite(ESC3_PIN, LOW); mOn[2] = false; }
  if (mOn[3] && elapsed >= esc[3]) { digitalWrite(ESC4_PIN, LOW); mOn[3] = false; }
}