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

#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


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;

hw_timer_t *timerHeater1;
hw_timer_t *timerHeater2;


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 (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() {
    digitalWrite(PIN_HEATER1, HIGH);  // Fire TRIAC
    delayMicroseconds(10);          // Short pulse to trigger TRIAC
    digitalWrite(PIN_HEATER1, LOW);   // Turn off TRIAC trigger
}

void ARDUINO_ISR_ATTR onTimer2() {
    digitalWrite(PIN_HEATER2, HIGH);  // Fire TRIAC
    delayMicroseconds(10);          // Short pulse to trigger TRIAC
    digitalWrite(PIN_HEATER2, LOW);   // Turn off TRIAC trigger
}

// Zero-Cross Detection Interrupt Service Routine
void ARDUINO_ISR_ATTR zcdACISR() {
    uint32_t clock = micros();
    static uint32_t lastClock = 0l;

    if (clock - lastClock < 8000)
      return;
    lastClock = clock;

    zcdACCount++;

    // Heater 1
    if (dutyHeater1 == MAX_PULSE_COUNT) {
        onTimer1();
    }
    else if (dutyHeater1 >= LEADING_PULSE_COUNT && dutyHeater1 < MAX_PULSE_COUNT) {
          // Stop the timer, configure new alarm, then explicitly start
          timerStop(timerHeater1);                // Stop any existing timer action
          timerWrite(timerHeater1, 0);            // Reset counter to 0
          timerAlarm(timerHeater1, dutyHeater1, false, 0);  // Set alarm with updated duty
          timerStart(timerHeater1);               // Start the timer explicitly
    }
    // Heater 2
    if (dutyHeater2 == MAX_PULSE_COUNT) {
        onTimer2();
    }
    else if (dutyHeater2 >= LEADING_PULSE_COUNT && dutyHeater2 < MAX_PULSE_COUNT) {
          // Stop the timer, configure new alarm, then explicitly start
          timerStop(timerHeater2);                // Stop any existing timer action
          timerWrite(timerHeater2, 0);            // Reset counter to 0
          timerAlarm(timerHeater2, dutyHeater2, false, 0);  // Set alarm with updated duty
          timerStart(timerHeater2);               // Start the timer explicitly
    }
}

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;

    attachInterrupt(PIN_ZCD_AC, zcdACISR, CHANGE);  // Attach zero-cross detection ISR
    attachInterrupt(PIN_ZCD_LOAD, zcdLoadISR, CHANGE);  // Attach zero-cross detection ISR

    // Initialize and configure the timer
    if ((timerHeater1 = timerBegin(1000000)) != NULL) {
        timerAttachInterrupt(timerHeater1, &onTimer1);    // Attach TRIAC firing routine
        timerStop(timerHeater1); // Ensure timer is stopped initially
        timerStart(timerHeater1);  // Explicitly start the timer after setup

    }

    if ((timerHeater2 = timerBegin(1000000)) != NULL) {
        timerAttachInterrupt(timerHeater2, &onTimer2);    // Attach TRIAC firing routine
        timerStop(timerHeater2); // Ensure timer is stopped initially
        timerStart(timerHeater2);  // Explicitly start the timer after setup
    }
}