//#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 "periph_init.h"
#include "data_report.h"
#include "modbus_fn.h"
#include "settings.h"
#include <driver/uart.h>
#include <string.h>
#include <stddef.h>
#include <inttypes.h>

static const char *TAG = "co2";

#define CO2_ADDR 104
#define CO2_UART_NUM 1
#define TIMEOUT_MS 500

static int mb_write_and_read(uint8_t *buffer, int num, size_t resplen) {
    if (num < 0) {
        ESP_LOGE(TAG, "MB build failed");
        return 0;
    }

    ESP_LOGD(TAG, "Sending");
    ESP_LOG_BUFFER_HEXDUMP(TAG, buffer, num, ESP_LOG_DEBUG);
    uart_write_bytes(CO2_UART_NUM, buffer, num);

    ESP_LOGD(TAG, "Expect resp of %d bytes", resplen);
    num = uart_read_bytes(CO2_UART_NUM, buffer, resplen, pdMS_TO_TICKS(500));
    ESP_LOGD(TAG, "Received %d bytes", num);
    ESP_LOG_BUFFER_HEXDUMP(TAG, buffer, num, ESP_LOG_DEBUG);
    return num;
}

static void write_saved_calib_to_sensor() {
    static uint8_t buffer[40];
    size_t resplen;
    int num;
    int resp;

    if (g_Settings.co2_calib[0] > 0) {
        ESP_LOGI(TAG, "Writing saved calib to sensor");

        /* Write calib */
        resplen = 0;
        num = mb_build_fc16(buffer, 128, &resplen, 104, 4, g_Settings.co2_calib, 5);
        num = mb_write_and_read(buffer, num, resplen);
        resp = mb_parse_fc16(buffer, num);
        if (resp < 0) {
            ESP_LOGE(TAG, "Fail to write calib constants!");
        }
    } else {
        ESP_LOGW(TAG, "No saved calib to write to sensor!");
    }
}

void co2_restart(bool restore_calib) {
    ESP_LOGI(TAG, "Restarting CO2 sensor");

    gpio_set_level(CONFIG_PIN_CO2_EN, 0);
    vTaskDelay(pdMS_TO_TICKS(100));
    gpio_set_level(CONFIG_PIN_CO2_EN, 1);
    vTaskDelay(pdMS_TO_TICKS(50));

    if (restore_calib) {
        write_saved_calib_to_sensor();
    }
}

static bool ppm_looks_valid(uint16_t ppm) {
    return ppm > 400 && ppm < 5000;
}

void co2_read_task(void *param) {
    (void) param;

    vTaskDelay(pdMS_TO_TICKS(500));

    // TODO

    static uint8_t buffer[128];
    static uint16_t values[32];
    int qty;
    size_t resplen;
    int num;

    write_saved_calib_to_sensor();

    const uint32_t read_cycle_time_ticks = 10 * 1000;
    const uint32_t calib_persist_time_ticks = 12 * 3600 * 1000;
    _Static_assert(configTICK_RATE_HZ == 1000, "1kHz tick");

    uint32_t last_calib_persist = xTaskGetTickCount();

    ESP_LOGD(TAG, "Calib persist time = %d ticks", calib_persist_time_ticks);

    int num_fails = 0;
    uint16_t ppm = 0;
    while (1) {
        ppm = 0;
        vTaskDelay(read_cycle_time_ticks);

        if (num_fails > 10) {
            ESP_LOGW(TAG, "Too many CO2 fails, restarting sensor");
            co2_restart(true);
            num_fails = 0;
        }

        resplen = 0;
        num = mb_build_fc4(buffer, 128, &resplen, 104, 0, 5);
        num = mb_write_and_read(buffer, num, resplen);
        qty = mb_parse_fc4(buffer, num, values, 32);
        if (qty < 0) {
            num_fails++;
            goto next;
        }

        // Read was OK, reset the fail counter
        num_fails = 0;

        uint16_t status = values[0];
        bool co2_needs_restart = false;
        bool calibration_error = false;
        if (status != 0) {
            if (status & 0x01) {
                ESP_LOGE(TAG, "CO2 fatal error");
                co2_needs_restart = true;
            }
            if (status & 0x02) {
                ESP_LOGE(TAG, "CO2 I2C communication error");
            }
            if (status & 0x04) {
                ESP_LOGE(TAG, "CO2 internal I2C operation error");
            }
            if (status & 0x08) {
                ESP_LOGE(TAG, "CO2 calibration error");
                calibration_error = true;
            }
            if (status & 0x10) {
                ESP_LOGE(TAG, "CO2 self-diagnostics error");
                co2_needs_restart = true;
            }
            if (status & 0x20) {
                ESP_LOGE(TAG, "CO2 out of range error");
                // nonsense, this means calibration failed
                calibration_error = true;
            }
            if (status & 0x40) {
                ESP_LOGE(TAG, "CO2 memory error");
                co2_needs_restart = true;
            }
            if (status & 0x80) {
                ESP_LOGE(TAG, "CO2 external I2C error");
            }

            if (co2_needs_restart) {
                co2_restart(!calibration_error);
                if (calibration_error) {
                    // Prevent persisting values too soon, give it some time to calibrate
                    last_calib_persist = xTaskGetTickCount();
                }
            }
            goto next;
        }

        ppm = values[3];
        ESP_LOGI(TAG, "CO2 ppm %d", ppm);

        if (!ppm_looks_valid(ppm)) {
            ESP_LOGW(TAG, "CO2 measures nonsense!");
            co2_restart(true);
        } else {
            // CO2 measurement looks OK

            uint32_t tickNow = xTaskGetTickCount();
            uint32_t elapsed = tickNow - last_calib_persist;
            if (elapsed > calib_persist_time_ticks) {
                ESP_LOGI(TAG, "Read & persist CO2 calibration");

                last_calib_persist = tickNow;

                resplen = 0;
                num = mb_build_fc3(buffer, 128, &resplen, 104, 4, 5);
                num = mb_write_and_read(buffer, num, resplen);
                qty = mb_parse_fc3(buffer, num, values, 32);
                if (qty < 0) {
                    num_fails++;
                }

                if (qty == 5) {
                    memcpy(g_Settings.co2_calib, values, 5 * 2);
                    settings_persist(SETTINGS_co2_calib);
                }
            }
        }

        next:
        if (pdPASS == xSemaphoreTake(g_mux_data_report, pdMS_TO_TICKS(750))) {
            if (ppm_looks_valid(ppm)) {
                g_data_report.co2_ppm = (float) ppm;
                g_data_report.co2_ready = true;
                g_data_report.co2_timestamp = xTaskGetTickCount();
            } else {
                g_data_report.co2_ready = false;
            }
        }
        xSemaphoreGive(g_mux_data_report);
    }
}