#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include <string.h>
#include <math.h>
#include "esp_log.h"
#include <ds18x20.h>

#include "inputs.h"

#define MAX_DN18B20_SENSORS 4U
#define ONE_WIRE_LOOPS 4U    // try to read the 1-Wire sensors that often
#define PERIODIC_INTERVAL 1U // read and compute the inputs every 1sec

static const char *TAG = "smart-oil-heater-control-system-inputs";
const uint8_t uBurnerFaultPin = 19U;
const uint8_t uDS18B20Pin = 4U;

const onewire_addr_t uChamperTempSensorAddr = 0x78000000c6c2f728;
const onewire_addr_t uOutdoorTempSensorAddr = 0x78000000c6c2f728;
const onewire_addr_t uInletFlowTempSensorAddr = 0x78000000c6c2f728;
const onewire_addr_t uReturnFlowTempSensorAddr = 0x78000000c6c2f728;

onewire_addr_t uOneWireAddresses[MAX_DN18B20_SENSORS];
float fDS18B20Temps[MAX_DN18B20_SENSORS];
size_t sSensorCount = 0U;

static SemaphoreHandle_t xMutexAccessInputs = NULL;
static eBurnerErrorState sBurnerErrorState;
static sMeasurement sChamperTemperature;
static sMeasurement sOutdoorTemperature;
static sMeasurement sInletFlowTemperature;
static sMeasurement sReturnFlowTemperature;

void taskInput(void *pvParameters);
void initMeasurement(sMeasurement *pMeasurement);
void updateAverage(sMeasurement *pMeasurement);
void updatePrediction(sMeasurement *pMeasurement);
float linearRegressionPredict(const float *samples, size_t count, float futureIndex);

void initInputs(void)
{

    gpio_config_t ioConfBurnerFault = {
        .pin_bit_mask = (1ULL << uBurnerFaultPin), // Pin mask
        .mode = GPIO_MODE_INPUT,                   // Set as inout
        .pull_up_en = GPIO_PULLUP_ENABLE,          // Enable pull-up
        .pull_down_en = GPIO_PULLDOWN_DISABLE,     // Disable pull-down
        .intr_type = GPIO_INTR_DISABLE             // Disable interrupts
    };

    gpio_config(&ioConfBurnerFault);

    xMutexAccessInputs = xSemaphoreCreateRecursiveMutex();
    if (xMutexAccessInputs == NULL)
    {
        ESP_LOGE(TAG, "Unable to create mutex");
    }
    xSemaphoreGiveRecursive(xMutexAccessInputs);

    initMeasurement(&sChamperTemperature);
    initMeasurement(&sOutdoorTemperature);
    initMeasurement(&sInletFlowTemperature);
    initMeasurement(&sReturnFlowTemperature);

    BaseType_t taskCreated = xTaskCreate(
        taskInput,   // Function to implement the task
        "taskInput", // Task name
        4096,        // Stack size (in words, not bytes)
        NULL,        // Parameters to the task function (none in this case)
        5,           // Task priority (higher number = higher priority)
        NULL         // Task handle (optional)
    );

    if (taskCreated == pdPASS)
    {
        ESP_LOGI(TAG, "Task created successfully!");
    }
    else
    {
        ESP_LOGE(TAG, "Failed to create task");
    }
}

void initMeasurement(sMeasurement *pMeasurement)
{
    if (!pMeasurement)
        return;

    pMeasurement->state = MEASUREMENT_FAULT;
    pMeasurement->fCurrentValue = 0.0f;

    pMeasurement->average10s.fValue = 0.0f;
    pMeasurement->average10s.bufferCount = 0U;
    pMeasurement->average10s.bufferIndex = 0U;
    memset(pMeasurement->average10s.samples, 0U, AVG10_SAMPLE_SIZE);

    pMeasurement->average60s.fValue = 0.0f;
    pMeasurement->average60s.bufferCount = 0U;
    pMeasurement->average60s.bufferIndex = 0U;
    memset(pMeasurement->average60s.samples, 0U, AVG60_SAMPLE_SIZE);

    pMeasurement->predict60s.fValue = 0.0f;
    pMeasurement->predict60s.bufferCount = 0U;
    pMeasurement->predict60s.bufferIndex = 0U;
    memset(pMeasurement->predict60s.samples, 0U, PRED60_SAMPLE_SIZE);
}

