#include "HermitCrab.h"
#include "Config.h"
#include "zcd.h"
#include <math.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

// Timer Headers
#include "hal/timer_ll.h"
#include "hal/timer_types.h"
#include "rom/ets_sys.h"

// GPIO Headers
#include "driver/gpio.h"
#include "driver/timer.h"

//#include "driver/timer_types_legacy.h"
//#include "esp_intr_alloc.h"


#define ESP_INTR_FLAG_LEVEL3 (1<<3)
#define ESP_INTR_FLAG_LEVEL1 (1<<1)

#define TAG_ZCD "ZCD"
// Constants
#define EFFECTIVE_POWER 0.86
#define LEADING_TIME_RATIO 0.06

// ESP32 Clock Constants
const uint32_t AC_CYCLE_TIME_CLOCKS = 8333; // Half cycle of 60Hz AC in clock cycles
const uint32_t EFFECTIVE_HALF_CYCLE = EFFECTIVE_POWER * AC_CYCLE_TIME_CLOCKS; // Effective half cycle in clock cycles
const uint32_t LEADING_PULSE_COUNT = EFFECTIVE_HALF_CYCLE * LEADING_TIME_RATIO; // Leading pulse count
const uint32_t MAX_PULSE_COUNT = LEADING_PULSE_COUNT + EFFECTIVE_HALF_CYCLE;    // Maximum valid pulse count
const uint32_t LEADING_ZCD_COUNT = LEADING_PULSE_COUNT / 2;


volatile uint32_t dutyHeater1;  // Calculated timerHeater1 count for TRIAC firing
volatile uint32_t dutyHeater2;  // Calculated timerZCD count for TRIAC firing
volatile uint8_t zcdACCount;
volatile uint8_t zcdLoadCount;

volatile uint8_t ac1ControlMode;
volatile uint8_t ac2ControlMode;

volatile uint8_t fireStatusTimer0;
volatile uint8_t fireStatusTimer1;

timg_dev_t *tg0;
timg_dev_t *tg1;

//hw_timer_t *timer10K;
//hw_timer_t *timerDummy;
//hw_timer_t *timerHeater1;
//hw_timer_t *timerHeater2;

const char fireTable[17][16] { 
    {0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0}, // 0   0%
    {1,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0}, // 1   6.25%  (Dist: 16)
    {1,0,0,0, 0,0,0,0, 1,0,0,0, 0,0,0,0}, // 2   12.5%  (Dist: 8, 8)
    {1,0,0,0, 0,1,0,0, 0,0,0,1, 0,0,0,0}, // 3   18.75% (Dist: 5, 6, 5)
    {1,0,0,0, 1,0,0,0, 1,0,0,0, 1,0,0,0}, // 4   25%    (Dist: 4, 4, 4, 4)
    {1,0,0,1, 0,0,1,0, 0,1,0,0, 1,0,0,0}, // 5   31.25% (Dist: 3, 3, 3, 3, 4)
    {1,0,1,0, 0,1,0,0, 1,0,1,0, 0,1,0,0}, // 6   37.5%  (Dist: 2, 3, 3, 2, 3, 3)
    {1,0,1,0, 1,0,0,1, 0,1,0,1, 0,1,0,0}, // 7   43.75% (Dist: 2, 2, 3, 2, 2, 2, 3)
    {1,0,1,0, 1,0,1,0, 1,0,1,0, 1,0,1,0}, // 8   50%    (Perfect Toggle)
    {0,1,0,1, 0,1,1,0, 1,0,1,0, 1,0,1,1}, // 9   56.25% (Inv 7: spaced 0s)
    {0,1,0,1, 1,0,1,1, 0,1,0,1, 1,0,1,1}, // 10  62.5%  (Inv 6: spaced 0s)
    {0,1,1,0, 1,1,0,1, 1,0,1,1, 0,1,1,1}, // 11  68.75% (Inv 5: spaced 0s)
    {0,1,1,1, 0,1,1,1, 0,1,1,1, 0,1,1,1}, // 12  75%    (Inv 4: spaced 0s)
    {0,1,1,1, 1,0,1,1, 1,1,0,1, 1,1,1,0}, // 13  81.25% (Inv 3: spaced 0s)
    {0,1,1,1, 1,1,1,1, 0,1,1,1, 1,1,1,1}, // 14  87.5%  (Inv 2: spaced 0s)
    {0,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1}, // 15  93.75% (Inv 1: spaced 0s)
    {1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1}  // 16  100%
};

