#include "hardware/gpio.h"
#include "hardware/i2c.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 <stdio.h>

#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

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);
  activateENS();
  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);
  printf("Status register is 0x%0X\n", result[0]);
  valid = (result[0] & ENS_STATUS_MASK) == 0
              ? 1
              : 0; // check if we are in warm-up or normal mode
  // deactivateENS();
  return valid;
}

int main() {
  uint16_t tvoc, eco2, etoh;
  uint8_t aqi, validData;
  double t, h;
  stdio_init_all();
  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));

  while (1) {
    getAHTData(&t, &h);
    printf("Temperature %f Humidity %f\n", t, h);
    validData = getENSData(&t, &h, &aqi, &tvoc, &eco2, &etoh);
    if (!validData) {
      printf("ENS Data not yet valid\n");
    }
    printf("AQI: %d TVOC: %d eCO2: %d EtOH: %d\n", aqi, tvoc, eco2, etoh);
    sleep_ms(2000);
  }

  return 0;
}