#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "mqtt.h"
#include "pico/cyw43_arch.h"
#include "pico/stdlib.h"
#include <boards/pico_w.h>
#include <hardware/structs/io_bank0.h>
#include <pico/binary_info/code.h>
#include <pico/double.h>
#include <pico/error.h>
#include <stdio.h>

// I2C device defines
#define AHT_ADDR 0x38
#define ENS_ADDR 0x53
uint8_t AHT_MEASURE[3] = {0xAC, 0x33, 0x00};
uint8_t ENS_ACTIVATE[2] = {0x10, 0x02};
uint8_t ENS_IDLE[2] = {0x10, 0x01};
#define ENS_TEMP_IN 0x13
#define ENS_HUM_IN 0x15
#define ENS_STATUS 0x20
#define ENS_STATUS_MASK 0x0C
#define ENS_AQI 0x21
#define ENS_TVOC 0x22
#define ENS_ECO2 0x24
#define ENS_ETOH 0x26

#define AVERAGES 16

// MQTT and WiFi defines
#define ADAFRUIT_IO_HOST "io.adafruit.com"
#define ADAFRUIT_IO_USERNAME "ADAFRUIT_IO_USERNAME"
#define ADAFRUIT_IO_PASSWORD "ADAFRUIT_IO_KEY"
#define WIFI_SSID "Meosjin"
#define WIFI_PASSWORD "WIFI_PASSWORD"
#define MQTT_KEEP_ALIVE_SECONDS 60

// MQTT Topics
char temp_topic[] = "ADAFRUIT_IO_USERNAME/feeds/ensaht.temperature";
char humi_topic[] = "ADAFRUIT_IO_USERNAME/feeds/ensaht.humidity";
char aqi_topic[] = "ADAFRUIT_IO_USERNAME/feeds/ensaht.aqi";
char tvoc_topic[] = "ADAFRUIT_IO_USERNAME/feeds/ensaht.tvoc";
char etoh_topic[] = "ADAFRUIT_IO_USERNAME/feeds/ensaht.etoh";
char eco2_topic[] = "ADAFRUIT_IO_USERNAME/feeds/ensaht.eco2";

static void mqtt_connection_cb(mqtt_client_t *client, void *arg,
                               mqtt_connection_status_t status) {
  LWIP_UNUSED_ARG(client);
  LWIP_UNUSED_ARG(arg);

  printf("MQTT status: %d\n", status);
}

static double average_double(const double *samples, uint8_t count) {
  double sum = 0.0;
  for (uint8_t i = 0; i < count; i++) {
    sum += samples[i];
  }
  return sum / count;
}

static uint16_t average_uint16(const uint16_t *samples, uint8_t count) {
  uint32_t sum = 0;
  for (uint8_t i = 0; i < count; i++) {
    sum += samples[i];
  }
  return (uint16_t)((sum + count / 2) / count);
}

void getAHTData(double *t, double *h) {
  uint8_t result[6];
  uint32_t temperature, humidity;
  i2c_write_blocking(i2c_default, AHT_ADDR, AHT_MEASURE, 3,
                     false); // trigger measurement
  // now we must delay ~100ms
  sleep_ms(100);
  i2c_read_blocking(i2c_default, AHT_ADDR, result, 6,
                    false); // read 6 bytes back
  humidity = (result[1] << 12) | (result[2] << 4) | (result[3] & 0xF0) >> 4;
  temperature = (result[3] & 0x0F) << 16 | (result[4] << 8) | result[5];
  *t = ((double)temperature / 1048576.0f) * 200.0f - 50.0f;
  *h = ((double)humidity / 1048576.0f) * 100.0f;
  return;
}

