#include <Arduino.h>
#include "SerialProcess.h"

#define LEDRED 14
#define LEDORANGE 13
#define LEDGREEN 12 

unsigned long previousMillis = 0;
const unsigned long greenDuration = 5000;
const unsigned long yellowDuration = 2000;
const unsigned long redDuration = 5000;
const unsigned long transitionDuration = 2000;
const unsigned long heartbeatInterval = 1000;
const unsigned long heartbeatTimeout = 3000;
const unsigned long blinkInterval = 500;
unsigned long lastHeartbeatMillis = 0;
unsigned long lastBlinkMillis = 0;
bool blinkState = false;

enum State { GREEN, YELLOW, RED, TRANSITION, ERROR };
State currentState = GREEN;

const char *slavecheck = "#slvchck;";
const char *slaveack = "#slvack;";
const char *masterack = "#mstack;";

const char *turnRed = "tr";
const char *turnOrange = "to";
const char *turnGreen = "tg";
const char *heartbeat = "hb";

int node; // other bord addres number
int address = 0; // Device address

void setRed(){
  digitalWrite(LEDRED, HIGH); 
  digitalWrite(LEDORANGE, LOW);
  digitalWrite(LEDGREEN, LOW); 
}
void setOrange(){
  digitalWrite(LEDRED, LOW); 
  digitalWrite(LEDORANGE, HIGH);
  digitalWrite(LEDGREEN, LOW); 
}
void setGreen(){
  digitalWrite(LEDRED, LOW); 
  digitalWrite(LEDORANGE, LOW);
  digitalWrite(LEDGREEN, HIGH); 
}

SerialProcess serialcom(0);

bool MasterCheck() {
  SerialProcess serialcomchecker(1000);
  const unsigned long timeout = 2000;
  unsigned long startTime = millis();
  bool isMaster = true;
  bool checkSent = false;
  bool gotResponse = false;

  delay(random(100, 600));  // Randomize startup slightly

  Serial.print(slavecheck);  // Send check to see if someone replies
  checkSent = true;

  while (millis() - startTime < timeout) {
    if (Serial.available()) {
      serialcomchecker.SerialInput();
      char* payload = serialcomchecker.getReceivedData();

      if (strcmp(payload, slavecheck) == 0) {
        // Got a check while we also sent a check — respond and become SLAVE
        Serial.print(slaveack);
        isMaster = false;
        break;
      } else if (strcmp(payload, slaveack) == 0) {
        // Got an ACK from the other side — we are master
        Serial.print(masterack);
        isMaster = true;
        break;
      } else if (strcmp(payload, masterack) == 0) {
        // Got master ack — we must be slave
        isMaster = false;
        break;
      }
    }
  }

  // Failsafe: no response at all
  if (!Serial.available() && !gotResponse) {
    // Assume we are master
    isMaster = true;
    Serial.print(masterack);
  }

  return isMaster;
}


void updateLights() {
  switch (currentState) {
    case GREEN:
      setGreen();
      serialcom.sendMessage(node, turnGreen);
      break;
    case YELLOW:
      setOrange();          
      serialcom.sendMessage(node, turnOrange);
      break;
    case RED:
      setGreen();
      serialcom.sendMessage(node, turnRed);
      break;
    case TRANSITION:
      serialcom.sendMessage(node, turnOrange);
      break;
    case ERROR:
      if (millis() - lastBlinkMillis >= blinkInterval) {
        blinkState = !blinkState;
        digitalWrite(LEDORANGE, blinkState ? HIGH : LOW);
        lastBlinkMillis = millis();
      }
      break;
  }
}

void sendHeartbeat() {
  serialcom.sendMessage(node,heartbeat);
  Serial.println("Sent: Heartbeat");
}

void master(){
  bool running = true;
  while (running){
    unsigned long currentMillis = millis();
    
    if (currentMillis - lastHeartbeatMillis >= heartbeatInterval) {
      sendHeartbeat();
      lastHeartbeatMillis = currentMillis;
    }
    
    if (currentMillis - lastHeartbeatMillis > heartbeatTimeout) {
      currentState = ERROR;
      updateLights();
      return;
    }
    
    switch (currentState) {
      case GREEN:
        if (currentMillis - previousMillis >= greenDuration) {
          currentState = YELLOW;
          previousMillis = currentMillis;
          updateLights();
        }
        break;
      case YELLOW:
        if (currentMillis - previousMillis >= yellowDuration) {
          currentState = RED;
          previousMillis = currentMillis;
          updateLights();
        }
        break;
      case RED:
        if (currentMillis - previousMillis >= redDuration) {
          currentState = TRANSITION;
          previousMillis = currentMillis;
          updateLights();
        }
        break;
      case TRANSITION:
        if (currentMillis - previousMillis >= transitionDuration) {
          currentState = GREEN;
          previousMillis = millis();
          updateLights();
        }
        break;
      case ERROR:
        updateLights();
        break;
    }
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(LEDGREEN, OUTPUT);
  pinMode(LEDORANGE, OUTPUT);
  pinMode(LEDRED, OUTPUT);
  bool MorS = MasterCheck(); // Check if master or slave
  //set address for master or slave
  if (MorS == false){
    address = 2; // Set address for this slave 
    node = 1; // Set address for master
  } else {
    address = 1; // Set address for this master
    node = 2; // Set address for slave
  }
  previousMillis = millis();
  lastHeartbeatMillis = millis();
  updateLights();
  setRed();
  serialcom.changeAddress(address); // Set address for serial communication
}

void slave(){
  bool running = true;
  char *command;
  while (running && Serial.available() > 0){
    serialcom.SerialInput();
    serialcom.getPayload(command);
    if (strcmp(command, turnRed)) setRed();
    else if (strcmp(command, turnOrange)) setOrange();
    else if (strcmp(command, turnGreen)) setGreen();
    else if (strcmp(command, heartbeat)) {
      lastHeartbeatMillis = millis();
      digitalWrite(LEDRED, LOW);  // Reset the orange blink pattern
      digitalWrite(LEDORANGE, LOW);
    }
    else {
      Serial.print("slave: unknown command");
    }
  }
  if (millis() - lastHeartbeatMillis > heartbeatTimeout) {
    // Blink the red and yellow LEDs to indicate an error (orange light)
    if (millis() - lastBlinkMillis >= blinkInterval) {
      blinkState = !blinkState;
      digitalWrite(LEDORANGE, blinkState ? HIGH : LOW);
      lastBlinkMillis = millis();
    }
  }
}

void loop(){
  serialcom.changeAddress(2);
  if (address == 1) slave(); //master
  else if (address == 2) slave(); //slave
  else Serial.print("master slave issue");
}
