#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 "driver/timer.h" // Needed for timer_isr_register
#include "rom/ets_sys.h"


timg_dev_t *tg0 = &TIMERG0;
timg_dev_t *tg1 = &TIMERG1;

#define SET_TIMER_1(duty) do { \
    timer_ll_enable_counter(tg1, 0, false); \
    tg1->hw_timer[0].loadhi.val = 0; \
    tg1->hw_timer[0].loadlo.val = 0; \
    tg1->hw_timer[0].load.val = 1; \
    timer_ll_set_alarm_value(tg1, 0, duty); \
    timer_ll_enable_alarm(tg1, 0, true); \
    timer_ll_enable_counter(tg1, 0, true); \
} while(0)

#define SET_TIMER_2(duty) do { \
    timer_ll_enable_counter(tg1, 1, false); \
    tg1->hw_timer[1].loadhi.val = 0; \
    tg1->hw_timer[1].loadlo.val = 0; \
    tg1->hw_timer[1].load.val = 1; \
    timer_ll_set_alarm_value(tg1, 1, duty); \
    timer_ll_enable_alarm(tg1, 1, true); \
    timer_ll_enable_counter(tg1, 1, true); \
} while(0)

#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

#define PHASE_CONTROL 0
#define ZCD_CONTROL   1

// 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 = PHASE_CONTROL;
volatile uint8_t ac2ControlMode = PHASE_CONTROL;

volatile uint8_t fireStatusTimer1 = 0;
volatile uint8_t fireStatusTimer2 = 0;


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%
};

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

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

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)
ARDUINO_ISR_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);
    ESP_LOGD(TAG_ZCD,"Set Duty: %.2f%%, Timer Count: %u clock cycles", duty, dutyHeater1);
}

// Function to set the duty based on percentage (0 to 10000)
ARDUINO_ISR_ATTR void setHeater2Duty(short duty) {
    if (config.bAC2_OnOff) {
        if (duty >= 10000) {
            digitalWrite(PIN_HEATER2, HEATER_ON);
            duty = 10000;
        } else {
            digitalWrite(PIN_HEATER2, HEATER_OFF);
            duty = 0;
        }
        dutyHeater2 = 0;
    } 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);
    ESP_LOGD(TAG_ZCD,"Set Duty: %.2f%%, Timer Count: %u clock cycles\n", duty, dutyHeater2);
}


void ARDUINO_ISR_ATTR onTimer1(void *) {
    tg1->int_clr_timers.t0_int_clr = 1; // Clear Interrupt
    
    if (fireStatusTimer1 == 0) {
        // First Trigger: Turn ON
        REG_WRITE(GPIO_OUT1_W1TS_REG, (1UL << (PIN_HEATER1 - 32)));
        fireStatusTimer1 = 1;
        SET_TIMER_1(8); // Schedule OFF pulse 8us later
    } else {
        // Second Trigger: Turn OFF
        REG_WRITE(GPIO_OUT1_W1TC_REG, (1UL << (PIN_HEATER1 - 32)));
    }
}

void ARDUINO_ISR_ATTR onTimer2(void *) {
    tg1->int_clr_timers.t1_int_clr = 1;
    
    if (fireStatusTimer2 == 0) {
        REG_WRITE(GPIO_OUT1_W1TS_REG, (1UL << (PIN_HEATER2 - 32)));
        fireStatusTimer2 = 1;
        SET_TIMER_2(8);
    } else {
        REG_WRITE(GPIO_OUT1_W1TC_REG, (1UL << (PIN_HEATER2 - 32)));
    }
}

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

    // 3. Heater 1
    if (ac1ControlMode == ZCD_CONTROL) {
        if (fireTable[dutyAC1TableIndex][seqStep]) SET_TIMER_1(LEADING_ZCD_COUNT);
    } else if ( dutyHeater1 >= LEADING_PULSE_COUNT){
        SET_TIMER_1(dutyHeater1);
    }

    // 4. Heater 2
    if (ac2ControlMode == ZCD_CONTROL) {
        if (fireTable[dutyAC2TableIndex][seqStep]) SET_TIMER_2(LEADING_ZCD_COUNT);
    } else if (dutyHeater2 >= LEADING_PULSE_COUNT) {
        SET_TIMER_2(dutyHeater2);
    }

    seqStep = ++seqStep & 0x0F;
}

