#include <Arduino.h>

// -------------------------------------------------- 
// Estruturas, Enum e Variáveis Globais de Exemplo
// --------------------------------------------------

enum class CanalPWM : uint8_t {
  CANAL_A,
  CANAL_B,
  CANAL_C
};

struct {
  uint16_t posicao; // Setpoint (posição desejada, em valor de pressão)
} modoTrabalho;

struct ValvulaPWMState {
  float erroAnterior = 0.0f;
  float integral = 0.0f;
  uint32_t temporizador = 0;
  uint8_t dutyCycle = 0;
};

ValvulaPWMState valvulaStates[3];

// Parâmetros PWM
const uint8_t dutyMinimo = 0;
const uint8_t dutyMaximo = 255;

// Frequência desejada ~490Hz para Timer3 (pinos 2 e 3)
const uint16_t freqPWMValvula = 490;

// Ajuste do tempo mínimo e máximo de atualização (em ms)
const uint16_t tempoMin = 50;   
const uint16_t tempoMax = 500;  
const uint16_t maxDif = 500;

// Saídas globais para cada canal (para monitorar)
uint8_t dutyCycle1 = 0;
uint8_t dutyCycle2 = 0;
uint8_t dutyCycle3 = 0;

// Configurações do PID
float Kp = 0.2f;
float Ki = 0.3f;
float Kd = 0.01f;

// Função de Constrain (igual a do Arduino)
int16_t constrain_int16(int16_t x, int16_t a, int16_t b) {
  if (x < a) return a;
  if (x > b) return b;
  return x;
}

// --------------------------------------------------
// Configuração do Timer3 para ~490Hz nos pinos 2 e 3
// --------------------------------------------------
// Modo Fast PWM com ICR3 definindo TOP
// Frequência = F_CPU/(Prescaler*(1+ICR3))
// Queremos ~490 Hz: ICR3 ≈ 509 com prescaler = 64
// ICR3 = 509 => Frequência ≈ 490Hz

void configuraTimer3() {
  // Desabilita saída do timer para configurar
  TCCR3A = 0;
  TCCR3B = 0;

  // Modo Fast PWM com TOP em ICR3 (WGM = 14: WGM3=1110)
  // WGM31:0 no TCCR3A, WGM33:2 no TCCR3B
  // WGM33=1, WGM32=1, WGM31=1, WGM30=0 => binário 1110
  TCCR3A = (1 << WGM31);
  TCCR3B = (1 << WGM33) | (1 << WGM32) | (1 << CS31) | (1 << CS30); 
  // CS31 e CS30 => Prescaler = 64 (fórmula: 16MHz/64)
  
  // Define TOP
  ICR3 = 509; 

  // Configura pinos 2 (OC3B) e 3 (OC3C) como saída PWM não-invertido
  // Não-invertido: Clear OC3x on Compare Match, set at BOTTOM
  TCCR3A |= (1 << COM3B1) | (1 << COM3C1);

  // Inicialmente duty = 0
  OCR3B = 0; // pino 2
  OCR3C = 0; // pino 3
}

// --------------------------------------------------
// Função para configurar PWM em cada canal
// --------------------------------------------------
void configuraPWM(CanalPWM canal, uint16_t freq, uint8_t duty) {
  // Ajuste do dutyCycle de acordo com o canal
  // Canal A -> pino 2  (OC3B)
  // Canal B -> pino 3  (OC3C)
  // Canal C -> pino 4  (OC0B, Timer0)

  switch (canal) {
    case CanalPWM::CANAL_A:
      // Converte duty 0-255 para 0-ICR3
      OCR3B = map(duty, 0, 255, 0, ICR3);
      break;
    case CanalPWM::CANAL_B:
      OCR3C = map(duty, 0, 255, 0, ICR3);
      break;
    case CanalPWM::CANAL_C:
      // Para o pino 4 (OC0B), manteremos analogWrite padrão
      // Isso manterá ~490Hz aproximado (Timer0 ~976Hz default, mas pode ser levemente alterado)
      analogWrite(4, duty);
      break;
    default:
      break;
  }
}