void activateENS(void) {
  i2c_write_blocking(i2c_default, ENS_ADDR, ENS_ACTIVATE, 2, false);
  return;
}
void deactivateENS(void) {
  i2c_write_blocking(i2c_default, ENS_ADDR, ENS_IDLE, 2, false);
  return;
}
uint8_t getENSData(double *t, double *h, uint8_t *AQI, uint16_t *TVOC,
                   uint16_t *ECO2, uint16_t *ETOH) {
  uint16_t K = (uint16_t)(*t + 273.15) * 64; // convert to ENS format
  uint16_t RH = (uint16_t)*h * 512;
  uint8_t inputData[5] = {0x13, (uint8_t)(K & 0x00FF), (uint8_t)(K >> 8),
                          (uint8_t)(RH & 0x00FF), (uint8_t)(RH >> 8)};
  uint8_t result[7], dataReg = ENS_AQI, valid;
  i2c_write_blocking(i2c_default, ENS_ADDR, inputData, 5, false);
  sleep_ms(100);
  i2c_write_blocking(i2c_default, ENS_ADDR, &dataReg, 1, true);
  i2c_read_blocking(i2c_default, ENS_ADDR, result, 7, false);
  *AQI = result[0];
  *TVOC = (result[2] << 8) | result[1];
  *ECO2 = (result[4] << 8) | result[3];
  *ETOH = (result[6] << 8) | result[5];
  dataReg = 0x20; // status register
  i2c_write_blocking(i2c_default, ENS_ADDR, &dataReg, 1, true);
  i2c_read_blocking(i2c_default, ENS_ADDR, result, 1, false);
  valid = (result[0] & ENS_STATUS_MASK) == 0
              ? 1
              : 0; // check if we are in warm-up or normal mode
  return valid;
}
int main() {
  uint16_t tvoc, eco2, etoh, avg_tvoc, avg_eco2, avg_etoh;
  uint8_t aqi, validData;
  double t, h, avg_t, avg_h;
  uint16_t tvoc_avg[AVERAGES], eco2_avg[AVERAGES], etoh_avg[AVERAGES];
  double t_avg[AVERAGES], h_avg[AVERAGES];
  char payload[32];
  // MQTT setup
  ensaht_mqtt_t mqtt;
  struct mqtt_connect_client_info_t mqtt_client_info = {
      .client_id = ADAFRUIT_IO_USERNAME,
      .client_user = ADAFRUIT_IO_USERNAME,
      .client_pass = ADAFRUIT_IO_PASSWORD,
      .keep_alive = MQTT_KEEP_ALIVE_SECONDS,
  };
  // setup stdio
  stdio_init_all();
  // wifi chip initialization
  if (cyw43_arch_init_with_country(CYW43_COUNTRY_CANADA)) {
    printf("failed to initialise\n");
    return 1;
  }
  cyw43_arch_enable_sta_mode();
  int wifierr = 1;

  wifierr = cyw43_arch_wifi_connect_timeout_ms(WIFI_SSID, WIFI_PASSWORD,
                                               CYW43_AUTH_WPA2_AES_PSK, 30000);
  if (wifierr) {
    printf("failed to connect to Wi-Fi, reason %d\n", wifierr);
    switch (wifierr) {
    case PICO_ERROR_BADAUTH:
      printf("Password incorrect.\n");
      break;
    case PICO_ERROR_TIMEOUT:
      printf("Timeout ran out before connection.\n");
      break;
    case PICO_ERROR_CONNECT_FAILED:
      printf("A damn generic error code.\n");
      break;
    default:
      printf("Not a known error code.\n");
      break;
    }
  }
  ensaht_mqtt_init(&mqtt);
  err_t mqtt_err =
      ensaht_mqtt_connect(&mqtt, ADAFRUIT_IO_HOST, MQTT_PORT, &mqtt_client_info,
                          mqtt_connection_cb, NULL);
  if (mqtt_err != ERR_OK) {
    printf("MQTT connect start failed: %d\n", mqtt_err);
  }
  i2c_init(i2c_default, 100 * 1000); // start i2c at 100khz
  gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
  gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
  gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
  gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);

  bi_decl(bi_2pins_with_func(PICO_DEFAULT_I2C_SDA_PIN, PICO_DEFAULT_I2C_SCL_PIN,
                             GPIO_FUNC_I2C));
  printf("\x1b[2J"); // clear screen
  uint8_t i = 0;
  uint8_t sample_count = 0;
  activateENS();
  while (1) {
    // cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
    getAHTData(&t, &h);
    t_avg[i] = t;
    h_avg[i] = h;
    if (sample_count < AVERAGES) {
      sample_count++;
    }
    avg_t = average_double(t_avg, sample_count);
    avg_h = average_double(h_avg, sample_count);
    printf("\x1b[J\x1b[HTemperature %0.2f Humidity %0.2f\n", avg_t, avg_h);
    validData = getENSData(&avg_t, &avg_h, &aqi, &tvoc, &eco2, &etoh);
    tvoc_avg[i] = tvoc;
    eco2_avg[i] = eco2;
    etoh_avg[i] = etoh;
    avg_tvoc = average_uint16(tvoc_avg, sample_count);
    avg_eco2 = average_uint16(eco2_avg, sample_count);
    avg_etoh = average_uint16(etoh_avg, sample_count);
    if (!validData) {
      printf("!! ");
    }
    printf("AQI: %d TVOC: %d eCO2: %d EtOH: %d\n", aqi, avg_tvoc, avg_eco2,
           avg_etoh);
    // cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 0);
    i++;
    if (i == AVERAGES)
      i = 0;
    snprintf(payload, sizeof(payload), "%.2f", avg_t);
    ensaht_mqtt_publish_client(&mqtt, temp_topic, payload, strlen(payload),
                               NULL, NULL);
    snprintf(payload, sizeof(payload), "%.2f", avg_h);
    ensaht_mqtt_publish_client(&mqtt, humi_topic, payload, strlen(payload),
                               NULL, NULL);
    snprintf(payload, sizeof(payload), "%d", avg_tvoc);
    ensaht_mqtt_publish_client(&mqtt, tvoc_topic, payload, strlen(payload),
                               NULL, NULL);
    snprintf(payload, sizeof(payload), "%d", avg_eco2);
    ensaht_mqtt_publish_client(&mqtt, eco2_topic, payload, strlen(payload),
                               NULL, NULL);
    snprintf(payload, sizeof(payload), "%d", avg_etoh);
    ensaht_mqtt_publish_client(&mqtt, etoh_topic, payload, strlen(payload),
                               NULL, NULL);
    snprintf(payload, sizeof(payload), "%d", aqi);
    ensaht_mqtt_publish_client(&mqtt, aqi_topic, payload, strlen(payload), NULL,
                               NULL);
    sleep_ms(30000);
  }

  return 0;
}