volatile uint8_t seqStep = 0; 
volatile uint8_t dutyAC1TableIndex = 0; 
volatile uint8_t dutyAC2TableIndex = 0;

// AC Frequency and health status
volatile uint8_t zcdACISRCount = 0;
volatile uint8_t zcdLoadISRCount = 0;

volatile bool fire_enable_1 = false;
volatile bool fire_enable_2 = false;

void setAC1ControlMode(uint8_t mode) { ac1ControlMode = mode; }                             
void setAC2ControlMode(uint8_t mode) { ac2ControlMode = mode; }

short getHeater1Duty() {
    if (dutyHeater1 == 0) return 0;
    if (dutyHeater1 >= LEADING_PULSE_COUNT + EFFECTIVE_HALF_CYCLE) return 10000;
    return round(10000.0f * (dutyHeater1 - LEADING_PULSE_COUNT) / EFFECTIVE_HALF_CYCLE);
}

short getHeater2Duty() {
    if (dutyHeater2 == 0) return 0;
    if (dutyHeater2 >= LEADING_PULSE_COUNT + EFFECTIVE_HALF_CYCLE) return 10000;
    return round(10000.0f * (dutyHeater1 - LEADING_PULSE_COUNT) / EFFECTIVE_HALF_CYCLE);
}

// Function to set the duty based on percentage (0 to 10000)
MY_IRAM_ATTR void setHeater1Duty(short duty) {
    if (ac1ControlMode == ZCD_CONTROL) {
        if (duty > 10000) duty = 10000;
        dutyAC1TableIndex = (uint8_t)((duty + 10000/32)/625);
    } else {
        if (duty <= 0) {
            dutyHeater1 = 0;  // If 0% duty, no pulse (turn off TRIAC)
        } else if (duty >= 10000) {
            // 100% duty corresponds to the leading pulse + full effective half cycle
            dutyHeater1 = LEADING_PULSE_COUNT;
        } else {
            // Map duty to power ratio (0 to 1)
            float powerRatio = (float) duty / 10000.0f;

            // Calculate the angle in radians using the inverse cosine directly
            float angleRadians = acosf(1.0f - 2.0f * powerRatio);

            // Convert angle to time delay (in clock cycles)
            // Normalized angle (0 to PI) maps to half-cycle (0 to EFFECTIVE_HALF_CYCLE)
            uint32_t pulseCount = (angleRadians / M_PI) * EFFECTIVE_HALF_CYCLE;

            dutyHeater1 = LEADING_PULSE_COUNT + EFFECTIVE_HALF_CYCLE - pulseCount;
        }
    }
    
    uint32_t nDuty = duty * PWM_FULL / 10000;
    ledcWrite(PIN_LED_HEATER1, PWM_FULL - nDuty);
    //DPRINTF("[ZCD] Set Duty: %.2f%%, Timer Count: %u clock cycles", duty, dutyHeater1);
}

