#include "wiring_private.h"

// MASKS
#define violation_S1_S4_mask 0b00100000 // Mask associated to PORTB Violation Led for traffic light S14
#define red_S1_S4_mask 0b00010000 // Mask associated to PORTB Red Led for traffic light S14
#define yellow_S1_S4_mask 0b00001000 // Mask associated to PORTB Yellow Led for traffic light S14
#define green_S1_S4_mask 0b00000100 // Mask associated to PORTB Green Led for traffic light S14
#define pedestrian_S1_S4_mask 0b00000010 // Mask associated to PORTB Pedestrian Call Button for traffic light S14
#define traffic_S1_S4_mask 0b00000001 // Mask associated to PORTB Traffic Switch for traffic light S14

#define violation_S2_S3_mask 0b10000000 // Mask associated to PORTD Violation Led for traffic light S23
#define red_S2_S3_mask 0b01000000 // Mask associated to PORTD Red Led for traffic light S23
#define yellow_S2_S3_mask 0b00100000 // Mask associated to PORTD Yellow Led for traffic light S23
#define green_S2_S3_mask 0b00010000 // Mask associated to PORTD Green Led for traffic light S23
#define pedestrian_S2_S3_mask 0b00001000 // Mask associated to PORTD Pedestrian Call Button for traffic light S23
#define traffic_S2_S3_mask 0b00000100 // Mask associated to PORTD Traffic Switch for traffic light S23

#define speed_sensor_S1_mask 0b00000001 // Mask associated to PORTC Speed Potentiometer for traffic light S1
#define speed_sensor_S4_mask 0b00001000 // Mask associated to PORTC Speed Potentiometer for traffic light S4

#define speed_sensor_S2_mask 0b00000010 // Mask associated to PORTC Speed Potentiometer for traffic light S2
#define speed_sensor_S3_mask 0b00000100 // Mask associated to PORTC Speed Potentiometer for traffic light S3

#define key_mask 0b00010000 // Mask associated to PORTC Key Switch for all traffic lights

// Global Variables
volatile uint8_t trafficLights14Status = 0;
volatile uint8_t trafficLights23Status = 0;

// Global Variables bits
#define isPedestrianCallInHandle 0b10000000
#define pedestrianBit 0b01000000
#define keyBit 0b00100000
#define violationBit 0b00010000
#define trafficBit 0b00001000
#define redBit 0b00000100
#define yellowBit 0b00000010
#define greenBit 0b00000001

#define setBit(trafficLightsSxSyStatus, mask) (trafficLightsSxSyStatus |= (mask))
#define clearBit(trafficLightsSxSyStatus, mask) (trafficLightsSxSyStatus &= ~(mask))
#define isBitSet(trafficLightsSxSyStatus, mask) ((trafficLightsSxSyStatus & (mask)) != 0)

// Time Intervals
#define yellowDuration 2000;
#define noColorDuration 1000;
// TODO review
#define redDuration 10000;
#define greenDuration 10000;
#define extendedRedDuration 20000;
#define extendedGreenDuration 20000;


// possibile modifica uint16t con reset del tempo quando raggiunge il limite
unsigned long systemTimer = 0;
unsigned long oldSystemTime = 0; // used for correct calculation of time intervals
// TODO unsigned long oldSystemTimeS2S3 = 0;
unsigned long timeSinceLastPedestrianCallForS1S4 = 0;

void setup() {
  // Serial.begin(115200);
  setupTrafficLightsS1S4();
  setupTrafficLightsS2S3();
  setupKey();
  setupAnalogicToDigitalConverter();
  // Timer Setup
  cli(); // Critical Section
  TCCR1A = 0; // Timer1 normal mode
  TCCR1B = _BV(WGM12) | _BV(CS11) | _BV(CS10); // CTC Mode, prescaler 64
  OCR1A = 249; // Interrupt every 1 ms thanks to 64 & 249
  TIMSK1 = _BV(OCIE1A); // Interrupt for OCR1A
  sei(); // Critical Section End
}

// Setup related logic

void setupTrafficLightsS1S4() {
  DDRB |= 0b00111100;
  PORTB |= _BV(1); // Input Pullup for pedestrian_S1_S4
  PORTB |= _BV(0); // Input Pullup for traffic_S1_S4
  trafficLights14Status = setBit(trafficLights14Status, yellowBit); // Blinking Yellow
  PORTB |= yellow_S1_S4_mask;
}

