#include "Motion.h"
#include "power/Power.h"

#define MOTOR_LEFT_PIN 12
#define MOTOR_RIGHT_PIN 11

Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel) {
  this->pin = pin;
  this->channel = channel;
  this->timer = timer;
  this->duty = 0;
};

void Motor::begin(void) {
  pinMode(this->pin, OUTPUT);
  ledc_channel_config_t channelConfig = {.gpio_num = this->pin,
                                         .speed_mode = LEDC_MODE,
                                         .channel = this->channel,
                                         .intr_type = LEDC_INTR_DISABLE,
                                         .timer_sel = this->timer,
                                         .duty = 0, // Set duty to 0%
                                         .hpoint = 0};
  ledc_channel_config(&channelConfig);
  Serial.println("Motor begin done");
};

bool Motor::setSpeed(uint16_t duty) {
  const float current = this->modelCurrentConsumption(duty);
  if (this->pin == MOTOR_LEFT_PIN) {
    if (!Power::waitForCurrentAllowance(
            PowerParameters::PowerConsumers::MOTOR_LEFT, current,
            MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
      ESP_LOGW(TAG,
               "Power to set LEFT MOTOR to speed %d not granted in time. "
               "Skipping.",
               duty);
      return false;
    }
  } else {
    if (!Power::waitForCurrentAllowance(
            PowerParameters::PowerConsumers::MOTOR_RIGHT, current,
            MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
      ESP_LOGW(TAG,
               "Power to set RIGHT MOTOR to speed %d not granted in time. "
               "Skipping.",
               duty);
      return false;
    }
  }
  int difference = duty - this->getSpeed();
  if (difference > 0) {
    for (int i = 0; i < difference; i += difference / 20) {
      this->duty += difference / 20;
      ledc_set_duty(LEDC_MODE, this->channel, duty);
      ledc_update_duty(LEDC_MODE, this->channel);
      delayMicroseconds(5);
    }
  } else {
    for (int i = 0; i > difference; i -= abs(difference / 20)) {
      this->duty -= abs(difference / 20);
      ledc_set_duty(LEDC_MODE, this->channel, duty);
      ledc_update_duty(LEDC_MODE, this->channel);
      delayMicroseconds(5);
    }
  }
  return true;
};

uint16_t Motor::getSpeed(void) { return this->duty; };

float modelCurrentConsumption(uint16_t duty) {
  const float dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
  return PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
}

float modelChargeConsumptionOn(uint16_t duty, uint16_t durationMs) {
  return modelCurrentConsumption(duty) * durationMs * 10e6;
}