// currentshaper.c
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <sys/param.h>
#include "evse_api.h"
#include "input_filter.h"

static const char *TAG = "currentshaper";

#define EVSE_SHAPER_HYSTERESIS 50  // A*10
#define CHARGING_CURRENT_MIN 60    // A*10

static TaskHandle_t currentshaper_task = NULL;

static uint32_t smoothing_time_ms = 1000;
static uint32_t min_pause_time_ms = 300000;
static uint32_t max_data_interval_ms = 120000;

static int max_grid_current = 300;  // A*10
static int live_grid_current = 0;  // A*10
static int live_voltage = 230;
static double smoothed_current = 0;

static uint16_t max_current = 0;
static TickType_t timer = 0;
static TickType_t pause_timer = 0;

static bool updated = false;
static bool changed = false;

static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;

void shapeCurrent()
{
    taskENTER_CRITICAL(&mux);
    updated = true;
    smoothed_current = filter(live_grid_current, smoothed_current, smoothing_time_ms);
    int diff = max_grid_current - (int)smoothed_current;
    int charge_current = evse_get_charging_current();
    int new_current = diff + charge_current;

    new_current = MIN(new_current, evse_get_max_charging_current() * 10);
    new_current = MAX(new_current, CHARGING_CURRENT_MIN);

    max_current = new_current;
    if (charge_current != max_current)
    {
        evse_set_charging_current(max_current);
        changed = true;
    }
    taskEXIT_CRITICAL(&mux);
}

void setMaxGridCurrent(int value)
{
    taskENTER_CRITICAL(&mux);
    max_grid_current = value;
    taskEXIT_CRITICAL(&mux);
    ESP_LOGI(TAG, "Max grid current: %d A*10", value);
}

void setLiveGridCurrent(int value)
{
    taskENTER_CRITICAL(&mux);
    live_grid_current = value;
    taskEXIT_CRITICAL(&mux);
    ESP_LOGI(TAG, "Live grid current: %d A*10", value);
    if (evse_state_is_charging(evse_get_state()))
    {
        shapeCurrent();
    }
}

void setLiveVoltage(int value)
{
    taskENTER_CRITICAL(&mux);
    live_voltage = value;
    taskEXIT_CRITICAL(&mux);
    ESP_LOGD(TAG, "Live voltage: %d V", value);
}

static void currentshaper_task_func(void *param)
{
    while (true)
    {
        TickType_t now = xTaskGetTickCount();
        if (evse_state_is_charging(evse_get_state()))
        {
            taskENTER_CRITICAL(&mux);
            if (changed)
            {
                if (max_current < CHARGING_CURRENT_MIN)
                {
                    evse_set_charging_current(CHARGING_CURRENT_MIN);
                    if (pause_timer == 0)
                        pause_timer = now;
                }
                else if ((pause_timer != 0) && (now - pause_timer) * portTICK_PERIOD_MS >= min_pause_time_ms &&
                         (max_current - CHARGING_CURRENT_MIN >= EVSE_SHAPER_HYSTERESIS))
                {
                    pause_timer = 0;
                }
                timer = now;
                changed = false;
            }
            else if (!updated || (now - timer) * portTICK_PERIOD_MS > max_data_interval_ms)
            {
                if (updated)
                {
                    pause_timer = now;
                    updated = false;
                    smoothed_current = live_grid_current;
                }
            }
            taskEXIT_CRITICAL(&mux);
        }
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

void currentshaper_start()
{
    ESP_LOGI(TAG, "Starting current shaper task");
    xTaskCreate(currentshaper_task_func, "currentshaper_task", 4096, NULL, 5, &currentshaper_task);
}

void currentshaper_stop()
{
    ESP_LOGI(TAG, "Stopping current shaper task");
    if (currentshaper_task != NULL)
    {
        vTaskDelete(currentshaper_task);
        currentshaper_task = NULL;
    }
}