/**
 * TODO file description
 */

#include <stdio.h>
#include <FreeRTOS.h>
#include <task.h>
#include <math.h>
#include <driver/hw_timer.h>
#include <esp_log.h>
#include <inttypes.h>
#include "meteo_task.h"
#include "ds18b20.h"
#include "dht.h"
#include "driver/gpio.h"
#include "driver/i2c.h"
#include "circbuf.h"
#include "bmp280.h"
#include "mqttpub.h"
#include "esp_wifi.h"

static volatile uint32_t timestamp = 0;
static volatile uint32_t last_telemetry_ts = 0;

#define RPS_BUFFER_LEN (60*10)
static volatile uint16_t history[RPS_BUFFER_LEN] = {};
static CircBuf rps_cb;

static volatile float rpm_average = 0;
static volatile float rpm_inst = 0;
static volatile float rpm_gust = 0;

#define MAGNET_COUNT 3

static volatile uint16_t cycle_count = 0;

#define TAG "meteo"

void calculate_wind();

static void gpio_isr_handler(void *arg)
{
    if (cycle_count < 0xFFFF) {
        cycle_count++;
    }
}

void hw_timer_callback1s(void *arg)
{
    timestamp++;

    // FIXME use a freertos queue and pass this to a thread!
    if (cbuf_full(&rps_cb)) {
        cbuf_pop_back(&rps_cb, NULL);
    }
    cbuf_push(&rps_cb, (void *) &cycle_count);
    cycle_count = 0;

    calculate_wind();
}

void calculate_wind()
{
    // Wind speed is average from 10 minutes
    // Gust is max 3-second average anywhere within the 10 minutes

    float max_gust = 0;

    uint32_t tenmin_sum = 0;
    int numsecs = (int) cbuf_count(&rps_cb);
    uint16_t threesec1 = 0, threesec2 = 0;
    uint32_t tensec_sum = 0, tensec_cnt = 0;
    for (int i = 0; i < numsecs; i++) {
        uint16_t *slot = cbuf_ptr_nth(&rps_cb, i);
        if (!slot) {
            continue;
        }

        uint16_t slotval = *slot;
        tenmin_sum += (uint32_t) slotval;

        if (i >= numsecs - 10) {
            tensec_sum += slotval;
            tensec_cnt++;
        }

        // gust is max avg from 3 seconds within the 10 minutes
        uint32_t gust_sum = (uint32_t) threesec1 + (uint32_t) threesec2 + (uint32_t) slotval;
        threesec1 = threesec2;
        threesec2 = slotval;

        float gust_avg = (float) gust_sum * (float) 20.0f;
        if (gust_avg > max_gust) {
            max_gust = gust_avg;
        }
    }
    rpm_gust = max_gust;
    rpm_average = ((float) tenmin_sum / (float) numsecs) * 60.0f;
    rpm_inst = ((float) tensec_sum / (float) tensec_cnt) * 60.0f;
}

bool is_nan(float f) {
    return f != f;
}

float nan_fallback(float f, float fallback) {
    if (is_nan(f)) {
        return fallback;
    }
    return f;
}