void setupTrafficLightsS2S3() {
  DDRD |= 0b11110000;
  PORTD |= _BV(3); // Input Pullup for pedestrian_S2_S3
  PORTD |= _BV(2); // Input Pullup for traffic_S2_S3
  trafficLights23Status = setBit(trafficLights23Status, yellowBit); // Blinking Yellow
  PORTD |= yellow_S2_S3_mask;
}

void setupKey() {
  PORTC |= _BV(4); // Input Pullup for key
}

void setupAnalogicToDigitalConverter() {
  ADMUX = 0b01000000; // REFS0 Voltage Reference
  ADCSRA = 0b10000111; // ADEN, ADPS2, ADPS1, ADPS0 (ADC enable, division factor 128)
}

// Timer update

ISR(TIMER1_COMPA_vect) {
  systemTimer++; // Every millisecond systemTimer++
  // if (! isBitSet(trafficLights14Status, isPedestrianCallInHandle))
  //   timeSinceLastPedestrianCallForS1S4++;
}

void loop() {
  handleTrafficLightS1S4();
}

///////////////////////////////////// TRAFFIC LIGHT HANDLING

void handleTrafficLightS1S4() {
  handleS1S4Detections();
  handleTrafficLightS1S4YellowScenarios();
  handleTrafficLightS1S4RedScenarios();
  handleTrafficLightS1S4GreenScenarios();
}

void checkIfS1S4ShouldGoToKey() {
  if (isKeyActive()) {
    clearBit(trafficLights14Status, redBit);
    clearBit(trafficLights14Status, greenBit);
    setBit(trafficLights14Status, yellowBit);
    PORTB &= ~red_S1_S4_mask;
    PORTB &= ~green_S1_S4_mask;
    PORTB |= yellow_S1_S4_mask;
    oldSystemTime = systemTimer;
  }
}

void handleTrafficLightS1S4YellowScenarios() {
  if ((isCrossingClear() || isKeyActive()) && ! isS1S4HandlingAPedestrianCall()
      && (isYellowOnForS1S4() || isNoColorForS1S4()))
    handleBlinkingYellowS1S4();
  else if (isTrafficActiveForS1S4() && isYellowOnForS1S4())
    handleYellowS1S4();
  else if (isTrafficActiveForS1S4() && isNoColorForS1S4())
    handleNoColorS1S4(); // Recovery routine in case of Traffic & noColor
}

void handleTrafficLightS1S4RedScenarios() {
  if (isS1S4HandlingAPedestrianCall() && isRedOnForS1S4())
    handleExtendedRedForS1S4();
  else if (isTrafficActiveForS1S4() && isRedOnForS1S4())
    handleRedForS1S4();
}

void handleTrafficLightS1S4GreenScenarios() {
  if (isTrafficActiveForS1S4() && isGreenOnForS1S4())
    handleGreenForS1S4();
}

void handleTrafficLightS2S3() {
  
}

void checkIfS2S3ShouldGoToKey() {
  if (isKeyActive()) {
    clearBit(trafficLights23Status, redBit);
    clearBit(trafficLights23Status, greenBit);
    setBit(trafficLights23Status, yellowBit);
    PORTD &= ~red_S2_S3_mask;
    PORTD &= ~green_S2_S3_mask;
    PORTD |= yellow_S2_S3_mask;
    oldSystemTime = systemTimer;
  }
}

///////////////////////////////////// BOOLEAN CHECKS

boolean isCrossingClear() {
  return (! isBitSet(trafficLights14Status, trafficBit))
    && (! isBitSet(trafficLights23Status, trafficBit));
}

// Pedestrian call checks

boolean isPedestrianCallActiveForS1S4() {
  return isBitSet(trafficLights14Status, pedestrianBit);
}

boolean isPedestrianCallActiveForS2S3() {
  return isBitSet(trafficLights23Status, pedestrianBit);
}

// Traffic checks

boolean isTrafficActiveForS1S4() {
  return isBitSet(trafficLights14Status, trafficBit);
}

boolean isTrafficActiveForS2S3() {
  return isBitSet(trafficLights23Status, trafficBit);
}

// Key active

boolean isKeyActive() {
  return isBitSet(trafficLights14Status, keyBit) 
    || isBitSet(trafficLights23Status, keyBit);
}

// Is pedestrian call being handled

boolean isS1S4HandlingAPedestrianCall() {
  return isBitSet(trafficLights14Status, isPedestrianCallInHandle);
}

boolean isS2S3HandlingAPedestrianCall() {
  return isBitSet(trafficLights23Status, isPedestrianCallInHandle);
}

// Color boolean checks

boolean isRedOnForS1S4() {
  return isBitSet(trafficLights14Status, redBit);
}

