#define LOG_LOCAL_LEVEL ESP_LOG_DEBUG

#include "co2_sensor.h"

#include <esp_log.h>
#include <esp_err.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <driver/i2c.h>
#include <sys/time.h>
#include <hal/i2c_hal.h>
#include "voc_sensor.h"
#include "i2c_utils.h"
#include "periph_init.h"
#include "data_report.h"

static const char *TAG = "co2";

#define CO2_ADDR 104
#define CO2_I2C_NUM I2C_NUM_1
#define TIMEOUT_MS 500

// this works, but the bus is fucked up
bool wake_up() {
    esp_err_t suc;
#define TRY(x) suc=x; if(suc!=ESP_OK) return suc;
    for (int i = 0; i < 5; i++) {
        ESP_LOGD(TAG, "Wake up try %d", i);
        uint8_t dummy;

        i2c_cmd_handle_t chain;

        chain = i2c_cmd_link_create();
        TRY(i2c_master_start(chain));
        TRY(i2c_master_write_byte(chain, (CO2_ADDR << 1) | I2C_MASTER_WRITE, false));
        TRY(i2c_master_write_byte(chain, 0x30, false));
        TRY(i2c_master_start(chain));
        TRY(i2c_master_write_byte(chain, (CO2_ADDR << 1) | I2C_MASTER_READ, false)); // TODO expect ack?
        TRY(i2c_master_read(chain, &dummy, 1, I2C_MASTER_LAST_NACK));
        TRY(i2c_master_stop(chain));
        i2c_master_cmd_begin(CO2_I2C_NUM, chain, pdMS_TO_TICKS(70));
        i2c_cmd_link_delete(chain);

        chain = i2c_cmd_link_create();
        TRY(i2c_master_start(chain));
        TRY(i2c_master_write_byte(chain, (CO2_ADDR << 1) | I2C_MASTER_WRITE, false));
        TRY(i2c_master_write_byte(chain, 0x30, false));
        TRY(i2c_master_start(chain));
        TRY(i2c_master_write_byte(chain, (CO2_ADDR << 1) | I2C_MASTER_READ, true)); // TODO expect ack?
        TRY(i2c_master_read(chain, &dummy, 1, I2C_MASTER_LAST_NACK));
        TRY(i2c_master_stop(chain));
        suc = i2c_master_cmd_begin(CO2_I2C_NUM, chain, pdMS_TO_TICKS(80));
        i2c_cmd_link_delete(chain);

        if (suc == ESP_OK) {
            ESP_LOGD(TAG, "Woken up at try %d", i);
            return true;
        }
    }
    ESP_LOGE(TAG, "Fail to wake up");
    return false;
#undef TRY
}


static esp_err_t do_reg_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, uint32_t timeout_ms) {
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_reg_read(cmd, CO2_ADDR, reg_addr, reg_data, length);
    esp_err_t ret = i2c_master_cmd_begin(CO2_I2C_NUM, cmd, pdMS_TO_TICKS(TIMEOUT_MS));
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C read error: %s, devaddr %x, reg %d, len %d", esp_err_to_name(ret), CO2_ADDR, reg_addr, length);
    }
    ESP_LOG_BUFFER_HEXDUMP(TAG, reg_data, length, ESP_LOG_DEBUG);
    return ret;
}

static esp_err_t reg_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, uint32_t timeout_ms) {
//    BaseType_t suc = xSemaphoreTake(g_mux_i2c, pdMS_TO_TICKS(SEMA_TIMEOUT_MS));
//    if (suc != pdPASS) {
//        ESP_LOGE(TAG, "Sema fail");
//        return ESP_ERR_TIMEOUT;
//    }
    if (!wake_up()) {
//        xSemaphoreGive(g_mux_i2c);
        return ESP_ERR_TIMEOUT;
    }
    esp_err_t rv = do_reg_read(reg_addr, reg_data, length, timeout_ms);
//    xSemaphoreGive(g_mux_i2c);
//    ets_delay_us(12000);
    return rv;
}

static esp_err_t do_reg_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, uint32_t timeout_ms) {
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_reg_write(cmd, CO2_ADDR, reg_addr, reg_data, length);
    esp_err_t ret = i2c_master_cmd_begin(CO2_I2C_NUM, cmd, pdMS_TO_TICKS(timeout_ms));
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C write error: %s, devaddr %x, reg %d, len %d", esp_err_to_name(ret), CO2_ADDR, reg_addr, length);
    }
    return ret;
}

static esp_err_t reg_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, uint32_t timeout_ms) {
//    BaseType_t suc = xSemaphoreTake(g_mux_i2c, pdMS_TO_TICKS(SEMA_TIMEOUT_MS));
//    if (suc != pdPASS) {
//        ESP_LOGE(TAG, "Sema fail");
//        return ESP_ERR_TIMEOUT;
//    }
    if (!wake_up()) {
//        xSemaphoreGive(g_mux_i2c);
        return ESP_ERR_TIMEOUT;
    }
    esp_err_t rv = do_reg_write(reg_addr, reg_data, length, timeout_ms);
//    xSemaphoreGive(g_mux_i2c);
//    ets_delay_us(12000);
    return rv;
}

void co2_read_task(void *param) {
    // i2c is protected by a semaphore inside the driver, no need to worry about it here

    // continuous is the default

    vTaskDelay(pdMS_TO_TICKS(500));

    uint8_t pld[] = {0x00, 0x05, 0x00, 0x05};
    if (ESP_OK == reg_write(0x96, pld, 4, TIMEOUT_MS)) {
        ESP_LOGI(TAG, "CO2 init OK");
    }

    bool init_suc = false;
    vTaskDelay(pdMS_TO_TICKS(250));
    while (1) {

        vTaskDelay(pdMS_TO_TICKS(2000));

        uint8_t data[4] = {};

        for (int retry = 0; retry < 3; retry++) {
            if (ESP_OK == reg_read(0x06, data, 4, TIMEOUT_MS)) {
                uint16_t filtered = (data[0] << 8) | data[1];
                uint16_t unfiltered = (data[2] << 8) | data[3];

                ESP_LOGI(TAG, "CO2 ppm %d, raw %d", filtered, unfiltered);

                if (pdPASS == xSemaphoreTake(g_mux_data_report, pdMS_TO_TICKS(750))) {
                    if (filtered > 400 && filtered < 5000) {
                        g_data_report.co2_ppm = (float) filtered;
                        g_data_report.co2_ready = true;
                        g_data_report.co2_timestamp = xTaskGetTickCount();
                    } else {
                        g_data_report.co2_ready = false;
                    }
                }
                xSemaphoreGive(g_mux_data_report);
                break;
            }

            vTaskDelay(pdMS_TO_TICKS(50));
        }
    }
}