//#include <Wire.h>  // For I2C devices
#include "common.h"
#include "ultrasonic.h"
#include "buzzer.h"
#include "led.h"
#include "temperature.h"



msg_queue_t msg_queue;

// -------- SIMULATED INPUT GENERATOR --------
struct input_gen_t {
  unsigned long last_gen_time;
  unsigned long interval_ms;
  uint8_t type_counter;
};

input_gen_t input_gen;

void input_gen_init(unsigned long interval) {
  input_gen.last_gen_time = millis();
  input_gen.interval_ms = interval;
  input_gen.type_counter = 1;
}

void input_gen_update() {
  unsigned long now = millis();

  if (now - input_gen.last_gen_time >= input_gen.interval_ms) {
    input_gen.last_gen_time = now;

    message_t new_msg;
    new_msg.type = MSG_TYPE_UART_INPUT;
    new_msg.arrival_ms = now;
    new_msg.deadline_ms = now + random(50, 200);

    msg_queue_push(&msg_queue, new_msg);

    Serial.print("[GEN] type=");
    Serial.println(new_msg.type);
  }
}

// TEMPERATURE SENSOR READ AND OUTPUT
// unsigned long temp_request_time = 0;
// bool temp_requested = false;
// int16_t temperature_x10 = 0;

// void temperature_task() {
//     unsigned long now = millis();

//     if (!temp_requested) {
//         ds18b20_start_conversion();
//         temp_request_time = now;
//         temp_requested = true;
//     }

//     if (temp_requested && (now - temp_request_time >= 750)) {
//         temperature_x10 = ds18b20_get_temp_x10();
//         temp_requested = false;

//         uart_print_str("Temp: ");
//         uart_print_uint(temperature_x10 / 10);
//         uart_print_str(".");
//         uart_print_uint(abs(temperature_x10 % 10));
//         uart_print_str(" C\r\n");
//     }
// }


// --------------------------
// UTILITY FUNCTIONS
// --------------------------
void msg_queue_init(msg_queue_t *q) {
  q->count = 0;
}

// EDF pop: remove the message with earliest deadline
int msg_queue_pop_edf(msg_queue_t *q, message_t *msg_out) {
  if (q->count == 0) return -1; // queue empty

  // Find message with earliest deadline
  int idx = 0;
  unsigned long earliest = q->messages[0].deadline_ms;
  for (int i = 1; i < q->count; i++) {
    if (q->messages[i].deadline_ms < earliest) {
      earliest = q->messages[i].deadline_ms;
      idx = i;
    }
  }

  // Copy out
  *msg_out = q->messages[idx];

  // Shift remaining messages down
  for (int i = idx; i < q->count - 1; i++) {
    q->messages[i] = q->messages[i + 1];
  }
  q->count--;


  // Serial.println("msg popped. new q count is:");
  // Serial.println(q->count);
  return 0;
}

// Simple helper to push messages into queue
int msg_queue_push(msg_queue_t *q, message_t msg) {
  if (q->count >= QUEUE_SIZE) return -1; // queue full
  q->messages[q->count++] = msg;

  // Serial.println("** message pushed. q count is:");
  // Serial.println(q -> count);
  return 0;
}

// --------------------------
// UART / SERIAL HELPERS
// --------------------------
void uart_print_uint(unsigned long value) {
  Serial.print(value);
}

void uart_print_str(const char* s) {
  Serial.print(s);
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  while(!Serial); // Wait for Serial to be ready
  Serial.println("EDF Router Booting...");


  // Initialize I2C (Wire) for future devices
  //Wire.begin(); // uses default pins PB6=SCL, PB7=SDA
  // You can add Wire.beginTransmission() calls later

  // Initialize message queue
  msg_queue_init(&msg_queue);

  ultrasonic_init(2, 3);   // TRIG=2  ECHO=3
  buzzer_init(8);   // choose any free digital pin
  led_init(9,10,11);
  //temperature_init(4);


  //input_gen_init(100);   // generate message every 100ms


  // Example test messages
  // message_t test1 = {MSG_TYPE_UART_INPUT, millis(), millis() + 100};
  // message_t test2 = {MSG_TYPE_UART_INPUT, millis(), millis() + 50};
  // msg_queue_push(&msg_queue, test1);
  // msg_queue_push(&msg_queue, test2);
}

void loop() {

  // input_gen_update();


  message_t msg;

  message_t sensorMsg;

  if (ultrasonic_poll(&sensorMsg)) {
    msg_queue_push(&msg_queue, sensorMsg);
  }

  // message_t temperatureMsg;
  // if (temperature_poll(&temperatureMsg)) {
  //   msg_queue_push(&msg_queue, temperatureMsg);
  // }

  //temperature_task();

  // Pop next EDF message
  if (msg_queue_pop_edf(&msg_queue, &msg) == 0) {
    unsigned long now = millis();

    // if (msg.type == MSG_TYPE_SENSOR_TEMPERATURE) {
    //   uart_print_str(" temp=");
    //   uart_print_uint(msg.payload.temperature_value);
    // }

    if (now > msg.deadline_ms) {
      unsigned long lateness = now - msg.deadline_ms;
      uart_print_str("[MISS] type=");
      uart_print_uint(msg.type);
      uart_print_str(" arrival=");
      uart_print_uint(msg.arrival_ms);
      uart_print_str(" deadline=");
      uart_print_uint(msg.deadline_ms);
      uart_print_str(" lateness=");
      uart_print_uint(lateness);
      uart_print_str("ms\r\n");

      led_set_miss();

      buzzer_handle_message(&msg);

    } else {
      unsigned long latency = now - msg.arrival_ms;
      uart_print_str("[OK] type=");
      uart_print_uint(msg.type);
      uart_print_str(" latency=");
      uart_print_uint(latency);
      uart_print_str(" arrival=");
      uart_print_uint(msg.arrival_ms);
      uart_print_str("ms deadline=");
      uart_print_uint(msg.deadline_ms - msg.arrival_ms);
      uart_print_str("ms\r\n");

      led_set_ok();

      buzzer_handle_message(&msg);

    }
  }

  led_update();

  delay(10); // this speeds up the simulation
}