boolean isRedOnForS2S3() {
  return isBitSet(trafficLights23Status, redBit);
}

boolean isYellowOnForS1S4() {
  return isBitSet(trafficLights14Status, yellowBit);
}

boolean isYellowOnForS2S3() {
  return isBitSet(trafficLights23Status, yellowBit);
}

boolean isNoColorForS1S4() {
  return ! isBitSet(trafficLights14Status, yellowBit) && ! isBitSet(trafficLights14Status, redBit)
    && ! isBitSet(trafficLights14Status, greenBit);
}

boolean isNoColorForS2S3() {
  return ! isBitSet(trafficLights23Status, yellowBit) && ! isBitSet(trafficLights23Status, redBit)
    && ! isBitSet(trafficLights23Status, greenBit);
}

boolean isGreenOnForS1S4() {
  return isBitSet(trafficLights14Status, greenBit);
}

boolean isGreenOnForS2S3() {
  return isBitSet(trafficLights23Status, greenBit);
}

///////////////////////////////////// COLOR CYCLES




// TODO S1S3 HANNO PORTE DIVERSE !!!


// ALL RED HANDLES (TRAFFIC RED, EXTENDED RED, PEDESTRIAN RED)


void handleExtendedRedForS1S4() {
  handleS1S4Detections();
  checkIfS1S4ShouldGoToKey();
  if (systemTimer - oldSystemTime >= 20000) {
    oldSystemTime = systemTimer;
    clearBit(trafficLights14Status, isPedestrianCallInHandle);
    clearBit(trafficLights14Status, redBit);
    PORTB &= ~red_S1_S4_mask;
    if (! isTrafficActiveForS1S4() || isKeyActive()) {
      setBit(trafficLights14Status, yellowBit);
      PORTB |= yellow_S1_S4_mask;
    } else {
      setBit(trafficLights14Status, greenBit);
      PORTB |= green_S1_S4_mask;
    }
  }
}

void handleRedForS1S4() {
  handleS1S4Detections();
  checkIfS1S4ShouldGoToKey();
  if (systemTimer - oldSystemTime >= 10000) {
    oldSystemTime = systemTimer;
    clearBit(trafficLights14Status, redBit);
    PORTB &= ~red_S1_S4_mask;
    if (! isTrafficActiveForS1S4() || isKeyActive()) {
      setBit(trafficLights14Status, yellowBit);
      PORTB |= yellow_S1_S4_mask;
    } else {
      setBit(trafficLights14Status, greenBit);
      PORTB |= green_S1_S4_mask;
    }
  }
}

// ALL YELLOW HANDLES (YELLOW, BLINKING YELLOW)

void handleBlinkingYellowS1S4() {
  handleYellowS1S4();
  handleNoColorS1S4();
}

void handleYellowS1S4() {
  handleS1S4Detections();
  if (isYellowOnForS1S4() && (systemTimer - oldSystemTime >= 2000)) {
      oldSystemTime = systemTimer;
      clearBit(trafficLights14Status, yellowBit);
      PORTB &= ~yellow_S1_S4_mask; // yellow led off
      if ((isTrafficActiveForS1S4() && ! isKeyActive()) || isPedestrianCallActiveForS1S4()) {
          if (isPedestrianCallActiveForS1S4()) {  // if true start extended red handling
            clearBit(trafficLights14Status, pedestrianBit);
            setBit(trafficLights14Status, isPedestrianCallInHandle);
            timeSinceLastPedestrianCallForS1S4 = 0;
          }
          setBit(trafficLights14Status, redBit);
          PORTB |= red_S1_S4_mask;
      }
  }
}

void handleNoColorS1S4() {
  handleS1S4Detections();
  if (! isYellowOnForS1S4() && (systemTimer - oldSystemTime >= 1000)) {
      oldSystemTime = systemTimer;
      setBit(trafficLights14Status, yellowBit);
      PORTB |= yellow_S1_S4_mask; // yellow led on
  }
}

void handleBlinkingYellowS2S3() {
  handleYellowS2S3();
  handleNoColorS2S3();
}

void handleYellowS2S3() {
  handleS2S3Detections();
  if (systemTimer - oldSystemTime >= 2000) {
      oldSystemTime = systemTimer;
      clearBit(trafficLights23Status, yellowBit);
      PORTB &= ~yellow_S2_S3_mask; // yellow led off
      if ((isTrafficActiveForS2S3() && ! isKeyActive()) || isPedestrianCallActiveForS2S3()) {
          setBit(trafficLights23Status, redBit);
          PORTB |= red_S2_S3_mask;
      }
  }
}

