// #define ZCDEBUG
#include "Stab_config.h"
#ifdef ZCDEBUG
int zeroCrossPerSecond = 0;
volatile int zeroCrossCount = 0;
volatile int lastZeroCrossCount = 0;
Ticker timer;
#endif


void heat(bool SW) { // Переключатель нагрева
 heatEnabled = SW;
 digitalWrite(heatPin, SW ? HIGH : LOW);
 if (!heatEnabled && boostMode) boost(OFF);
 if (!heatEnabled) PowerLevel=0;
}

void boost(bool SW) { // Переключатель разгона
boostMode = SW;
    if (boostMode) {
      PowerLevelT = PowerLevel;
      PowerLevel = MaxPowerLevel;
      heat(ON);
    } else {
      PowerLevel = PowerLevelT;     
    }
 digitalWrite(boostPin, SW ? HIGH : LOW);   
}

//=========Функция пропорционального пересчета параметра(конец)================
uint16_t calc_proportion(const uint16_t multiplier1, const uint16_t multiplier2, const uint32_t divider) {
  uint32_t p;
  p = (1+(2 * (uint32_t)multiplier1 * (uint32_t)multiplier2)/divider)/2;
  if (p > 0xFFFF) {	  p = 0xFFFF;  }
  return (uint16_t)p;
} 

void setPower(int16_t newPower) {
        newPower = constrain(newPower, 0, 10000);  // Ограничиваем мощность в пределах 0-10000 Вт
        if (boostMode) boost(OFF); // Если был разгон отключаем
        powerSetpoint = newPower;
        heat(newPower != 0); // Включаем нагрев если уставка мощности больше 0, иначе выключаем 
        if (stabilizeMode==1) { // Если стабилизация по напряжению
        if (powerSetpoint < maxPower) PowerLevel_ust = calc_proportion(powerSetpoint, MaxPowerLevel, maxPower);
        else PowerLevel_ust = MaxPowerLevel; // Останемся в пределах диапазона
         for (int i = 0; i < powerHistorySize; i++) {powerHistory[i]=powerSetpoint;}
        }  
}

// Инициализация режима из EEPROM
void initModeFromEEPROM() {
  EEPROM.begin(EEPROM_SIZE);
    if (EEPROM.read(MODE_SAVED_FLAG_ADDR) == MODE_SAVED_FLAG_VALUE) {
    mode = EEPROM.read(MODE_ADDR);
    EEPROM.get(TIME_REG_ADDR, timeReg);//free
    EEPROM.get(K_UPR_ADDR, K_upr);
    EEPROM.get(PWM_FREQ_ADDR, pwmFreq);
    EEPROM.get(DELAY_LOW_ADDR, delayLow);
    EEPROM.get(DELAY_HIGH_ADDR, delayHigh);
    EEPROM.get(PMAX_TEN_ADDR, maxPower);
    stabilizeMode = EEPROM.read(STABILIZE_MODE_ADDR);
    filterEnabled = EEPROM.read(FILTER_ENABLED_ADDR) == 1;
    EEPROM.get(DELAY_TIME_DISPLACEMENT_ADDR, delayTimeDisplacement);
    // Проверяем допустимые пределы:
    if (maxPower < 1 || maxPower > 10000) maxPower = PMAX_TEN;//
    if (timeReg < 1 || timeReg > 8) timeReg = DEFAULT_TIME_REG;//
    if (K_upr < 1 || K_upr > 10) K_upr = DEFAULT_K_UPR;
    if (pwmFreq < 100 || pwmFreq > 10000) pwmFreq = DEFAULT_PWM_FREQ;
    if (delayLow < 100 || delayLow > 1500) delayLow = DEFAULT_DELAY_LOW;
    if (delayHigh < 7000 || delayHigh > 10200) delayHigh = DEFAULT_DELAY_HIGH;
    if (delayTimeDisplacement < -1500 || delayTimeDisplacement > 1500) delayTimeDisplacement = DELAY_TIME_DISPLACEMENT;
  }
  EEPROM.end();
}