// Function to set the duty based on percentage (0 to 10000)
MY_IRAM_ATTR void setHeater2Duty(short duty) {
    if (config.bAC2_OnOff) {
        if (duty >= 10000) {
            duty = 10000;
            dutyHeater2 = LEADING_PULSE_COUNT;
        } else {
            duty = 0;
            dutyHeater2 = 0;  // If 0% duty, no pulse (turn off TRIAC)
        }
    } else {
        if (ac2ControlMode == ZCD_CONTROL) {
            if (duty > 10000) duty = 10000;
            dutyAC2TableIndex = (uint8_t)((duty + 10000/32)/625);
        } else {
            if (duty <= 0) {
                dutyHeater2 = 0;  // If 0% duty, no pulse (turn off TRIAC)
            } else if (duty >= 10000) {
                // 100% duty corresponds to the leading pulse + full effective half cycle
                dutyHeater2 = LEADING_PULSE_COUNT;
            } else {
                // Map duty to power ratio (0 to 1)
                float powerRatio = (float) duty / 10000.0f;

                // Calculate the angle in radians using the inverse cosine directly
                float angleRadians = acosf(1.0f - 2.0f * powerRatio);

                // Convert angle to time delay (in clock cycles)
                // Normalized angle (0 to PI) maps to half-cycle (0 to EFFECTIVE_HALF_CYCLE)
                uint32_t pulseCount = (angleRadians / M_PI) * EFFECTIVE_HALF_CYCLE;

                dutyHeater2 = LEADING_PULSE_COUNT + EFFECTIVE_HALF_CYCLE - pulseCount;
            }
        }
    }

    uint32_t nDuty = duty * PWM_FULL / 10000;
    ledcWrite(PIN_LED_HEATER2, PWM_FULL - nDuty);
    //DPRINTF("[ZCD] Set Duty: %.2f%%, Timer Count: %u clock cycles\n", duty, dutyHeater2);
}

void IRAM_ATTR timer0ISR(void *) {
    #ifdef DEBUG_ZCD
    zcdLoadISRCount++;
    #endif         
    //timer_group_clr_intr_status_in_isr(TIMER_GROUP_1, TIMER_0);
    TIMERG1.int_clr_timers.t0_int_clr = 1;
    if (fire_enable_1) {
        if (fireStatusTimer0 == 0) {
            gpio_set_level((gpio_num_t)PIN_HEATER1, 1);
            fireStatusTimer0 = 1;

            // Set next timer
            timer_set_counter_value(TIMER_GROUP_1, TIMER_0, 0);
            timer_set_alarm_value(TIMER_GROUP_1, TIMER_0, 18);
            timer_set_alarm(TIMER_GROUP_1, TIMER_0, TIMER_ALARM_EN);
        } else {
            gpio_set_level((gpio_num_t)PIN_HEATER1, 0);

            // Suspend Timer until next zcdACISR
            fire_enable_1 = false;
        }
    }
}

void IRAM_ATTR timer1ISR(void *)
{
    //timer_group_clr_intr_status_in_isr(TIMER_GROUP_1, TIMER_1);
    TIMERG1.int_clr_timers.t1_int_clr = 1;
    if (fire_enable_2) {
        if (fireStatusTimer1 == 0) {
            gpio_set_level((gpio_num_t)PIN_HEATER2, 1);
            fireStatusTimer1 = 1;

            // Set next timer
            timer_set_counter_value(TIMER_GROUP_1, TIMER_1, 0);
            timer_set_alarm_value(TIMER_GROUP_1, TIMER_1, 18);
            timer_set_alarm(TIMER_GROUP_1, TIMER_1, TIMER_ALARM_EN);
        } else {
            gpio_set_level((gpio_num_t)PIN_HEATER2, 0);

            // Suspend Timer until next zcdACISR
            fire_enable_2 = false;
        }
    }
}

