#include "motor.hpp"
#include "pwm.hpp"

namespace motor {
  Motor::Motor(const MotorPins &pins, float freq, DecayMode mode)
    : pins(pins), pwm_frequency(freq), motor_decay_mode(mode) {
  }

  Motor::~Motor() {
    gpio_set_function(pins.positive, GPIO_FUNC_NULL);
    gpio_set_function(pins.negative, GPIO_FUNC_NULL);
  }

  bool Motor::init() {
    bool success = false;

    uint16_t period; uint16_t div16;
    if(pimoroni::calculate_pwm_factors(pwm_frequency, period, div16)) {
      pwm_period = period;

      pwm_cfg = pwm_get_default_config();

      //Set the new wrap (should be 1 less than the period to get full 0 to 100%)
      pwm_config_set_wrap(&pwm_cfg, period - 1);

      //Apply the divider
      pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f);

      pwm_init(pwm_gpio_to_slice_num(pins.positive), &pwm_cfg, true);
      gpio_set_function(pins.positive, GPIO_FUNC_PWM);

      pwm_init(pwm_gpio_to_slice_num(pins.negative), &pwm_cfg, true);
      gpio_set_function(pins.negative, GPIO_FUNC_PWM);
      update_pwm();

      success = true;
    }
    return success;
  }

  float Motor::get_speed() {
    return motor_speed;
  }

  void Motor::set_speed(float speed) {
    motor_speed = MIN(MAX(speed, -1.0f), 1.0f);
    update_pwm();
  }

  float Motor::get_frequency() {
    return pwm_frequency;
  }

  bool Motor::set_frequency(float freq) {
    bool success = false;

    //Calculate a suitable pwm wrap period for this frequency
    uint16_t period; uint16_t div16;
    if(pimoroni::calculate_pwm_factors(freq, period, div16)) {

      //Record if the new period will be larger or smaller.
      //This is used to apply new pwm values either before or after the wrap is applied,
      //to avoid momentary blips in PWM output on SLOW_DECAY
      bool pre_update_pwm = (period > pwm_period);

      pwm_period = period;
      pwm_frequency = freq;

      uint pos_num = pwm_gpio_to_slice_num(pins.positive);
      uint neg_num = pwm_gpio_to_slice_num(pins.negative);

      //Apply the new divider
      uint8_t div = div16 >> 4;
      uint8_t mod = div16 % 16;
      pwm_set_clkdiv_int_frac(pos_num, div, mod);
      if(neg_num != pos_num) {
        pwm_set_clkdiv_int_frac(neg_num, div, mod);
      }

      //If the the period is larger, update the pwm before setting the new wraps
      if(pre_update_pwm)
        update_pwm();

      //Set the new wrap (should be 1 less than the period to get full 0 to 100%)
      pwm_set_wrap(pos_num, pwm_period - 1);
      if(neg_num != pos_num) {
        pwm_set_wrap(neg_num, pwm_period - 1);
      }

      //If the the period is smaller, update the pwm after setting the new wraps
      if(!pre_update_pwm)
        update_pwm();

      success = true;
    }
    return success;
  }

  Motor::DecayMode Motor::get_decay_mode() {
    return motor_decay_mode;
  }

  void Motor::set_decay_mode(Motor::DecayMode mode) {
    motor_decay_mode = mode;
    update_pwm();
  }

  void Motor::stop() {
    motor_speed = 0.0f;
    update_pwm();
  }

  void Motor::disable() {
    motor_speed = 0.0f;
    pwm_set_gpio_level(pins.positive, 0);
    pwm_set_gpio_level(pins.negative, 0);
  }

  void Motor::update_pwm() {
    int32_t signed_duty_cycle = (int32_t)(motor_speed * (float)pwm_period);

    switch(motor_decay_mode) {
    case SLOW_DECAY: //aka 'Braking'
      if(signed_duty_cycle >= 0) {
        pwm_set_gpio_level(pins.positive, pwm_period);
        pwm_set_gpio_level(pins.negative, pwm_period - signed_duty_cycle);
      }
      else {
        pwm_set_gpio_level(pins.positive, pwm_period + signed_duty_cycle);
        pwm_set_gpio_level(pins.negative, pwm_period);
      }
      break;

    case FAST_DECAY: //aka 'Coasting'
    default:
      if(signed_duty_cycle >= 0) {
        pwm_set_gpio_level(pins.positive, signed_duty_cycle);
        pwm_set_gpio_level(pins.negative, 0);
      }
      else {
        pwm_set_gpio_level(pins.positive, 0);
        pwm_set_gpio_level(pins.negative, 0 - signed_duty_cycle);
      }
      break;
    }
  }
};