#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BMP280.h>
// #include <Servo.h> // cant use with altSoftSerial b/c both use Timer 1.(or customize lib for yourself) 
#include <Servo8Bit.h> // uses Timer 2 //also lighter but lower resolution // will only operate at 8Mhz or 1Mhz(clock) // i used a 16Mhz compatible fork from github
#include <math.h>
// #include <SoftwareSerial.h> // not for reltime comm / slow / unreliable / uses cpu polling 
#include <AltSoftSerial.h> // much better than softSerial, / Uses RX = 8, TX = 9 automatically 
// #define attinySerial Serial // Uno has only one hardware UART: Serial (used by USB). So using Serial means you can't debug via Serial Monitor unless you use an external USB-UART adapter.

// Constants
#define SEA_LEVEL_PRESSURE 101325.0
#define TEMPERATURE 288.15
#define GRAVITY 9.80665
#define GAS_CONSTANT 287.05

// UART pins for ATtiny communication
// #define ATTINY_RX_PIN 0    // Arduino pin 7 connects to ATtiny RX (pin 0)
// #define ATTINY_TX_PIN 1    // Arduino pin 8 connects to ATtiny TX (pin 2)
// SoftwareSerial attinySerial(ATTINY_TX_PIN, ATTINY_RX_PIN); // RX, TX / not using due to slow speed and unstable 
AltSoftSerial attinySerial; // RX = 8, TX = 9

// RF24 configuration
#define CE_PIN 5
#define CSN_PIN 10
RF24 radio(CE_PIN, CSN_PIN);
const byte address[6] = "00001";

// Servos
#define PROBE_SERVO_PIN 5
#define CLIP_SERVO_PIN 3
Servo8Bit probeServo;
Servo8Bit clipServo;

// Other pins
#define BUTTON_PIN 2
#define CONNECTION_PIN 4

// BMP280 setup
Adafruit_BMP280 bmp;

// Data structure
struct BMPData {
  float temperature;
  float pressure;
  float distance;
} bmpData_module, bmpData_inbuilt;

// Communication protocol - using simple byte commands instead of complex packets
#define CMD_SET_DELAY 0xAA    // Command to set delay value
#define CMD_TRIGGER 0xBB      // Command to trigger the servo
#define CMD_STATUS 0xCC       // Command to request status
#define ACK_RECEIVED 0xDD     // Acknowledgment from ATtiny

// Functions
float calculate_height_from_sea(float pressure) {
  return (TEMPERATURE / (GRAVITY / GAS_CONSTANT)) * log(SEA_LEVEL_PRESSURE / pressure);
}

float calculate_delay(float distance_from_ground) {
  return sqrt((2 * distance_from_ground) / GRAVITY);
}

// Improved communication function that sends data in a more reliable way
bool program_clip(float delayToSend) {
  Serial.print("Programming ATtiny with delay: ");
  Serial.println(delayToSend);
  
  // Send command byte first
  attinySerial.write(CMD_SET_DELAY);
  delay(10); // Give ATtiny time to process
  
  // Send the float value byte by byte with verification
  byte* floatBytes = (byte*)&delayToSend;
  for (int i = 0; i < sizeof(float); i++) {
    attinySerial.write(floatBytes[i]);
    delay(5); // Small delay between bytes to prevent data loss
  }
  
  // Send a simple checksum (XOR of all float bytes)
  byte checksum = 0;
  for (int i = 0; i < sizeof(float); i++) {
    checksum ^= floatBytes[i];
  }
  attinySerial.write(checksum);
  
  // Wait for acknowledgment from ATtiny
  // unsigned long timeout = millis() + 1000; // 1 second timeout
  // while (millis() < timeout) {
  //   if (attinySerial.available()) {
  //     byte response = attinySerial.read();
  //     if (response == ACK_RECEIVED) {
  //       Serial.println("ATtiny acknowledged delay programming");
  //       return true;
  //     }
  //   }
  // }
  
  // Serial.println("ATtiny did not acknowledge - programming may have failed");
  // return false; 
   
   return true;
}

// Function to trigger the ATtiny to activate its servo
bool trigger_attiny() {
  Serial.println("Sending trigger command to ATtiny");
  attinySerial.write(CMD_TRIGGER);
  delay(50);
  return true;
}

int calculate_clicks() {
  static int clicks = 0;
  static bool timerStarted = false;
  static unsigned long startTime = 0;
  static bool lastButtonState = LOW;

  int buttonState = digitalRead(BUTTON_PIN);

  if (buttonState == HIGH && lastButtonState == LOW) {
    clicks++;
    if (!timerStarted) {
      timerStarted = true;
      startTime = millis();
    }
    delay(50); // Debounce delay
  }

  lastButtonState = buttonState;

  if (timerStarted && millis() - startTime > 5000) {
    int result = clicks;
    clicks = 0;
    timerStarted = false;
    return result;
  }

  return -1; // Still counting or no clicks
}

void signal_as_connected() {
  digitalWrite(CONNECTION_PIN, HIGH);
}