void meteo_task(void *pvParameters)
{
    static char pldbuf[512];

    cbuf_init(&rps_cb, (void *) history, RPS_BUFFER_LEN, 2); // uint16 fields

    // Try to unfuck GPIOs
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDI_U, FUNC_GPIO12);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTCK_U, FUNC_GPIO13);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTMS_U, FUNC_GPIO14);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDO_U, FUNC_GPIO15);

    // start timer used for timebase
    ESP_ERROR_CHECK(hw_timer_init(hw_timer_callback1s, NULL));
    ESP_ERROR_CHECK(hw_timer_alarm_us(1000000, true)); // 1s timer

    gpio_config_t io_conf;
    io_conf.intr_type = GPIO_INTR_POSEDGE;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = 1 << 14;
    io_conf.pull_down_en = 1;
    io_conf.pull_up_en = 0;
    ESP_ERROR_CHECK(gpio_config(&io_conf));
    ESP_ERROR_CHECK(gpio_install_isr_service(0));
    ESP_ERROR_CHECK(gpio_isr_handler_add(14, gpio_isr_handler, NULL));

    /* I2C driver & peri setup */
    int i2c_master_port = I2C_NUM_0;
    i2c_config_t conf;
    conf.mode = I2C_MODE_MASTER;
    conf.sda_io_num = 4;
    conf.sda_pullup_en = 1;
    conf.scl_io_num = 5;
    conf.scl_pullup_en = 1;
    conf.clk_stretch_tick = 300; // 300 ticks, Clock stretch is about 210us, you can make changes according to the actual situation.
    ESP_ERROR_CHECK(i2c_driver_install(i2c_master_port, conf.mode));
    ESP_ERROR_CHECK(i2c_param_config(i2c_master_port, &conf));


    bmp280_params_t bmp_conf;
    bmp280_init_default_params(&bmp_conf);

    bmp280_t bmp_dev = {
            .i2c_dev = {
                    .addr = BMP280_I2C_ADDRESS_0,
                    .i2c_num = I2C_NUM_0,
            }
    };
    bmp280_init(&bmp_dev, &bmp_conf);


    vTaskDelay(pdMS_TO_TICKS(5000));

    float dht_hum, dht_temp, ds_temp, bmp_temp, bmp_hum, bmp_press;
    while (1) {
        dht_hum = 0;
        dht_temp = 0;
        ds_temp = 0;
        bmp_temp = 0;
        bmp_press = 0;

        // this works ...
        ds_temp = ds18b20_measure_and_read(0, DS18B20_ANY);
        if (ds_temp != ds_temp) {
            ESP_LOGE(TAG, "DS failed");
        }

        if (!dht_read_float_data(DHT_TYPE_DHT22, 12, &dht_hum, &dht_temp)) {
            dht_hum = dht_temp = NAN;
            ESP_LOGE(TAG, "DHT failed");
        }

        if(!bmp280_read_float(&bmp_dev, &bmp_temp, &bmp_press, &bmp_hum)) {
            ESP_LOGE(TAG, "BMP failed");
        }

        /*
        {"uptime":125,"heap_free":44472,"heap_lwm":44028,"wifi_rssi":-48}
        {"ds18b20":21.500000,"dht22_t":22.000000,"dht22_h":42.500000,"wind_avg":0.000000,"wind_inst":0.000000,"wind_gust":0.000000,"bmp280_t":22.540001,"bmp280_p":97578.734375}
         */

        ESP_LOGI(TAG, "Dallas: %.2f °C, ** DHT %.2f °C, %.1f %%r.H, ** WIND avg %.1f, gust %.1f RPM, ** BMP %.2f °C, %f Pa",
               ds_temp, dht_temp, dht_hum,
               rpm_average, rpm_gust,
               bmp_temp, bmp_press);

        snprintf(pldbuf, 512, "{\"ds18b20\":%.2f,\"dht22_t\":%.1f,\"dht22_h\":%.1f,\"wind_avg\":%.1f,\"wind_inst\":%.0f,\"wind_gust\":%.0f,\"bmp280_t\":%.2f,\"bmp280_p\":%.1f}",
                 nan_fallback(ds_temp, -300), nan_fallback(dht_temp, -300), nan_fallback(dht_hum, -1),
                 nan_fallback(rpm_average, 0)/MAGNET_COUNT, nan_fallback(rpm_inst, 0)/MAGNET_COUNT, nan_fallback(rpm_gust, 0)/MAGNET_COUNT,
                 nan_fallback(bmp_temp, -300), nan_fallback(bmp_press, -1));

        mqtt_publish("measurement", pldbuf);

        if (timestamp - last_telemetry_ts >= 5*60 || last_telemetry_ts == 0) {
            wifi_ap_record_t ap_info = {};
            esp_wifi_sta_get_ap_info(&ap_info);
            snprintf(pldbuf, 512, "{\"uptime\":%"PRIu32",\"heap_free\":%d,\"heap_lwm\":%d,\"wifi_rssi\":%d}",
                     timestamp, heap_caps_get_free_size(0), heap_caps_get_minimum_free_size(0), ap_info.rssi);
            mqtt_publish("telemetry", pldbuf);
            last_telemetry_ts = timestamp;
        }

        vTaskDelay(pdMS_TO_TICKS(15000));
    }

    vTaskDelete(NULL);
}