void handleNoColorS2S3() {
  handleS2S3Detections();
  if (!isYellowOnForS2S3() && (systemTimer - oldSystemTime >= 1000)) {
    oldSystemTime = systemTimer;
    setBit(trafficLights23Status, yellowBit);
    PORTB |= yellow_S2_S3_mask; // yellow led on
  }
}

// ALL GREEN HANDLES (GREEN, EXTENDED GREEN)

void handleGreenForS1S4() {
  handleS1S4Detections();
  checkIfS1S4ShouldGoToKey();
  if (systemTimer - oldSystemTime >= 10000) {
    oldSystemTime = systemTimer;
    clearBit(trafficLights14Status, greenBit);
    PORTB &= ~green_S1_S4_mask; // yellow led off
    setBit(trafficLights14Status, yellowBit);
    PORTB |= yellow_S1_S4_mask; // yellow led on
  }
}

///////////////////////////////////// DETECTOR HANDLERS

void handleS1S4Detections() {
  handleS1S4PedestrianCallDetections();
  handleS1S4TrafficDetections();
  handleS1S4SpeedDetections();
  handleKeyDetections();
}

void handleS1S4PedestrianCallDetections() {
  // Pedestrian Call detected for S1S4 traffic light flow
  !(PINB & pedestrian_S1_S4_mask) ? setBit(trafficLights14Status, pedestrianBit): NULL;
  if ((timeSinceLastPedestrianCallForS1S4 >= 60000 && isBitSet(trafficLights14Status, trafficBit))
      || timeSinceLastPedestrianCallForS1S4 >= 300000) {
    setBit(trafficLights14Status, pedestrianBit);
  }
}

void handleS1S4TrafficDetections() {
  // Traffic detected for S1S4 traffic light flow
  !(PINB & traffic_S1_S4_mask) ? setBit(trafficLights14Status, trafficBit)
    : clearBit(trafficLights14Status, trafficBit);
}

void handleS1S4SpeedDetections() { 
  // Speed detection for S1S4 traffic light flow
  my_analogRead(0) > 50 || my_analogRead(2) > 50 
    ? setBit(trafficLights14Status, violationBit)
    : clearBit(trafficLights14Status, violationBit);
  // Update traffic lights S1S4 status
  if (isBitSet(trafficLights14Status, yellowBit) || ! isBitSet(trafficLights14Status, yellowBit))
    isBitSet(trafficLights14Status, violationBit) ? PORTB |= _BV(5) : PORTB &= ~_BV(5);
}

void handleS2S3Detections() {
  handleS2S3PedestrianCallDetections();
  handleS2S3TrafficDetections();
  handleS2S3SpeedDetections();
  handleKeyDetections();
}

void handleS2S3PedestrianCallDetections() {
  // Pedestrian Call detected for S2S3 traffic light flow
  !(PIND & pedestrian_S2_S3_mask) ? setBit(trafficLights23Status, pedestrianBit): NULL;
}

void handleS2S3TrafficDetections() {
  // Traffic detected for S2S3 traffic light flow
  !(PIND & traffic_S2_S3_mask) ? setBit(trafficLights23Status, trafficBit)
    : clearBit(trafficLights23Status, trafficBit);
}

void handleS2S3SpeedDetections() {
  // Speed detection for S2S3 traffic light flow
  my_analogRead(1) > 50 || my_analogRead(3) > 50 
    ? setBit(trafficLights23Status, violationBit)
    : clearBit(trafficLights23Status, violationBit);
  if (isBitSet(trafficLights23Status, yellowBit) || ! isBitSet(trafficLights23Status, yellowBit))
    isBitSet(trafficLights23Status, violationBit) ? PORTD |= _BV(7) : PORTD &= ~_BV(7);
}

void handleKeyDetections() {
  // if key has been detected...
  if (!(PINC & key_mask)) {
    setBit(trafficLights14Status, keyBit);
    setBit(trafficLights23Status, keyBit);
  } else {
    clearBit(trafficLights14Status, keyBit);
    clearBit(trafficLights23Status, keyBit);
  }
}

///////////////////////////////////// UTILITIES

int my_analogRead(uint8_t pin) {
  ADMUX = (ADMUX & 0xF8) | (pin & 0x07);
  ADCSRA |= (1 << ADSC);
  while (ADCSRA & (1 << ADSC));
  return ADC;
}

unsigned long my_millis() {
  return (my_micros() / 1000);
}

unsigned long my_micros() {
  return (unsigned long)(TCNT1 * (64 / 16.0));
}