#include "InfraredLight.h"

#define pwmSpeedMode LEDC_LOW_SPEED_MODE
#define IR_FRONT_PIN 14
#define IR_BOTTOM_PIN 13
#define DUTY_RESOLUTION LEDC_TIMER_10_BIT
#define DUTY_CYCLE_FREQUENCY 512

InfraredLED::InfraredLED(uint8_t pin,ledc_timer_t timer, ledc_channel_t channel){
    this->ledPin = pin;
    this->timer = timer;
    this->channel = channel;
};

void InfraredLED::begin(void){
   //we want to change frequency instead of 
    pwmTimer = ledc_timer_config_t{
        .speed_mode = pwmSpeedMode,
        .duty_resolution = DUTY_RESOLUTION,
        .timer_num = this->timer,
        .freq_hz = 800,
        .clk_cfg = LEDC_AUTO_CLK
    };
    ledc_timer_config(&pwmTimer);

    pwmChannel = ledc_channel_config_t{
        .gpio_num = this->ledPin,
        .speed_mode =pwmSpeedMode,
        .channel = this->channel,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = this->timer,
        .duty = 0,
        .hpoint = 0
    };
    ledc_channel_config(&pwmChannel);
};

void InfraredLED::turnOn(void){
    InfraredLED::setState(true);
};

void InfraredLED::turnOff(void){
    InfraredLED::setState(false);
};

void InfraredLED::setState(bool state){
    ledc_set_freq(pwmSpeedMode,timer,1);
    if (state) {
      if (this->ledPin == IR_BOTTOM_PIN) {
        if (!Power::waitForCurrentAllowance(
                PowerParameters::PowerConsumers::LED_IR_BOTTOM,
                PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM,
                IR_LED_MAX_EXECUTION_DELAY_MS, NULL)) {
          ESP_LOGE(TAG,
                   "Could not get power for Bottom IR LED. Not turning on.");
          return;
        }
      } else if (this->ledPin == IR_FRONT_PIN) {
        if (!Power::waitForCurrentAllowance(
                PowerParameters::PowerConsumers::LED_IR_FRONT,
                PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT,
                IR_LED_MAX_EXECUTION_DELAY_MS, NULL)) {
          ESP_LOGE(TAG,
                   "Could not get power for Front IR LED. Not turning on.");
          return;
        }
      }
        ledc_set_duty(pwmSpeedMode,channel,1023);
    } else {
      if (this->ledPin == IR_BOTTOM_PIN) {
        Power::releaseCurrent(PowerParameters::PowerConsumers::LED_IR_BOTTOM);
      } else {
        Power::releaseCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT);
      }
        ledc_set_duty(pwmSpeedMode,channel,0);
    }
    ledc_update_duty(pwmSpeedMode,channel);
    
};

void InfraredLED::sendFrequency(uint16_t frequency){
  constexpr uint32_t duty = DUTY_CYCLE_FREQUENCY;
  // Float to force float division without casting
  constexpr float resolution = 1 << DUTY_RESOLUTION;
  if (this->ledPin == IR_BOTTOM_PIN) {
    Power::waitForCurrentAllowance(
        PowerParameters::PowerConsumers::LED_IR_BOTTOM,
        this->modelCurrentConsumption(duty), IR_LED_MAX_EXECUTION_DELAY_MS,
        NULL);
  } else if (this->ledPin == IR_FRONT_PIN) {
    Power::waitForCurrentAllowance(
        PowerParameters::PowerConsumers::LED_IR_FRONT,
        this->modelCurrentConsumption(duty), IR_LED_MAX_EXECUTION_DELAY_MS,
        NULL);
  }
    ledc_set_freq(pwmSpeedMode,timer,frequency);
    ledc_set_duty(pwmSpeedMode,channel,duty);
    ledc_update_duty(pwmSpeedMode,channel);
};

float InfraredLED::modelCurrentConsumption(uint32_t duty){
    // Float to force float division without casting
    constexpr float max_value = 1 << DUTY_RESOLUTION;
    const float duty_factor = duty / max_value;
    if (this->ledPin == IR_BOTTOM_PIN) {
        return duty_factor * PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM;
    } else if (this->ledPin == IR_FRONT_PIN) {
        return duty_factor * PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT;
    }
    return NAN;
};

float InfraredLED::modelChargeConsumptionOn(uint16_t durationMs) {
  // Float to force float division without casting
  constexpr float resolution = 1 << DUTY_RESOLUTION;
  if (this->ledPin == IR_BOTTOM_PIN) {
    return durationMs *
           PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM * 10e6;
  } else if (this->ledPin == IR_FRONT_PIN) {
    return durationMs *
           PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT *  10e6;
  }
  return NAN;
}

float InfraredLED::modelChargeConsumptionSendFrequency(uint16_t durationMs) {
  // Float to force float division without casting

  return durationMs * this->modelCurrentConsumption(DUTY_CYCLE_FREQUENCY) *
         10e6;
}