void updateAverage(sMeasurement *pMeasurement)
{
    if (!pMeasurement)
        return;

    // Average form the last 10sec
    pMeasurement->average10s.samples[pMeasurement->average10s.bufferIndex] = pMeasurement->fCurrentValue;
    pMeasurement->average10s.bufferIndex = (pMeasurement->average10s.bufferIndex + 1) % AVG10_SAMPLE_SIZE;

    if (pMeasurement->average10s.bufferCount < AVG10_SAMPLE_SIZE)
    {
        pMeasurement->average10s.bufferCount++;
    }

    float sum = 0.0;
    for (int i = 0; i <= pMeasurement->average10s.bufferCount; i++)
    {
        sum += pMeasurement->average10s.samples[i];
    }

    pMeasurement->average10s.fValue = sum / pMeasurement->average10s.bufferCount;

    // Average form the last 60sec
    pMeasurement->average60s.samples[pMeasurement->average60s.bufferIndex] = pMeasurement->fCurrentValue;
    pMeasurement->average60s.bufferIndex = (pMeasurement->average60s.bufferIndex + 1) % AVG60_SAMPLE_SIZE;

    if (pMeasurement->average60s.bufferCount < AVG60_SAMPLE_SIZE)
    {
        pMeasurement->average60s.bufferCount++;
    }

    if (pMeasurement->average60s.bufferCount == 0U)
    {
        pMeasurement->average60s.fValue = pMeasurement->fCurrentValue;
    }

    sum = 0.0;
    for (int i = 0; i <= pMeasurement->average60s.bufferCount; i++)
    {
        sum += pMeasurement->average60s.samples[i];
    }

    pMeasurement->average60s.fValue = sum / pMeasurement->average60s.bufferCount;
}

void updatePrediction(sMeasurement *pMeasurement)
{
    if (!pMeasurement)
        return;

    // Update predict60s buffer
    sPredict *predict60s = &pMeasurement->predict60s;
    predict60s->samples[predict60s->bufferIndex] = pMeasurement->fCurrentValue;
    predict60s->bufferIndex = (predict60s->bufferIndex + 1) % PRED60_SAMPLE_SIZE;
    if (predict60s->bufferCount < PRED60_SAMPLE_SIZE)
        predict60s->bufferCount++;

    // Predict 60s future value using linear regression
    predict60s->fValue = linearRegressionPredict(
        predict60s->samples,
        predict60s->bufferCount,
        predict60s->bufferCount + 60.0f);
}

void taskInput(void *pvParameters)
{
    while (1)
    {
        vTaskDelay(PERIODIC_INTERVAL * 1000U / portTICK_PERIOD_MS);
        if (xSemaphoreTakeRecursive(xMutexAccessInputs, portMAX_DELAY) == pdTRUE)
        {
            sChamperTemperature.state = MEASUREMENT_FAULT;
            sOutdoorTemperature.state = MEASUREMENT_FAULT;
            sInletFlowTemperature.state = MEASUREMENT_FAULT;
            sReturnFlowTemperature.state = MEASUREMENT_FAULT;

            if (gpio_get_level(uBurnerFaultPin) == 1)
            {
                sBurnerErrorState = FAULT;
            }
            else
            {
                sBurnerErrorState = NO_ERROR;
            }

            if (ds18x20_scan_devices(uDS18B20Pin, uOneWireAddresses, MAX_DN18B20_SENSORS, &sSensorCount) != ESP_OK)
            {
                ESP_LOGE(TAG, "1-Wire device scan error!");
            }

            if (!sSensorCount)
            {
                ESP_LOGW(TAG, "No 1-Wire devices detected!");
            }
            else
            {
                // ESP_LOGI(TAG, "%d 1-Wire devices detected", sSensorCount);

                if (sSensorCount > MAX_DN18B20_SENSORS)
                {
                    sSensorCount = MAX_DN18B20_SENSORS;
                    ESP_LOGW(TAG, "More 1-Wire devices found than expected!");
                }

                for (size_t iReadLoop = 0; iReadLoop < ONE_WIRE_LOOPS; iReadLoop++)
                {
                    if (ds18x20_measure_and_read_multi(uDS18B20Pin, uOneWireAddresses, sSensorCount, fDS18B20Temps) != ESP_OK)
                    {
                        ESP_LOGE(TAG, "1-Wire devices read error");
                        vTaskDelay(PERIODIC_INTERVAL * 100U / portTICK_PERIOD_MS); // Wait 100ms if bus error occurred
                    }
                    else
                    {
                        for (int j = 0; j < sSensorCount; j++)
                        {
                            float temp_c = fDS18B20Temps[j];
                            ESP_LOGI(TAG, "Sensor: %08" PRIx64 " reports %lf°C", (uint64_t)uOneWireAddresses[j], temp_c);

                            switch ((uint64_t)uOneWireAddresses[j])
                            {
                            case ((uint64_t)uChamperTempSensorAddr):
                                sChamperTemperature.fCurrentValue = temp_c;
                                sChamperTemperature.state = MEASUREMENT_NO_ERROR;
                                updateAverage(&sChamperTemperature);
                                updatePrediction(&sChamperTemperature);

                                sOutdoorTemperature.fCurrentValue = temp_c;
                                sOutdoorTemperature.state = MEASUREMENT_NO_ERROR;
                                // updateAverage(&sOutdoorTemperature);

                                sInletFlowTemperature.fCurrentValue = temp_c;
                                sInletFlowTemperature.state = MEASUREMENT_NO_ERROR;
                                // updateAverage(&sInletFlowTemperature);

                                sReturnFlowTemperature.fCurrentValue = temp_c;
                                sReturnFlowTemperature.state = MEASUREMENT_NO_ERROR;
                                // updateAverage(&sReturnFlowTemperature);
                                break;
                            default:
                                break;
                            }
                        }
                        break;
                    }
                }
            }
            xSemaphoreGiveRecursive(xMutexAccessInputs);
        }
        else
        {
            sChamperTemperature.state = MEASUREMENT_FAULT;
            sOutdoorTemperature.state = MEASUREMENT_FAULT;
            sInletFlowTemperature.state = MEASUREMENT_FAULT;
            sReturnFlowTemperature.state = MEASUREMENT_FAULT;

            ESP_LOGE(TAG, "Unable to take mutex: taskInput()");
        }
    }
}

