#include <Wire.h>
#include <time.h>

#define CAMERAS_NUMBER 2
#define SERIAL_BAUD_RATE 115200
#define DELAY_LOWHIGH 1000 // Duration of closed switch (= trigger)

// Declare functions
void send_timestamp(byte cameraNumber, unsigned long timestamp);
void states_machine(struct Camera *camera);
void update_states_machine(struct Camera *camera);
void cameras_setup();

enum States {
  START,
  IDLE,
  TRIGGER,
  RELEASE,
  REFOCUS
};

enum Events {
  EVENT_INIT,
  EVENT_TRIGGER,
  EVENT_FOCUS_OFF,
  EVENT_DELAY_EXPIRED,
  EVENT_RELEASE
};

struct Camera {
  States STATES;
  byte NUMBER;
  unsigned long StartTime[3];
  int SHUTTER;
  int FOCUS;
};

Camera cameras[CAMERAS_NUMBER];

void setup() {
  Serial.begin(SERIAL_BAUD_RATE);
  cameras_setup();
}

void cameras_setup() {
  for (int i = 0; i < CAMERAS_NUMBER; i++) {
    cameras[i].SHUTTER = A0 + 2 * i;
    cameras[i].FOCUS = A1 + 2 * i;
    pinMode(cameras[i].SHUTTER, OUTPUT);
    pinMode(cameras[i].FOCUS, OUTPUT);
    digitalWrite(cameras[i].SHUTTER, LOW);
    digitalWrite(cameras[i].FOCUS, LOW);
    cameras[i].NUMBER = 0x01 + i;
    cameras[i].STATES = START;
    for (int j = 0; j < 3; j++) {
        cameras[i].StartTime[j] = 0;
    }
  }
}

void states_machine(struct Camera *camera, Events event) {
  switch (camera->STATES) {
    case START:
      if (event == EVENT_INIT) {
        camera->STATES = REFOCUS;
        Serial.write(0xFE);
      }
      break;
    case IDLE:
      if (event == EVENT_TRIGGER) {
        camera->STATES = TRIGGER;
      }
      else if (event == EVENT_FOCUS_OFF) {
        camera->STATES = RELEASE;
      }
      else if (event == EVENT_INIT) {
        Serial.write(0xFD);
      }
      break;
    case TRIGGER:
      if (event == EVENT_RELEASE)
        camera->STATES = RELEASE;
      break;
    case RELEASE:
      if (event == EVENT_DELAY_EXPIRED) {
        camera->StartTime[1] = millis();
        camera->STATES = REFOCUS;
      }
      break;
    case REFOCUS:
      if (event == EVENT_DELAY_EXPIRED) {
        camera->StartTime[2] = millis();
        send_timestamp(camera->NUMBER, camera->StartTime);
        camera->STATES = IDLE;
      }
      break;
  }
}

void update_states_machine(struct Camera *camera) {
  switch (camera->STATES) {
    case START:
      break;
    case IDLE:
      camera->StartTime[0] = millis();
      break;
    case TRIGGER:
      digitalWrite(camera->FOCUS, HIGH);
      digitalWrite(camera->SHUTTER, HIGH);
      camera->StartTime[0] = millis();
      states_machine(camera, EVENT_RELEASE);
      break;
    case RELEASE:
      digitalWrite(camera->SHUTTER, LOW);
      digitalWrite(camera->FOCUS, LOW);
      if (millis() - camera->StartTime[0] >= DELAY_LOWHIGH) {
        states_machine(camera, EVENT_DELAY_EXPIRED);
      }
      break;
    case REFOCUS:
      digitalWrite(camera->SHUTTER, LOW);
      digitalWrite(camera->FOCUS, HIGH);
      camera->StartTime[1] = millis();
      if (millis() - camera->StartTime[0] >= DELAY_LOWHIGH) {
        states_machine(camera, EVENT_DELAY_EXPIRED);
      }
      break;
  }
}

void send_timestamp(byte cameraNumber, unsigned long millisArray[3]) {
  byte message[16];

  // Add camera number to message
  message[0] = 0xFF; // start of message
  message[1] = 16;   // message size = 16 ; 0x10
  message[2] = cameraNumber;

  // Loop through the array and add each timestamp to the message
  int offset = 3;
  for (int i = 0; i < 3; i++) {
    // Extract each byte from the timestamp and add it to the message
    message[0 + i * 4 + offset] = (millisArray[i] >> 24) & 0xFF;
    message[1 + i * 4 + offset] = (millisArray[i] >> 16) & 0xFF;
    message[2 + i * 4 + offset] = (millisArray[i] >> 8) & 0xFF;
    message[3 + i * 4 + offset] = millisArray[i] & 0xFF;
  }

  message[15] = '\n'; // end of message = 0x10

  // Send the message over the serial port
  Serial.write(message, sizeof(message));

}


void loop() {
  if (Serial.available() > 0) {
    String inputString = Serial.readStringUntil('\n');
    inputString.trim(); // Supprime les espaces blancs

    // Convertir la chaîne hexadécimale en entier
    int input = strtol(inputString.c_str(), NULL, 16);

    // Pour débogage
    Serial.print("Valeur reçue : 0x");
    Serial.println(input, HEX);
    switch (input) {
      case 0xFE:
        for (int i = 0; i < CAMERAS_NUMBER; i++) {
          states_machine(&cameras[i], EVENT_INIT);
        }
        break;
      case 0xFF:
        for (int i = 0; i < CAMERAS_NUMBER; i++) {
          if (cameras[i].STATES != START) {
            states_machine(&cameras[i], EVENT_TRIGGER);
          }
        }
        break;
      case 0xAA:
        for (int i = 0; i < CAMERAS_NUMBER; i++) {
          states_machine(&cameras[i], EVENT_FOCUS_OFF);
        }
        break;
      case 0xBB:
        if (cameras[0].STATES != START) {
          states_machine(&cameras[0], EVENT_TRIGGER);
        }
        break;
      case 0xCC:
        if (cameras[1].STATES != START) {
          states_machine(&cameras[1], EVENT_TRIGGER);
        }
        break;
    }
  }
  for (int i = 0; i < CAMERAS_NUMBER; i++) {
    update_states_machine(&cameras[i]);
  }
}