#include "PWM_control.h"
#include "state_machine.h"
#include "stm32f3xx_hal.h"
#include <stdint.h>

//uint32_t powerground1_CCR, powerground2_CCR, battery_cooling_CCR;

TIM_HandleTypeDef *powerground, *battery_cooling, *esc_cooling;

/*
  Pulse width modulation mode allows for generating a signal with a frequency determined by 
  the value of the TIMx_ARR register and a duty cycle determined by the value of the TIMx_CCRx register. 
*/

void PWM_control_init(TIM_HandleTypeDef* pg, TIM_HandleTypeDef* bat_cool, TIM_HandleTypeDef* esc_cool){
  powerground_status = 0;
  //battery_cooling_status = 0;
  
  powerground = pg;
  battery_cooling = bat_cool;
  esc_cooling = esc_cool;

  // htim2 CH3,4 BAT_COOLING_PWM,ENABLE
  // htim3 CH3,4 ESC_L_PWM,R_PWM
  // htim4 CH1,2,3 LED R,G,B
  // htim15 CH1,2 ESC_COOLING_ENABLE,PWM

  HAL_TIM_PWM_Start(powerground, TIM_CHANNEL_3);          //TIM3CH3
  HAL_TIM_PWM_Start(powerground, TIM_CHANNEL_4);          //TIM3CH4
  //HAL_TIM_PWM_Start(bat_cool, TIM_CHANNEL_3);     //TIM1CH3

  __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_1, 0);
  __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_2, 0);

  //PWM_powerground_control(0);
  //__HAL_TIM_SET_COMPARE(battery_cooling, TIM_CHANNEL_3, 2000);
}              

/*
  controls the duty cycle of the fans by setting the CCR of the channel percent/100 = x/ARR
  DUTYCYCLE = 40000 * X/100
*/
void PWM_powerground_control(uint8_t percent){
  if (percent > 100){ //something went wrong
    __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_3, 0);
    __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_4, 0);
    return;
  }
  powerground_status = percent;

  int ccr = 2000 + (20 * percent);
  __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_3, ccr);
  __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_4, ccr);
}

void PWM_set_throttle(){
  uint32_t timestamp = HAL_GetTick() + 5000;
  while (timestamp > HAL_GetTick()) {}

  __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_3, 4000);
  __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_4, 4000);
  timestamp = HAL_GetTick() + 2000;
  while (timestamp > HAL_GetTick()) {}

  __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_3, 2000);
  __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_4, 2000);
  timestamp = HAL_GetTick() + 1000;
  while (timestamp > HAL_GetTick()) {}
}

void PWM_battery_cooling_control(uint8_t percent){}
void PWM_esc_cooling(uint8_t percent){}