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


const char* tag = "pwmout";

const float binaryPow[17] = {
    0, 2, 4, 8, 15, 31, 63, 127, 255, 511, 1023, 2047, 4095, 8191, 16383, 32767, 65535
};

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;
  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);
    return;
  }
  
  ledcAttachPin(pin, ch);
  setOutput(this->currDuty);
}

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){
  // 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, 2^res - 1]
  int maxVal = static_cast<int>(binaryPow[this->res]);
  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){
      this->freq = 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
  this->standardFactor = binaryPow[this->res] * 0.01f;  
  this->visionFactor = binaryPow[this->res] * 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);
}