#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "freertos/timers.h"
#include "freertos/queue.h"
#include <TM1637.h>
#include <string.h>

// ------------- pins ---------------------
// up and down lights
#define RED1 42
#define YELLOW1 41
#define GREEN1 40
#define PIRUPDOWN 14
#define BTNUPDOWN 12
//left and right lights
#define RED2 37
#define YELLOW2 38
#define GREEN2 39
#define PIRLEFTRIGHT 15
#define BTNLEFTRIGHT 13

#define CROSSWALKLED 36

//display variables and objects
const int CLK = 2;
const int DIO = 4;
TM1637 display(CLK, DIO);

// traffic states
enum TrafficState {
  STATE_BOTH_RED,
  STATE_D1_GREEN,
  STATE_D1_YELLOW,
  STATE_BOTH_RED_2,
  STATE_D2_GREEN,
  STATE_D2_YELLOW,
  STATE_BOTH_RED_3,
  STATE_CROSSWALK
};
volatile int state = STATE_BOTH_RED;

// flags for isr
volatile bool needD1Short = false;
volatile bool needD2Short = false;
volatile bool crosswalkFlag = false;
volatile int crossCount = 0;

// track next state after crosswalk completes
volatile int nextStateAfterCrosswalk = STATE_D1_GREEN;

// freertos handles
TimerHandle_t mainTimer;        // Timer for stoplight state transitions
TimerHandle_t crosswalkTimer;  // Timer for crosswalk countdown (1 second intervals)
TimerHandle_t statusTimer;     // Timer for periodic status updates
SemaphoreHandle_t crosswalkSema; // Binary semaphore from pushbuttons to crosswalk task
QueueHandle_t printQueue;       // Queue for serial communication (gatekeeper pattern)
QueueHandle_t stoplightQueue;   // Queue for stoplight task to receive timer notifications

/**
 * Helper: Display countdown on 4-digit 7-segment display
 * Parameters: value - the countdown value to display (0-99)
 * Shared variables: display
 */
void showCountdown(int value) {
  int8_t digits[4] = {0, 0, (value / 10) % 10, value % 10};
  for (int i = 0; i < 4; i++) {
    display.display(i, digits[i]);
  }
}

/**
 * Helper: Clear 4-digit 7-segment display
 * Shared variables: display
 */
void clearDisplay() {
  for (int i = 0; i < 4; i++) {
    display.display(i, 0x7f);
  }
}

/**
 * Helper: Start crosswalk cycle
 * Sets state to CROSSWALK, turns on crosswalk LED, starts 22-second timer
 * Parameters: nextState - the state to transition to after crosswalk completes
 * Shared variables: state, nextStateAfterCrosswalk, crossCount
 */
void startCrosswalk(int nextState) {
  state = STATE_CROSSWALK;
  nextStateAfterCrosswalk = nextState;
  digitalWrite(RED1, HIGH);
  digitalWrite(RED2, HIGH);
  digitalWrite(CROSSWALKLED, HIGH);  // Turn on crosswalk LED
  crossCount = 20;  // Start countdown at 20 seconds
  showCountdown(crossCount);
  // Start crosswalk timer (1 second periodic) and main timer (22 seconds total)
  xTimerStart(crosswalkTimer, 0);
  xTimerChangePeriod(mainTimer, pdMS_TO_TICKS(22000), 0);  // 20s countdown + 2s safety
  xTimerStart(mainTimer, 0);  // Start the 22-second timer
}

/**
 * Helper: Calculate green duration based on PIR flag
 * If PIR detected, reduce to 8 seconds or remaining time, whichever is lower
 * Parameters: needShort - true if PIR detected for this direction
 *             remainingTime - current remaining green time in ms
 * Returns: Duration in milliseconds for green light
 */
int calculateGreenDuration(bool needShort, int remainingTime) {
  if (needShort) {
    // Reduce to 8 seconds or remaining time, whichever is lower
    return (remainingTime < 8000) ? remainingTime : 8000;
  } else {
    return 18000;  // Normal 18 second green duration
  }
}

/**
 * Helper: Turn direction green
 * Sets the traffic light to green for the specified direction
 * Parameters: redPin, greenPin, yellowPin - pin numbers for the direction
 *             newState - the new traffic state
 *             needShort - pointer to flag indicating if PIR detected (will be cleared)
 * Shared variables: state, mainTimer
 */
void turnDirectionGreen(int redPin, int greenPin, int yellowPin, int newState, volatile bool* needShort) {
  digitalWrite(redPin, LOW);
  digitalWrite(greenPin, HIGH);
  digitalWrite(yellowPin, LOW);
  state = newState;
  int duration = calculateGreenDuration(*needShort, 18000);  // Start with full 18s
  *needShort = false;
  xTimerChangePeriod(mainTimer, pdMS_TO_TICKS(duration), 0);
  xTimerStart(mainTimer, 0);
}