void ARDUINO_ISR_ATTR zcdLoadISR(void *) {
    // Load side AC ZCD Count
    zcdLoadISRCount++;
}

void setupZCD() {
    pinMode(PIN_ZCD_AC, INPUT);
    pinMode(PIN_ZCD_LOAD, INPUT);
    pinMode(PIN_HEATER1, OUTPUT);
    pinMode(PIN_HEATER2, OUTPUT);
    digitalWrite(PIN_HEATER1, HEATER_OFF);
    digitalWrite(PIN_HEATER2, HEATER_OFF);

    dutyHeater1 = 0;  // Calculated timerHeater1 count for TRIAC firing
    dutyHeater2 = 0;  // Calculated timerZCD count for TRIAC firing
    zcdACCount = 0;
    zcdLoadCount = 0;
    timerHeater1 = NULL;

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

    ac1ControlMode = config.ac1ControlMode;
    ac2ControlMode = config.ac2ControlMode;

    // --- 0. Hardware Config for TG0-Timer0 (g_millis)
    timer_ll_set_clock_prescale(tg0, 0, 8000); // 10KHz
    timer_ll_set_count_direction(tg0, 0, GPTIMER_COUNT_UP);
    tg0->hw_timer[0].loadhi.val = 0UL;
    tg0->hw_timer[0].loadlo.val = 0UL;
    tg0->hw_timer[0].load.val = 1UL;
    timer_ll_enable_alarm(tg0, 0, false); // No ISR needed for g_millis
    timer_ll_enable_counter(tg0, 0, true);

    // --- 1. Hardware Config for TG0-Timer1 (Watchdog) ---
    tg0->hw_timer[1].loadhi.val = 0UL;
    tg0->hw_timer[1].loadlo.val = 0UL;
    tg0->hw_timer[1].load.val = 1UL;
    timer_ll_set_clock_prescale(tg0, 1, 80); // 1MHz
    timer_ll_set_count_direction(tg0, 1, GPTIMER_COUNT_UP);
    timer_ll_enable_alarm(tg0, 1, false); // No ISR needed for zcdACISR Watchdog
    timer_ll_enable_counter(tg0, 1, true);

    // --- 2. Basic Hardware Config for TG1 (Heaters) ---
    // Timer 0
    timer_ll_set_clock_prescale(tg1, 0, 80); 
    timer_ll_set_count_direction(tg1, 0, GPTIMER_COUNT_UP);
    timer_ll_enable_alarm(tg1, 0, true);
    
    // Timer 1
    timer_ll_set_clock_prescale(tg1, 1, 80); 
    timer_ll_set_count_direction(tg1, 1, GPTIMER_COUNT_UP);
    timer_ll_enable_alarm(tg1, 1, true);

    // --- 3. Manual ISR Registration ---
    esp_intr_alloc(ETS_TG1_T0_LEVEL_INTR_SOURCE, ESP_INTR_FLAG_IRAM, onTimer1, NULL, NULL);
    esp_intr_alloc(ETS_TG1_T1_LEVEL_INTR_SOURCE, ESP_INTR_FLAG_IRAM, onTimer2, NULL, NULL);

    tg0->hw_timer[0].update.val = 1;
    uint32_t startupTime = timer_ll_get_counter_value(tg0, 0);
    
    zcdACISRCount = 0;
    zcdLoadISRCount = 0;

    // --- 4. Hardware Trigger ISR Registration ---
    //attachInterrupt(PIN_ZCD_AC, zcdACISR, CHANGE);  // Attach zero-cross detection ISR
    //attachInterrupt(PIN_ZCD_LOAD, zcdLoadISR, CHANGE);  // Attach zero-cross detection ISR

    // Attach Load ISR with higher priority
    esp_intr_alloc(ETS_GPIO_INTR_SOURCE, ESP_INTR_FLAG_LEVEL3, zcdLoadISR, NULL, NULL);

    // Attach AC ISR with normal/lower priority
    esp_intr_alloc(ETS_GPIO_INTR_SOURCE, ESP_INTR_FLAG_LEVEL1, zcdACISR, NULL, NULL);
}

void setACLoadStatus(uint32_t tNow) {
    static uint32_t lastTick = 0;
    // --- 1. Process AC/Load Window (Every 1s) ---
    if (tNow - lastTick >= 9999) {
        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;
    }
}