#include "bme280.h"
#include "pico/cyw43_arch.h"
#include "pico/stdlib.h"
#include "pms5003.h"
#include "tcp_client.h"
#include <hardware/watchdog.h>
#include <pico/time.h>
#include <pico/types.h>
#include <stdio.h>

#define SENSOR_READING_INTERVAL_US (1000000 * 15) // 15 seconds
#define WIFI_CHECK_INTERVAL_US (1000000 * 10)     // 10 seconds
#define WIFI_STATUS_LED_PIN 16

static tcp_client_config tcp_config;
static bme280_config bem_config;
static bme280_reading current_bem280_reading;
static pms5003_config pms_config;
static pms5003_reading current_pms5003_reading;
char msg_to_send[256];

static int8_t readings_index = 0;

static void send_msg(char *msg) {
  bool success = tcp_client_send_message(&tcp_config, msg);
  if (success) {
    printf("✓ Data sent successfully\n");
  } else {
    printf("✗ Failed to send data\n");
  }
}

int main() {
  stdio_init_all();
  watchdog_enable(120000, 1); // 120 second watchdog

  printf("\n=== NODE 001 ===\n");
  printf("Initializing board...\n");
  sleep_ms(2000);

  printf("Initializing WIFI status LED...\n");
  gpio_init(WIFI_STATUS_LED_PIN);
  gpio_set_dir(WIFI_STATUS_LED_PIN, GPIO_OUT);
  gpio_put(WIFI_STATUS_LED_PIN, false);

  printf("Initializing BME280...\n");
  bme280_init(&bem_config, i2c1, 14, 15);
  sleep_ms(1000);

  printf("Initializing PMS5003...\n");
  pms5003_init(&pms_config, uart1, 20, 21, 18, 19);
  sleep_ms(1000);

  printf("Initializing CYW43...\n");
  if (cyw43_arch_init()) {
    printf("FATAL: Failed to initialize CYW43\n");
    while (1) {
      watchdog_update();
      sleep_ms(1000);
    }
  }

  printf("Enabling WiFi station mode...\n");
  cyw43_arch_enable_sta_mode();
  cyw43_wifi_pm(&cyw43_state, CYW43_NONE_PM);
  sleep_ms(1000);

  printf("Connecting to WiFi '%s'...\n", WIFI_SSID);
  watchdog_update();

  int result = cyw43_arch_wifi_connect_timeout_ms(
      WIFI_SSID, WIFI_PASSWORD, CYW43_AUTH_WPA2_AES_PSK, 60000);

  watchdog_update();

  if (result != 0) {
    printf("FATAL: WiFi connection failed with result=%d\n", result);
    while (1) {
      watchdog_update();
      sleep_ms(1000);
    }
  }

  printf("WiFi connected!\n");

  if (!tcp_client_init(&tcp_config, BACKEND_SERVER_IP, BACKEND_SERVER_PORT,
                       20000)) {
    panic("TCP client initialization failed!");
  }
  watchdog_update();

  // Poll every 5 seconds
  absolute_time_t last_check = get_absolute_time();
  absolute_time_t last_sensor_reading = get_absolute_time();

  while (1) {
    watchdog_update();
    cyw43_arch_poll();

    absolute_time_t now = get_absolute_time();

    if (absolute_time_diff_us(last_sensor_reading, now) >=
        SENSOR_READING_INTERVAL_US) {
      printf("Sensor reading: %d\n", readings_index);
      last_sensor_reading = now;
      printf("Making BME280 Reading\n");
      current_bem280_reading = bme280_read(&bem_config);
      snprintf(msg_to_send, sizeof(msg_to_send), "M001,%.2f,%.2f,%2f\n",
               current_bem280_reading.temperature,
               current_bem280_reading.pressure,
               current_bem280_reading.humidity);
      printf(
          "Sending temperature, pressure, and humidity to backend server...\n");
      send_msg(msg_to_send);
      watchdog_update();
      if (readings_index == 2) {
        printf("Warming up PMS5003\n");
        pms5003_warmup(&pms_config);
      }

      if (readings_index == 4) {
        printf("Starting reads on PMSS5003\n");
        pms5003_start_reading(&pms_config);
      }

      if (readings_index == 6) {
        printf("Finished reading PMSS5003\n");
        current_pms5003_reading = pms5003_finish_reading(&pms_config);
        snprintf(msg_to_send, sizeof(msg_to_send), "M02,%.2f,%.2f,%2f\n",
                 current_pms5003_reading.pm1, current_pms5003_reading.pm2_5,
                 current_pms5003_reading.pm10);
        printf(
            "Sending particle matter readings to backend server...\n");
        send_msg(msg_to_send);
      }

      readings_index++;
      if (readings_index >= 10) {
        readings_index = 0;
      }
    }

    if (absolute_time_diff_us(last_check, now) >= WIFI_CHECK_INTERVAL_US) {
      last_check = now;

      // Check connection status
      int link_status = cyw43_tcpip_link_status(&cyw43_state, CYW43_ITF_STA);

      // printf("[WiFi Status Check]\n");
      // printf("  Link Status: ");
      switch (link_status) {
      case CYW43_LINK_DOWN:
        // printf("DOWN\n");
        gpio_put(WIFI_STATUS_LED_PIN, false);
        break;
      case CYW43_LINK_JOIN:
        // printf("JOINING\n");
        gpio_put(WIFI_STATUS_LED_PIN, false);
        break;
      case CYW43_LINK_NOIP:
        // printf("NO IP\n");
        gpio_put(WIFI_STATUS_LED_PIN, false);
        break;
      case CYW43_LINK_UP:
        // printf("UP\n");
        gpio_put(WIFI_STATUS_LED_PIN, true);
        break;
      default:
        // printf("UNKNOWN (%d)\n", link_status);
        gpio_put(WIFI_STATUS_LED_PIN, false);
        break;
      }
    }

    sleep_ms(100);
  }

  return 0;
}