/**
 * Helper: Get traffic light state string
 * Reads the current state of a traffic light direction
 * Parameters: greenPin, yellowPin - pin numbers to read
 * Returns: String representation of current state
 */
const char* getTrafficState(int greenPin, int yellowPin) {
  if (digitalRead(greenPin)) {
    return "Green";
  } else if (digitalRead(yellowPin)) {
    return "Yellow";
  } else {
    return "Red";
  }
}

/**
 * Helper: Send message from ISR to serial queue
 * Thread-safe way to send messages from interrupt context
 * Parameters: message - the message string to send
 * Shared variables: printQueue
 */
void sendMessageFromISR(const char* message) {
  char msg[64];
  strcpy(msg, message);
  xQueueSendFromISR(printQueue, msg, NULL);
}

/**
 * Task 1: Serial Task (Gatekeeper)
 * Waits on queue for serial data available, sends serial output
 * Implements gatekeeper pattern - all serial communication goes through this task
 * Shared variables: printQueue (reads from)
 */
void serialTask(void *pvParameters) {
  char msg[64];
  while (1) {
    if (xQueueReceive(printQueue, msg, portMAX_DELAY) == pdPASS) {
      Serial.println(msg);
    }
  }
}

/**
 * Task 2: Stoplight Task
 * Keeps track of state of stoplight LEDs based on timer callbacks & interrupts from buttons & PIR
 * Responds to timer notifications via queue to perform state transitions
 * Shared variables: state, needD1Short, needD2Short, crosswalkFlag, mainTimer
 */
void stoplightTask(void *pvParameters) {
  int timerMsg;
  
  while (1) {
    // Wait for timer notification
    if (xQueueReceive(stoplightQueue, &timerMsg, portMAX_DELAY) == pdPASS) {
      // Timer expired - perform state transition
      switch (state) {
        case STATE_BOTH_RED:
          // 2 second safety window elapsed, check for crosswalk
          if (crosswalkFlag) {
            startCrosswalk(STATE_D1_GREEN);
          } else {
            turnDirectionGreen(RED1, GREEN1, YELLOW1, STATE_D1_GREEN, &needD1Short);
          }
          break;
          
        case STATE_D1_GREEN:
          // Green time elapsed, switch to yellow
          digitalWrite(GREEN1, LOW);
          digitalWrite(YELLOW1, HIGH);
          state = STATE_D1_YELLOW;
          xTimerChangePeriod(mainTimer, pdMS_TO_TICKS(4000), 0);  // 4 second yellow
          xTimerStart(mainTimer, 0);
          break;
          
        case STATE_D1_YELLOW:
          // Yellow time elapsed, turn D1 red (both red now)
          digitalWrite(YELLOW1, LOW);
          digitalWrite(RED1, HIGH);
          state = STATE_BOTH_RED_2;
          xTimerChangePeriod(mainTimer, pdMS_TO_TICKS(2000), 0);  // 2 second safety
          xTimerStart(mainTimer, 0);
          break;
          
        case STATE_BOTH_RED_2:
          // 2 second safety window elapsed, check for crosswalk
          if (crosswalkFlag) {
            startCrosswalk(STATE_D2_GREEN);
          } else {
            turnDirectionGreen(RED2, GREEN2, YELLOW2, STATE_D2_GREEN, &needD2Short);
          }
          break;
          
        case STATE_D2_GREEN:
          // Green time elapsed, switch to yellow
          digitalWrite(GREEN2, LOW);
          digitalWrite(YELLOW2, HIGH);
          state = STATE_D2_YELLOW;
          xTimerChangePeriod(mainTimer, pdMS_TO_TICKS(4000), 0);  // 4 second yellow
          xTimerStart(mainTimer, 0);
          break;
          
        case STATE_D2_YELLOW:
          // Yellow time elapsed, turn D2 red (both red now)
          digitalWrite(YELLOW2, LOW);
          digitalWrite(RED2, HIGH);
          state = STATE_BOTH_RED_3;
          xTimerChangePeriod(mainTimer, pdMS_TO_TICKS(2000), 0);  // 2 second safety
          xTimerStart(mainTimer, 0);
          break;
          
        case STATE_BOTH_RED_3:
          // 2 second safety window elapsed, check for crosswalk
          if (crosswalkFlag) {
            startCrosswalk(STATE_D1_GREEN);
          } else {
            turnDirectionGreen(RED1, GREEN1, YELLOW1, STATE_D1_GREEN, &needD1Short);
          }
          break;
          
        case STATE_CROSSWALK:
          // Crosswalk period (22 seconds) completed, transition to next state
          xTimerStop(crosswalkTimer, 0);  // Stop countdown timer
          state = nextStateAfterCrosswalk;
          if (state == STATE_D1_GREEN) {
            turnDirectionGreen(RED1, GREEN1, YELLOW1, STATE_D1_GREEN, &needD1Short);
          } else if (state == STATE_D2_GREEN) {
            turnDirectionGreen(RED2, GREEN2, YELLOW2, STATE_D2_GREEN, &needD2Short);
          }
          break;
      }
    }
  }
}

