#include "PWM_Output.h"
#include <Arduino.h>
#include "global.h"


static const char* tag = "pwmout";

PWM_Output::PWM_Output(int8_t pin, uint8_t ch, uint8_t res, uint32_t freq, float maxDuty, bool visionCorrected){
  this->currDuty = 0;
  this->ch = ch;
  this->maxDuty = maxDuty;
  this->visionCorrected = visionCorrected;
  this->freq = freq;
  this->pin = pin;
  setResolution(res);
  pinMode(pin, OUTPUT);

  uint32_t actualFreq = ledcSetup(ch, freq, res);
  if (actualFreq != freq) {
    ESP_LOGE(tag, "pwmOut-> ch:%d ledcSetup failed! Requested freq: %d, Actual freq: %d", ch, freq, actualFreq);
    // continue but mark as uninitialized to prevent usage
    this->initialized = false;
    return;
  }
  
  ledcAttachPin(pin, ch);
  setOutput(this->currDuty);
  this->initialized = true;
}

void PWM_Output::setMaxDuty(float duty) {
  // add any validation or constraints here
  if(duty < 0) {
    duty = 0.0;
  }
  else if(duty > 100.0) {
      duty = 100.0;
  }
  maxDuty = duty;
}

// Range is 0 to 100%
void PWM_Output::setOutput(float duty){
  if(!this->initialized){
    ESP_LOGW(tag, "setOutput called on uninitialized PWM_Output (ch=%d)", this->ch);
    return;
  }
  // Clamp the duty cycle to the range [0.0, maxDuty]
  if (duty < 0.0) {
    duty = 0.0;
  } else if (duty > maxDuty) {
    duty = maxDuty;
  }

  // calculate correct duty value
  int outDutyVal;
  if(this->visionCorrected){
    outDutyVal = linearizeOutput(duty);
  } else {
    outDutyVal = static_cast<int>(duty * this->standardFactor);
  }

  // Clamp to valid resolution range [0, (1<<res)-1]
  int maxVal = (this->res >= 31) ? INT32_MAX : ((1 << this->res) - 1);
  if (outDutyVal < 0) outDutyVal = 0;
  if (outDutyVal > maxVal) outDutyVal = maxVal;

  ledcWrite(this->ch, outDutyVal); 
  this->currOutVal = outDutyVal;
  this->currDuty = duty;
}

void PWM_Output::setFreq(uint32_t fq){
  uint32_t newFreq;
  if(this->freq != fq){ 
    newFreq = ledcChangeFrequency(this->ch, fq, this->res);
    if(newFreq > 0){
      this->freq = newFreq;
    } else {
      ESP_LOGW(tag, "ledcChangeFrequency failed for ch:%d requested:%u", this->ch, fq);
    }
  }
}

void PWM_Output::setResolution(uint8_t res){
  this->res = res;
  if(this->res < 4) this->res = 4;
  if(this->res > 16) this->res = 16;

  // Use the clamped resolution when computing factors
  int maxVal = (this->res >= 31) ? INT32_MAX : ((1 << this->res) - 1);
  this->standardFactor = static_cast<float>(maxVal) * 0.01f;
  this->visionFactor = static_cast<float>(maxVal) * 0.0001f;
  ESP_LOGD(tag, "factor=%f, vision=%f", this->standardFactor, this->visionFactor);
}

int PWM_Output::linearizeOutput(float inp){
  if (inp < 0.0) {
      inp = 0.0;
  } else if (inp > 100.0) {
      inp = 100.0;
  }
  return static_cast<int>(inp * inp * this->visionFactor);
}

int PWM_Output::getOutVal(void){
  return ledcRead(this->ch);
}