void calculateDelayTime() {
    // Нормализованное значение мощности (0..1)
    float powerRatio = (float)PowerLevel / MaxPowerLevel;

    // Общая площадь под полуволной синусоиды
    const float totalArea = 6366.1977f; // 20000 / π

    // Площадь для одного уровня мощности
    //const float areaPerLevel = totalArea / 1024.0f;

    // Вычисляем площадь для текущего уровня мощности
    float area = powerRatio * totalArea;

    // Решаем уравнение для нахождения t
    float t = (10000.0f / PI) * acos(1.0f - (PI * area) / 10000.0f);

    // Преобразуем t в задержку (микросекунды)
    // Задержка должна изменяться от большего к меньшему
    delayTime = 10000 - (unsigned long)t;

    // Ограничиваем задержку в пределах delayLow и delayHigh
    delayTime = constrain(delayTime, delayLow, delayHigh);

    // Корректируем крайние значения
    if (PowerLevel == 0) {
        delayTime = delayHigh; // Максимальная задержка
    }
    if (PowerLevel == 1023) {
        delayTime = delayLow; // Минимальная задержка
    }
    delayTime += delayTimeDisplacement; // учитываем смещение
    #ifdef REGDEBUG
    Serial.println("delayTime:" + String(delayTime));
    //Serial.println("delayTimeAlt:" + String(map(PowerLevel, 0, MaxPowerLevel, delayHigh, delayLow)));
    #endif
}

// Глобальные переменные для управления временем и состоянием
static unsigned long lastPowerLevelChange = 0;  // Время последнего изменения PowerLevel
static bool initialAdjustmentDone = false;     // Флаг завершения начальной коррекции
static float filteredPower = 0;                // Фильтрованное значение мощности