/**
 * Task 3: Crosswalk counter/LEDs task
 * Keeps track of current crosswalk counter value & LED value
 * Handles button presses via binary semaphore and updates display via timer callback
 * Shared variables: crosswalkFlag, crossCount, CROSSWALKLED, display
 */
void crosswalkTask(void *pvParameters) {
  char msg[64];
  
  while (1) {
    // Handle button presses (request crosswalk) - binary semaphore from pushbuttons
    if (xSemaphoreTake(crosswalkSema, portMAX_DELAY) == pdTRUE) {
      if (!crosswalkFlag) {
        crosswalkFlag = true;
        // LED will be turned on by startCrosswalk() when crosswalk actually begins
        strcpy(msg, "Crosswalk requested");
        xQueueSend(printQueue, msg, 0);
      }
    }
  }
}

/**
 * ISR: PIR Up/Down motion sensor interrupt
 * Detects vehicle presence for direction 1
 * If direction 1 is currently red, set flag to reduce green time to 8s
 * Shared variables: needD1Short, state
 */
void pirUpISR() {
  // PIR detected for direction 1 (currently red)
  if (!(state == STATE_D1_GREEN || state == STATE_D1_YELLOW)) {
    needD1Short = true;
    sendMessageFromISR("PIR Up/Down detected");
  } else if (state == STATE_D1_GREEN) {
    // If already green, reduce remaining time to 8s or current remaining, whichever is lower
    // Get remaining time from timer
    TickType_t remainingTicks = xTimerGetExpiryTime(mainTimer) - xTaskGetTickCountFromISR();
    int remainingMs = (remainingTicks * portTICK_PERIOD_MS);
    if (remainingMs > 8000) {
      // Reduce to 8 seconds
      BaseType_t xHigherPriorityTaskWoken = pdFALSE;
      xTimerChangePeriodFromISR(mainTimer, pdMS_TO_TICKS(8000), &xHigherPriorityTaskWoken);
      portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
    sendMessageFromISR("PIR Up/Down detected (during green)");
  }
}

/**
 * ISR: PIR Left/Right motion sensor interrupt
 * Detects vehicle presence for direction 2
 * If direction 2 is currently red, set flag to reduce green time to 8s
 * Shared variables: needD2Short, state
 */
void pirLeftISR() {
  // PIR detected for direction 2 (currently red)
  if (!(state == STATE_D2_GREEN || state == STATE_D2_YELLOW)) {
    needD2Short = true;
    sendMessageFromISR("PIR Left/Right detected");
  } else if (state == STATE_D2_GREEN) {
    // If already green, reduce remaining time to 8s or current remaining, whichever is lower
    TickType_t remainingTicks = xTimerGetExpiryTime(mainTimer) - xTaskGetTickCountFromISR();
    int remainingMs = (remainingTicks * portTICK_PERIOD_MS);
    if (remainingMs > 8000) {
      // Reduce to 8 seconds
      BaseType_t xHigherPriorityTaskWoken = pdFALSE;
      xTimerChangePeriodFromISR(mainTimer, pdMS_TO_TICKS(8000), &xHigherPriorityTaskWoken);
      portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
    sendMessageFromISR("PIR Left/Right detected (during green)");
  }
}

/**
 * ISR: Button Up/Down pushbutton interrupt
 * Signals crosswalk request via binary semaphore
 * Shared variables: crosswalkSema
 */
void btnUpISR() {
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  xSemaphoreGiveFromISR(crosswalkSema, &xHigherPriorityTaskWoken);
  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

/**
 * ISR: Button Left/Right pushbutton interrupt
 * Signals crosswalk request via binary semaphore
 * Shared variables: crosswalkSema
 */
void btnLeftISR() {
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  xSemaphoreGiveFromISR(crosswalkSema, &xHigherPriorityTaskWoken);
  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

/**
 * Timer Callback: Main timer for stoplight state transitions
 * Notifies stoplight task when state transition time has elapsed
 * Shared variables: stoplightQueue
 */
void mainTimerCallback(TimerHandle_t xTimer) {
  int msg = 1;  // Signal that timer expired
  xQueueSendFromISR(stoplightQueue, &msg, NULL);
}

/**
 * Timer Callback: Crosswalk countdown timer (1 second periodic)
 * Decrements crosswalk counter and updates display
 * Shared variables: crossCount, display, crosswalkFlag, CROSSWALKLED
 */
void crosswalkTimerCallback(TimerHandle_t xTimer) {
  if (crossCount > 0) {
    crossCount--;
    showCountdown(crossCount);
  } else {
    // Countdown complete
    xTimerStop(crosswalkTimer, 0);
    crosswalkFlag = false;
    digitalWrite(CROSSWALKLED, LOW);
    clearDisplay();
    char msg[64];
    strcpy(msg, "Crosswalk cycle complete");
    xQueueSendFromISR(printQueue, msg, NULL);
  }
}

/**
 * Timer Callback: Status timer (1 second periodic)
 * Sends periodic status updates to serial queue
 * Shared variables: printQueue, secCount
 */
volatile int secCount = 0;
void statusTimerCallback(TimerHandle_t xTimer) {
  secCount++;
  const char *d1State, *d2State;
  char msg[64];

  d1State = getTrafficState(GREEN1, YELLOW1);
  d2State = getTrafficState(GREEN2, YELLOW2);

  const char *crosswalkStatus;
  if (digitalRead(CROSSWALKLED)) {
    crosswalkStatus = "On";
  } else {
    crosswalkStatus = "Off";
  }
  snprintf(msg, sizeof(msg), "Time:%d D1:%s D2:%s CrosswalkLED:%s", secCount, d1State, d2State, crosswalkStatus);
  xQueueSendFromISR(printQueue, msg, NULL);
}

// setup
void setup() {
  Serial.begin(9600);

  //setting pin modes like assignment 2
  pinMode(RED1, OUTPUT);
  pinMode(RED2, OUTPUT);
  pinMode(YELLOW1, OUTPUT);
  pinMode(YELLOW2, OUTPUT);
  pinMode(GREEN1, OUTPUT);
  pinMode(GREEN2, OUTPUT);
  pinMode(CROSSWALKLED, OUTPUT);

  //doing pull up on the motin sensors as well as the buttons
  pinMode(PIRUPDOWN, INPUT_PULLUP);
  pinMode(PIRLEFTRIGHT, INPUT_PULLUP);
  pinMode(BTNUPDOWN, INPUT_PULLUP);
  pinMode(BTNLEFTRIGHT, INPUT_PULLUP);

  //default to red on start up
  digitalWrite(RED1, HIGH);
  digitalWrite(RED2, HIGH);

  //display initialization.
  display.init();
  display.set(7, 1);
  clearDisplay();

  // Creating FreeRTOS synchronization objects
  crosswalkSema = xSemaphoreCreateBinary();  // Binary semaphore from pushbuttons to crosswalk task
  printQueue = xQueueCreate(10, sizeof(char[64]));  // Queue for serial communication (gatekeeper)
  stoplightQueue = xQueueCreate(5, sizeof(int));  // Queue for stoplight task timer notifications

  // Creating software timers (required - no vTaskDelay allowed)
  mainTimer = xTimerCreate("MainTimer", pdMS_TO_TICKS(2000), pdFALSE, NULL, mainTimerCallback);
  crosswalkTimer = xTimerCreate("CrosswalkTimer", pdMS_TO_TICKS(1000), pdTRUE, NULL, crosswalkTimerCallback);
  statusTimer = xTimerCreate("StatusTimer", pdMS_TO_TICKS(1000), pdTRUE, NULL, statusTimerCallback);

  // Start timers
  xTimerStart(mainTimer, 0);  // Start with 2 second safety window
  xTimerStart(statusTimer, 0);

  attachInterrupt(digitalPinToInterrupt(PIRUPDOWN), pirUpISR, RISING);
  attachInterrupt(digitalPinToInterrupt(PIRLEFTRIGHT), pirLeftISR, RISING);
  attachInterrupt(digitalPinToInterrupt(BTNUPDOWN), btnUpISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(BTNLEFTRIGHT), btnLeftISR, FALLING);

  // Create the three recommended tasks
  xTaskCreate(serialTask, "SerialTask", 2048, NULL, 1, NULL);
  xTaskCreate(stoplightTask, "StoplightTask", 4096, NULL, 2, NULL);  // Higher priority for traffic control
  xTaskCreate(crosswalkTask, "CrosswalkTask", 2048, NULL, 1, NULL);
}

// loop
void loop() {
  delay(100);
}