float linearRegressionPredict(const float *samples, size_t count, float futureIndex)
{
    if (count == 0)
        return 0.0f; // No prediction possible with no data

    float sumX = 0.0f, sumY = 0.0f, sumXY = 0.0f, sumX2 = 0.0f;

    for (size_t i = 0; i < count; i++)
    {
        float x = (float)i;   // Time index
        float y = samples[i]; // Sample value

        sumX += x;
        sumY += y;
        sumXY += x * y;
        sumX2 += x * x;
    }

    // Calculate slope (m) and intercept (b) of the line: y = mx + b
    float denominator = (count * sumX2 - sumX * sumX);
    if (fabs(denominator) < 1e-6)  // Avoid division by zero
        return samples[count - 1]; // Return last value as prediction

    float m = (count * sumXY - sumX * sumY) / denominator;
    float b = (sumY - m * sumX) / count;

    // Predict value at futureIndex
    return m * futureIndex + b;
}

sMeasurement getChamberTemperature(void)
{
    sMeasurement ret;
    ret.state = MEASUREMENT_FAULT;
    if (xSemaphoreTakeRecursive(xMutexAccessInputs, pdMS_TO_TICKS(5000)) == pdTRUE)
    {
        ret = sChamperTemperature;
        xSemaphoreGiveRecursive(xMutexAccessInputs);
    }
    else
    {
        ESP_LOGE(TAG, "Unable to take mutex: getChamberTemperature()");
    }
    return ret;
}

sMeasurement getOutdoorTemperature(void)
{
    sMeasurement ret;
    ret.state = MEASUREMENT_FAULT;
    if (xSemaphoreTakeRecursive(xMutexAccessInputs, pdMS_TO_TICKS(5000)) == pdTRUE)
    {
        ret = sOutdoorTemperature;
        xSemaphoreGiveRecursive(xMutexAccessInputs);
    }
    else
    {
        ESP_LOGE(TAG, "Unable to take mutex: getOutdoorTemperature()");
    }
    return ret;
}

sMeasurement getInletFlowTemperature(void)
{
    sMeasurement ret;
    ret.state = MEASUREMENT_FAULT;
    if (xSemaphoreTakeRecursive(xMutexAccessInputs, pdMS_TO_TICKS(5000)) == pdTRUE)
    {
        ret = sInletFlowTemperature;
        xSemaphoreGiveRecursive(xMutexAccessInputs);
    }
    else
    {
        ESP_LOGE(TAG, "Unable to take mutex: getInletFlowTemperature()");
    }
    return ret;
}

sMeasurement getReturnFlowTemperature(void)
{
    sMeasurement ret;
    ret.state = MEASUREMENT_FAULT;
    if (xSemaphoreTakeRecursive(xMutexAccessInputs, pdMS_TO_TICKS(5000)) == pdTRUE)
    {
        ret = sReturnFlowTemperature;
        xSemaphoreGiveRecursive(xMutexAccessInputs);
    }
    else
    {
        ESP_LOGE(TAG, "Unable to take mutex: getReturnFlowTemperature()");
    }
    return ret;
}

eBurnerErrorState getBurnerError(void)
{
    eBurnerErrorState ret = FAULT;
    if (xSemaphoreTakeRecursive(xMutexAccessInputs, pdMS_TO_TICKS(5000)) == pdTRUE)
    {
        ret = sBurnerErrorState;
        xSemaphoreGiveRecursive(xMutexAccessInputs);
    }
    else
    {
        ESP_LOGE(TAG, "Unable to take mutex: getBurnerError()");
    }
    return ret;
}