// Адаптивный фильтр бегущего среднего
float Filtr(float newVal) {
    static float filVal = 0;
    float k;
      // Резкость фильтра зависит от отклонения
    if (abs(newVal - filVal) > maxPower/10) k = 0.9;  // Быстрая реакция на большие изменения
    else k = 0.2;                             // Плавная коррекция малых отклонений
    filVal += (newVal - filVal) * k;
    return filVal;
}
// Функция для обновления среднего значения мощности
void updateAveragePower() {

    powerHistory[powerHistoryIndex] = power; // Добавляем текущее значение мощности
    powerHistoryIndex = (powerHistoryIndex + 1) % powerHistorySize; // Обновляем индекс

    // Вычисляем среднее значение мощности
    float sum = 0;
    for (int i = 0; i < powerHistorySize; i++) {
        sum += powerHistory[i];
    }
    averagePower = sum / powerHistorySize;
    power = round(averagePower);
}
void updatePowerLevel() { // Регулятор
    static int16_t lastPowerSetpoint = 0;
  if (REG) { // если регулятор включен
    if (stabilizeMode==0) { // Если стабилизация по мощности  
        if (!boostMode && heatEnabled) { // если не разгон и включен нагрев
          // Если текущая мощность превышает номинал, меняем номинал, а если при этом еще и максимум регулирования считаем его истинным.
          if (power > maxPower) {
            if (PowerLevel != 0) {
              if (PowerLevel<1023) maxPower=round(power*MaxPowerLevel/PowerLevel); // Попытка рассчитать номинал
              else { maxPower = power; TrueMAXPowerTEN = true; }
            }
          } 
          // Если задание изменилось - выполнить грубую коррекцию
          if (powerSetpoint != lastPowerSetpoint) {
            if (maxPower == 0) {maxPower = 1; PowerLevel = 1023; return;} // Не знаем мощность ТЭНа - прогружаем его
                  // Расчет начального PowerLevel для приближения к powerSetpoint
            float ratio = (float)powerSetpoint / maxPower;
            PowerLevel = constrain(ratio * MaxPowerLevel, 0, MaxPowerLevel);
            //constrain(PowerLevel+(((powerSetpoint-power)*MaxPowerLevel)/maxPower), 0, MaxPowerLevel);// *K_upr  /10
                  // Сброс таймеров
            lastPowerLevelChange = millis();
            initialAdjustmentDone = false;
            lastPowerSetpoint = powerSetpoint;
            #ifdef REGDEBUG
            Serial.println(" Jump.");   
            #endif 
            return;
          }
          
          #ifdef REGDEBUG 
          Serial.print("Power = "+String(power));
          #endif 
          // Обновление фильтра каждую секунду
          if (filterEnabled){
            power = (int) Filtr((float) power); 
            #ifdef REGDEBUG  
            Serial.print(", FPower = "+String(power));
            #endif
          }          
          // Если с последнего изменения прошло меньше timeReg сек - не корректировать
          if (millis() - lastPowerLevelChange < timeReg*1000 && initialAdjustmentDone) {
            #ifdef REGDEBUG  
            Serial.println(" Прошло меньше timeReg мсек.");
            #endif
            return;
          }

    // Логика коррекции
    float error = abs(power - powerSetpoint);
    float threshold = maxPower * 0.04; // 5% от номинала
    if (error > threshold) {
        // Агрессивная коррекция при большом отклонении
        float ratio = (float)powerSetpoint / maxPower;

        PowerLevel = constrain(PowerLevel+(((powerSetpoint-power)*MaxPowerLevel*K_upr)/maxPower)/10, 0, MaxPowerLevel);
        lastPowerLevelChange = millis();
        #ifdef REGDEBUG
        Serial.println(" PowerLevel ="+String(PowerLevel)); 
        Serial.println(" Агрессивная коррекция.");
        #endif 
    } else {
        // Плавная коррекция ±1
        if (power < powerSetpoint) PowerLevel += 1;
        else if (power > powerSetpoint) PowerLevel -= 1;
        PowerLevel = constrain(PowerLevel, 0, MaxPowerLevel);
        lastPowerLevelChange = millis();
        #ifdef REGDEBUG 
        Serial.println(" Плавная коррекция ±1.");
        #endif 
    }
    
    initialAdjustmentDone = true;
    }
    if (heatEnabled) { // В режиме разгона полностью открываем симистор
      if (boostMode) PowerLevel = MaxPowerLevel;
    } else {           // А если нагрев вдруг отключен закрываем  
      PowerLevel = 0;
    }

    } else {
      PowerLevel = calc_proportion(PowerLevel_ust, 230*230, (uint32_t) voltage*voltage);
      if (PowerLevel > MaxPowerLevel) PowerLevel = MaxPowerLevel; 
      updateAveragePower();
    }// -стабилизация по мощности/напряжению
      } else { //REG
    PowerLevel = constrain(powerSetpoint, 0, MaxPowerLevel); // Если регулятор выведен просто меняем его установку от 0 до 1023
  } 
  
}