// --------------------------------------------------
// Rotina de controleValvulaPWM_PID
// --------------------------------------------------
void controleValvulaPWM_PID(CanalPWM canal, uint16_t pressao)
{
    ValvulaPWMState &state = valvulaStates[(uint8_t)canal];

    // Garante que o dutyCycle inicial esteja dentro dos limites
    state.dutyCycle = constrain(state.dutyCycle, dutyMinimo, dutyMaximo);

    // Calcula a diferença absoluta entre a pressão atual e a posição desejada
    float prop = fminf((float)abs((int)pressao - (int)modoTrabalho.posicao) / (float)maxDif, 1.0f);

    // Calcula o tempo de atualização com base na diferença
    uint16_t tempoAtualizacao = (uint16_t)(tempoMax - (tempoMax - tempoMin) * prop);

    // Atualiza o PWM com base no temporizador dinâmico
    uint32_t agora = millis();
    if ((agora - state.temporizador) >= tempoAtualizacao) {
        float deltaTime = (agora - state.temporizador) / 1000.0f;
        state.temporizador = agora;

        float erro = (float)modoTrabalho.posicao - (float)pressao;

        // Integral com anti-windup
        if (state.dutyCycle > dutyMinimo && state.dutyCycle < dutyMaximo) {
            state.integral += erro * deltaTime;
            if (state.integral > 50.0f) state.integral = 50.0f;
            if (state.integral < -50.0f) state.integral = -50.0f;
        }

        float derivativo = (erro - state.erroAnterior) / (deltaTime > 0.0f ? deltaTime : 1.0f);
        state.erroAnterior = erro;

        float outputPID = (Kp * erro) + (Ki * state.integral) + (Kd * derivativo);

        int16_t novoDutyCycle = (int16_t)state.dutyCycle + (int16_t)(outputPID);
        novoDutyCycle = constrain_int16(novoDutyCycle, dutyMinimo, dutyMaximo);

        if (novoDutyCycle == dutyMinimo || novoDutyCycle == dutyMaximo) {
            state.integral = 0;
        }

        state.dutyCycle = (uint8_t)novoDutyCycle;

        switch (canal) {
            case CanalPWM::CANAL_A:
                dutyCycle1 = state.dutyCycle;
                break;
            case CanalPWM::CANAL_B:
                dutyCycle2 = state.dutyCycle;
                break;
            case CanalPWM::CANAL_C:
                dutyCycle3 = state.dutyCycle;
                break;
            default:
                break;
        }

        configuraPWM(canal, freqPWMValvula, state.dutyCycle);
    }
}

// --------------------------------------------------
// Setup e Loop Principal para Teste
// --------------------------------------------------

void setup() {
  Serial.begin(115200);

  // Setpoint
  modoTrabalho.posicao = 500;

  // Configura pinos de saída
  pinMode(2, OUTPUT);  // Canal A (OC3B)
  pinMode(3, OUTPUT);  // Canal B (OC3C)
  pinMode(4, OUTPUT);  // Canal C (OC0B)
  pinMode(5, OUTPUT);  // Dir
  pinMode(8, OUTPUT);  // Enable

  digitalWrite(5, HIGH);
  digitalWrite(8, HIGH);

  // Configura Timer3 para ~490Hz nos pinos 2 e 3
  configuraTimer3();

  // Inicia o dutyCycle em 50% (por exemplo)
  valvulaStates[(uint8_t)CanalPWM::CANAL_A].dutyCycle = 128;
  valvulaStates[(uint8_t)CanalPWM::CANAL_A].temporizador = millis();
}

void loop() {
  // Simulação de pressão (exemplo: onda senoidal)
  static uint32_t tempo = 0;
  tempo += 10;
  delay(10);

  float simPressao = 500.0f + 250.0f * sin(tempo / 1000.0f);

  // Chama a rotina PID para canal A
  controleValvulaPWM_PID(CanalPWM::CANAL_A, (uint16_t)simPressao);

  // Imprime valores para análise
  Serial.print("Tempo: ");
  Serial.print(tempo);
  Serial.print(" ms | Pressao: ");
  Serial.print(simPressao, 2);
  Serial.print(" | Setpoint: ");
  Serial.print(modoTrabalho.posicao);
  Serial.print(" | DutyCycle (A): ");
  Serial.println(dutyCycle1);
}