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

#define SENSOR_READING_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;
char msg_to_send[256];

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");
  }
}

void comms_led_init() {
}

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 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) {
      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);
    }

    if (absolute_time_diff_us(last_check, now) >= 5000000) {
      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;
}