// Обработчик прерывания по детектору нуля
void IRAM_ATTR zeroCrossISR() {
    unsigned long currentTime = micros();
  if (currentTime - lastZeroCrossTime > debounceDelay) {
  zeroCrossDetected = true;
  zeroCrossTime = currentTime;
  lastZeroCrossTime = currentTime;
  #ifdef ZCDEBUG
  zeroCrossCount++;
  #endif
    if (mode==1) {
        // Логика управления SSR-40DA c сортировкой полуволн
    if (boostMode) {
        digitalWrite(triacPin, HIGH);  // В режиме разгона SSR всегда открыт
    } else {
        int32_t lev = PowerLevel + PowerLevel_err;  // Текущий уровень с учетом ошибки
        if (lev >= MaxPowerLevel / 2 && (ps == 0 || (zeroCrossDetected && ps < 0) || (!zeroCrossDetected && ps > 0))) {
            digitalWrite(triacPin, HIGH);  // Включаем SSR
            ssrTriggered = true;
            PowerLevel_err = lev - MaxPowerLevel;  // Считаем ошибку для следующего полупериода
        } else {
            digitalWrite(triacPin, LOW);  // Выключаем SSR
            ssrTriggered = false;
            PowerLevel_err = lev;  // Считаем ошибку
        }
    }
   }

  }
}
// Обработчик прерывания по таймеру для открытия семистора
void IRAM_ATTR timer1ISR() {
  if (mode==2) { //фазовое регулирование симистором
   if (zeroCrossDetected && !triacTriggered) {
      unsigned long currentTime = micros();
      if (currentTime - zeroCrossTime >= delayTime) {
        digitalWrite(triacPin, HIGH);  // Включаем симистор
        triacTriggered = true;
        triacTurnOffTime = currentTime + 100;  // Устанавливаем время выключения
        zeroCrossDetected = false; //Закомментировать чтоб видеть на осциллографе длину открытых полупериодов
      }
    }
    // Выключаем симистор через 100 микросекунд
    if (triacTriggered && micros() >= triacTurnOffTime) {
      digitalWrite(triacPin, LOW);
      triacTriggered = false;
    }
  }
}
#ifdef ZCDEBUG
void onTimer() {
  // Вычисляем количество переходов через ноль в 10 секунд
  zeroCrossPerSecond = zeroCrossCount - lastZeroCrossCount;
  lastZeroCrossCount = zeroCrossCount;

  // Выводим в Serial
  Serial.println("Z cr/10s:" + String(zeroCrossPerSecond));
}
#endif
void modeSwap() { //Смена режима регулятора на лету
  detachInterrupt(digitalPinToInterrupt(zeroCrossPin)); // Отмена прерывания на пин пересечения с 0
  analogWrite(triacPin, 0);                             // Отключаем ШИМ если вдруг был
  switch (mode) {
    case (0): // ШИМ
      timer1_disable();
      analogWriteResolution(10);//бит   //Регулятор ШИМ 0-1023
      analogWriteFreq(pwmFreq);         // Гц Установка частоты ШИМ
      Serial.println("Режим ШИМ");
      break;
    case (1): // Сортировка полуволн
      attachInterrupt(digitalPinToInterrupt(zeroCrossPin), zeroCrossISR, FALLING ); // Настройка прерывания по детектору нуля
      timer1_disable();
      Serial.println("Режим Сортировка полуволн");
      break;
    case (2): // Фазовое
      attachInterrupt(digitalPinToInterrupt(zeroCrossPin), zeroCrossISR, FALLING ); // Настройка прерывания по детектору нуля
      interrTriacFlag = heatEnabled;
      Serial.println("Режим Фазовое регулирование");
      break;
  }
}

void zeroCrossInit() {
   pinMode(zeroCrossPin, INPUT);
   pinMode(triacPin, OUTPUT);
   digitalWrite(triacPin, LOW);

   initModeFromEEPROM();
   modeSwap();                         // Включение режима регулятора
   timer1_attachInterrupt(timer1ISR);  // Настройка таймера для проверки задержки
   timer1_write(500);  // Прерывание каждые 0.1 мс (50000 тактов при 5 МГц)
   #ifdef ZCDEBUG
   Serial.println("Каждых 10 сек подсчет пересечений с нулем, должно быть около 1000");
   timer.attach(10.0, onTimer); // Период 10 секунд вызов подсчета пересечений с 0
   #endif
   Serial.println("Работа..."); Serial.println();
}

void triakLoop(){ //Раз в период регулирования
 updatePowerLevel(); //Пересчитываем уровень регулирования
 if (mode==2) { // В фазовом режиме 
                                                //если нагрев отключен прерывания отключаем
 if (heatEnabled && interrTriacFlag) {timer1_enable(TIM_DIV16, TIM_EDGE, TIM_LOOP);  interrTriacFlag=false;}// Таймер с частотой 5 МГц (80 МГц / 16)
 if (!heatEnabled && !interrTriacFlag) {timer1_disable(); interrTriacFlag=true; digitalWrite(triacPin, LOW);}//*/
 calculateDelayTime(); // Пересчитываем актуальную задержку открытия семистора раз в секунду
  }
   if (!mode) analogWrite(triacPin, PowerLevel); // Обрабатываем режим ШИМ
}