// Zero-Cross Detection Interrupt Service Routine
void IRAM_ATTR zcdACISR(void *) {
    // 1. Power side AC ZCD Count 
    zcdACISRCount = zcdACISRCount + 1;
    
    fireStatusTimer0 = 0;
    fireStatusTimer1 = 0;

    // 3. Heater 1
    if (ac1ControlMode == ZCD_CONTROL) {
        if (fireTable[dutyAC1TableIndex][seqStep]) {
            timer_set_counter_value(TIMER_GROUP_1, TIMER_0, 0);
            timer_set_alarm_value(TIMER_GROUP_1, TIMER_0, (uint64_t) LEADING_ZCD_COUNT);
            timer_set_alarm(TIMER_GROUP_1, TIMER_0, TIMER_ALARM_EN);
            fire_enable_1 = true;
        }
    } else if ( dutyHeater1 >= LEADING_PULSE_COUNT){
        timer_set_counter_value(TIMER_GROUP_1, TIMER_0, 0);
        timer_set_alarm_value(TIMER_GROUP_1, TIMER_0, (uint64_t) dutyHeater1);
        timer_set_alarm(TIMER_GROUP_1, TIMER_0, TIMER_ALARM_EN);
        fire_enable_1 = true;
    }

    // 4. Heater 2
    if (ac2ControlMode == ZCD_CONTROL) {
        if (fireTable[dutyAC2TableIndex][seqStep]) {
            timer_set_counter_value(TIMER_GROUP_1, TIMER_1, 0);
            timer_set_alarm_value(TIMER_GROUP_1, TIMER_1, (uint64_t)LEADING_ZCD_COUNT);
            timer_set_alarm(TIMER_GROUP_1, TIMER_1, TIMER_ALARM_EN);
            fire_enable_2 = true;
        }
    } else if (dutyHeater2 >= LEADING_PULSE_COUNT) {
        timer_set_counter_value(TIMER_GROUP_1, TIMER_1, 0);
        timer_set_alarm_value(TIMER_GROUP_1, TIMER_1, (uint64_t)dutyHeater2);
        timer_set_alarm(TIMER_GROUP_1, TIMER_1, TIMER_ALARM_EN);
        fire_enable_2 = true;
    }
    seqStep = (seqStep + 1) & 0x0F;
}

void IRAM_ATTR zcdLoadISR(void *) {
    // Load side AC ZCD Count
    #ifndef DEBUG_ZCD
    zcdLoadISRCount = zcdLoadISRCount + 1;
    #endif
}

void setupZCD() {
    //===============================================================================
    // Initialize variables
    dutyHeater1 = 0;  // Calculated timerHeater1 count for TRIAC firing
    dutyHeater2 = 0;  // Calculated timerZCD count for TRIAC firing
    zcdACCount = 0;
    zcdLoadCount = 0;

    // Test
    config.ac1ControlMode = PHASE_CONTROL;
    config.ac2ControlMode = PHASE_CONTROL;

    fireStatusTimer0 = 0;
    fireStatusTimer1 = 0;

    tg0 = &TIMERG0;
    tg1 = &TIMERG1;
    ac1ControlMode = config.ac1ControlMode;
    ac2ControlMode = config.ac2ControlMode;
}

void setACLoadStatus(uint32_t tNow) {
    static uint32_t lastTick = 0;
    // --- 1. Process AC/Load Window (Every 1s) ---
    if (tNow - lastTick >= 10000) {
        zcdACCount = zcdACISRCount;
        zcdLoadCount = zcdLoadISRCount;

        zcdACISRCount = 0;
        zcdLoadISRCount = 0;
        lastTick = tNow;
    }

    // Safety: If no ISR has fired in over 1.5 seconds, force count to 0
    if (tNow - lastTick > 15000) {
        zcdACCount = 0;
        zcdLoadCount = 0;
    }

    // ZCD
    status.zcdAC = zcdACCount;
    status.zcdLoad = zcdLoadCount;
    if (status.zcdAC < 119 || status.zcdAC > 121)  {
        status.nFlags |= FLAG_ZCD_AC;
    }
    else {
        status.nFlags &= ~FLAG_ZCD_AC;
    }
    if (status.zcdLoad < 6 || status.zcdLoad > 181) {
        status.nFlags |= FLAG_ZCD_LOAD;
    }
    else {
        status.nFlags &= ~FLAG_ZCD_LOAD;
    }
}