void setup() {
  Serial.begin(9600);
  attinySerial.begin(9600); // Start communication with ATtiny
  
  Serial.println("Arduino Uno starting up...");

  // Initialize servos
  probeServo.attach(PROBE_SERVO_PIN);
  clipServo.attach(CLIP_SERVO_PIN);
  probeServo.write(0);
  clipServo.write(0);

  // Initialize pins
  pinMode(BUTTON_PIN, INPUT);
  pinMode(CONNECTION_PIN, OUTPUT);
  signal_as_connected();

  // Initialize I2C for BMP280
  Wire.begin();

  // Initialize BMP280 sensor
  if (!bmp.begin(0x76) && !bmp.begin(0x77)) {
    Serial.println("Could not find BMP280 sensor!");
    while (1); // Halt execution
  }

  // Configure BMP280 for optimal performance
  bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
                  Adafruit_BMP280::SAMPLING_X1,
                  Adafruit_BMP280::SAMPLING_X4,
                  Adafruit_BMP280::FILTER_X4,
                  Adafruit_BMP280::STANDBY_MS_125);

  delay(100);

  // Initialize nRF24L01 radio
  if (!radio.begin()) {
    Serial.println("nRF24L01 initialization failed!");
    while (1);
  }

  radio.openReadingPipe(0, address);
  radio.startListening();
  
  Serial.println("Setup complete. Ready for operation.");
}

void loop() {
  // Read local sensor data
  float temperature = bmp.readTemperature();
  float pressure = bmp.readPressure();

  bmpData_inbuilt.temperature = temperature;
  bmpData_inbuilt.pressure = pressure;

  // Check for radio data from remote module
  if (radio.available()) {
    radio.read(&bmpData_module, sizeof(bmpData_module));
  }

  // Calculate pressure offset between modules (done once)
  static float offset = 0;
  static bool offset_calculated = false;
  if (!offset_calculated && bmpData_module.pressure > 0) {
    offset = bmpData_module.pressure - bmpData_inbuilt.pressure;
    offset_calculated = true;
    Serial.print("Calculated pressure offset: ");
    Serial.println(offset);
  }

  // Calculate altitudes and distances
  float module_pressure = bmpData_module.pressure - offset;
  float module_altitude = calculate_height_from_sea(module_pressure);
  float drone_altitude = calculate_height_from_sea(bmpData_inbuilt.pressure);
  float drone_distance_from_ground = drone_altitude - module_altitude;

  // Calculate the delay needed for the clip release
  float delayToSend = 5;//calculate_delay(abs(drone_distance_from_ground));
  
  // Constrain the delay to reasonable values
  delayToSend = constrain(delayToSend, 0.1, 50.0);

  // State variables
  static bool module_dropped = false;
  static bool clip_programmed = false;
  static bool clip_dropped = false;
 
  
  /*
  Click Actions:
    1 click  → Drop probe (or drop clip if already programmed)
    2 clicks → Program ATtiny with delay
    3 clicks → Emergency drop of clip
  */
  // Handle button clicks for different operations
  int click_count = calculate_clicks();

  if (click_count != -1) {
    Serial.print("Button clicked ");
    Serial.print(click_count);
    Serial.println(" times");
    
    switch (click_count) {
      case 1:
        // Single click: Drop probe module OR drop clip if module already dropped
        if (module_dropped) { 
          Serial.println("Programming ATtiny...");
          clip_programmed = program_clip(delayToSend); 
          // if(!clip_programmed) {
          //   break; 
          // }

          Serial.println("Dropping clip...");
          clipServo.write(180);
          trigger_attiny(); // Also trigger the ATtiny
          digitalWrite(CONNECTION_PIN, LOW); // Disconnect signal
          delay(50);
          clip_dropped = true;
        } else {
          Serial.println("Dropping probe module...");
          probeServo.write(180);
          module_dropped = true;
        }
        break;

      case 2:
        // Double click: Program the ATtiny with current delay 
        Serial.println("ATtiny Value :");
        attinySerial.write(CMD_SET_DELAY); 

        Serial.println("Programming ATtiny...");
        clip_programmed = program_clip(delayToSend); 
          
        //operator programs
        // Serial.println("Dropping clip...");
        // clipServo.write(180);
        // trigger_attiny(); // Also trigger the ATtiny
        // digitalWrite(CONNECTION_PIN, LOW); // Disconnect signal
        // delay(50);
        // clip_dropped = true;
        break;

      case 3:
        // Triple click: Drop clip immediately
        Serial.println("Emergency clip drop...");
        clipServo.write(180);
        trigger_attiny();
        digitalWrite(CONNECTION_PIN, LOW);
        delay(50);
        clip_dropped = true;
        break;

      default:
        Serial.println("Invalid click count");
        break;
    }
  }

  // Maintain connection signal
  if (!clip_dropped) {
    signal_as_connected();
  }

  // Lower connection pin when clip is programmed to signal ATtiny
  if (clip_programmed && !clip_dropped) {
    digitalWrite(CONNECTION_PIN, LOW);
  }

  // Debug output (uncomment for debugging)
  /*
  Serial.print("Distance from ground: ");
  Serial.print(drone_distance_from_ground);
  Serial.print("m, Calculated delay: ");
  Serial.print(delayToSend);
  Serial.print("s, Module dropped: ");
  Serial.print(module_dropped ? "Yes" : "No");
  Serial.print(", Clip programmed: ");
  Serial.print(clip_programmed ? "Yes" : "No");
  Serial.print(", Clip dropped: ");
  Serial.println(clip_dropped ? "Yes" : "No");
  */

  delay(100); // Main loop delay
}