/**
 * @file        motor.c
 */

/* Includes *******************************************************************/
#include "motor.h"
#include "Timer.h"
#include "stm32f4xx_hal.h"

/* Defines ********************************************************************/
#define MOTOR_ARR_STEP      100
#define MOTOR_FREQ_STEP     20
#define MOTOR_FREQ_MIN      100
#define MOTOR_RPM_MAX       900
#define MOTOR_RPS_MAX       60*MOTOR_RPM_MAX
#define MOTOR_STEPS_PER_ROT 200
#define MOTOR_MICROSTEP     16
#define MOTOR_PULLEY_RATIO  15
#define MOTOR_CLOCK         25000000ul
#define MOTOR_ARR_MAX       0xFFFF
#define MOTOR_ARR_MIN       0x01F4
#define MOTOR_TIME_STEP     50

/* Public variables ***********************************************************/

/* Private variables **********************************************************/
static uint16_t target_freq = 0;
static uint16_t current_freq = 0;
static uint16_t target_ARR = 0;
static uint16_t current_ARR = 0;
uint32_t motor_clock = 25000000ul;

/* Exported variables *********************************************************/
extern volatile uint16_t uiMOTORStepTimer;
extern volatile uint16_t uiMotorTimeout;

/* Constants ******************************************************************/
#define MSB16(a) ((uint8_t)(((a) & 0xFF00) >> 8))
#define LSB16(a) ((uint8_t)(((a) & 0x00FF) >> 0))

/* Private functions **********************************************************/

/* Public functions ***********************************************************/
/**
 * @brief   Motor_Init()
 *          Initialization of the motor module
 */
void Motor_Init(void)
{
    target_ARR = 0;
    current_ARR = 0;
    TIM3->CCR1 = 0u;
    TIM3->ARR = 0u;
}

/**
 * @brief   Motor_Handler()
 *          This functions must be called from the super-loop. It handles the
 *          motor speed.
 */
void Motor_Handler(void)
{
    uint16_t diff = 0;
    uint32_t tmp;
    /* Check communication timeout */
    if(target_freq > 0 && 0 == uiMotorTimeout) target_freq = 0;

    /* Check target rpm */
    if (0 == uiMOTORStepTimer) {
        if (target_freq != current_freq) {
            /* If motor is stopped, set minimum frequency */
            if (target_freq > current_freq) {
                diff = target_freq - current_freq;
                if (diff>MOTOR_FREQ_STEP) {
                    current_freq += MOTOR_FREQ_STEP;
                }
                else {
                    current_freq += diff;
                }
            }
            else {
                diff = current_freq - target_freq;
                if (diff>MOTOR_FREQ_STEP) {
                    current_freq -= MOTOR_FREQ_STEP;
                }
                else {
                    current_freq -= diff;
                }
            }
            if (0 == current_freq) {
                target_ARR = 0;
            }
            else {
                tmp = (uint32_t)(motor_clock/current_freq);
                if (tmp >MOTOR_ARR_MAX) {
                    target_ARR = MOTOR_ARR_MAX;
                }
                else if (tmp < MOTOR_ARR_MIN) {
                    target_ARR = MOTOR_ARR_MIN;
                }
                else {
                    target_ARR = (uint16_t)tmp;
                }
            }
            TIM3->CCR1 = (uint16_t)(target_ARR/2);
            TIM3->ARR = target_ARR;
        }
        uiMOTORStepTimer = MOTOR_TIME_STEP;
    }
}

void Motor_SetRPM(uint16_t input_rpm)
{
    uint32_t tmp;
    if (input_rpm < MOTOR_RPM_MAX+1) {
        tmp = (uint32_t)(input_rpm*MOTOR_STEPS_PER_ROT*MOTOR_MICROSTEP);
        target_freq = (uint16_t)(tmp/60);
    }
    else {
        target_freq = 0;
    }
}
uint16_t Motor_GetFreq(void)
{
    return current_freq;
}

void Motor_Stop(void)
{
    target_freq = 0;
}
