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

#include "hal/timer_ll.h"
#include "hal/timer_types.h"
#include "driver/timer.h" // Needed for timer_isr_register

// Assuming tg0 is defined as: timg_dev_t *tg0 = &TIMG0;
timg_dev_t *tg0 = &TIMERG0;
timg_dev_t *tg1 = &TIMERG1;

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

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


#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


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[9][8] { {0, 0, 0, 0, 0, 0, 0, 0},  // 0 - 0%
                             {1, 0, 0, 0, 0, 0, 0, 0},  // 1 - 12.5%
                             {1, 0, 0, 0, 1, 0, 0, 0},  // 2 - 25%
                             {1, 0, 0, 1, 0, 0, 1, 0},  // 3 - 37.5%
                             {1, 0, 1, 0, 1, 0, 1, 0},  // 4 - 50%
                             {1, 1, 0, 1, 1, 0, 1, 0},  // 5 - 62.5%
                             {1, 1, 1, 0, 1, 1, 1, 0},  // 6 - 75%
                             {1, 1, 1, 1, 1, 1, 1, 0},  // 7 - 87.5%
                             {1, 1, 1, 1, 1, 1, 1, 1}}; // 8 - 100%
uint8_t seqStep = 0; 
uint8_t dutyAC1 = 0; 
uint8_t dutyAC2 = 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) {
        dutyAC1 = (uint8_t)((duty + 10000/16)/1250);
    } 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 (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, dutyHeater1);
}


void ARDUINO_ISR_ATTR onTimer1(void *) {
    if (fireStatusTimer1) {
        REG_WRITE(GPIO_OUT1_W1TC_REG, (1UL << (PIN_HEATER1 - 32))); // Clear (Low)
    } else {
        REG_WRITE(GPIO_OUT1_W1TS_REG, (1UL << (PIN_HEATER1 - 32))); // Set (High)
        fireStatusTimer1 = 1;
        SET_TIMER_1(8); // 8 us
    }
}

void ARDUINO_ISR_ATTR onTimer2(void *) {
    if (fireStatusTimer2) {
        REG_WRITE(GPIO_OUT1_W1TC_REG, (1UL << (PIN_HEATER2 - 32))); // Clear (Low)
    } else {
        REG_WRITE(GPIO_OUT1_W1TS_REG, (1UL << (PIN_HEATER2 - 32))); // Set (High)
        fireStatusTimer2 = 1;
        SET_TIMER_2(8); // 8 us
    }
}


// Zero-Cross Detection Interrupt Service Routine
void ARDUINO_ISR_ATTR zcdACISR() {
    // 1. Check the dedicated Watchdog Timer (Group 1)
    uint32_t elapsed = timer_ll_get_counter_value(tg1, 0);
    if (elapsed < 8000) return; // Reject noise based on absolute time since last ZCD
    tg1->hw_timer[0].loadhi.val = 0UL;
    tg1->hw_timer[0].loadlo.val = 0UL;
    tg1->hw_timer[0].load.val = 1UL;

    zcdACCount++;
    fireStatusTimer1 = 0;
    fireStatusTimer2 = 0;

    // Heater 1
    if (ac1ControlMode == ZCD_CONTROL) {
        if (fireTable[dutyAC1][seqStep]) {
            onTimer1(NULL);
        }
    } else {
        if (dutyHeater1 >= LEADING_PULSE_COUNT && dutyHeater1 < MAX_PULSE_COUNT) {
            // Stop the timer, configure new alarm, then explicitly start
            SET_TIMER_1(dutyHeater1);               // Set alarm with updated duty
        }
        else if (dutyHeater1 == MAX_PULSE_COUNT) {
            onTimer1(NULL);
        }
    }

    // Heater 2
    if (ac2ControlMode == ZCD_CONTROL) {
        if (fireTable[dutyAC2][seqStep]) {
            onTimer2(NULL);
        }
    }
    else {
        if (dutyHeater2 >= LEADING_PULSE_COUNT && dutyHeater2 < MAX_PULSE_COUNT) {
            // Stop the timer, configure new alarm, then explicitly start
            SET_TIMER_2(dutyHeater2);               // Set alarm with updated duty
        }
        else if (dutyHeater2 == MAX_PULSE_COUNT) {
            onTimer2(NULL);
        }
    }

    seqStep = ++seqStep & 0x07;
}

void ARDUINO_ISR_ATTR zcdLoadISR() {
    ++zcdLoadCount;
}

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

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

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

    // --- 2. Manual ISR Registration ---
    // ESP_INTR_FLAG_IRAM ensures the ISR stays in IRAM for fast context switching.
    timer_isr_register((timer_group_t)0, (timer_idx_t)0, onTimer1, NULL, ESP_INTR_FLAG_IRAM, NULL);
    timer_isr_register((timer_group_t)0, (timer_idx_t)1, onTimer2, NULL, ESP_INTR_FLAG_IRAM, NULL);

    // --- 3. Hardware Config for TG1 (Watchdog) ---
    timer_ll_set_clock_prescale(tg1, 0, 80000); 
    timer_ll_set_count_direction(tg1, 0, GPTIMER_COUNT_UP);
    tg1->hw_timer[0].loadhi.val = 0UL;
    tg1->hw_timer[0].loadlo.val = 0UL;
    tg1->hw_timer[0].load.val = 1UL;
    timer_ll_enable_alarm(tg1, 0, false); // No ISR needed for watchdog
    timer_ll_enable_counter(tg1, 0, true);
}