/*
 * peripherial.c
 *
 *  Created on: Sep 10, 2024
 *      Author: jakubski
 */

#include <stdlib.h>

#include "peripherial.h"

void DbgLEDOn (uint8_t ledNumber) {
    HAL_GPIO_WritePin (GPIOD, ledNumber, GPIO_PIN_SET);
}

void DbgLEDOff (uint8_t ledNumber) {
    HAL_GPIO_WritePin (GPIOD, ledNumber, GPIO_PIN_RESET);
}

void DbgLEDToggle (uint8_t ledNumber) {
    HAL_GPIO_TogglePin (GPIOD, ledNumber);
}

void EnableCurrentSensors (void) {
    HAL_GPIO_WritePin (GPIOE, MCU_CS_PWR_EN, GPIO_PIN_SET);
}

void DisableCurrentSensors (void) {
    HAL_GPIO_WritePin (GPIOE, MCU_CS_PWR_EN, GPIO_PIN_RESET);
}

void SelectCurrentSensorGain (CurrentSensor sensor, CurrentSensorGain gain) {
    uint8_t gpioOffset = 0;
    switch (sensor) {
    case CurrentSensorL1: gpioOffset = CURRENT_SENSOR_L1_GPIO_OFFSET; break;
    case CurrentSensorL2: gpioOffset = CURRENT_SENSOR_L2_GPIO_OFFSET; break;
    case CurrentSensorL3: gpioOffset = CURRENT_SENSOR_L3_GPIO_OFFSET; break;
    default: break;
    }
    if (gpioOffset > 0) {
        uint16_t gain0Gpio = 1 << gpioOffset;
        uint16_t gain1Gpio = 1 << (gpioOffset + 1);
        uint16_t gpioState = ((uint16_t)gain) & 0x0001;
        HAL_GPIO_WritePin (GPIOE, gain0Gpio, gpioState);
        gpioState = (((uint16_t)gain) >> 1) & 0x0001;
        HAL_GPIO_WritePin (GPIOE, gain1Gpio, gpioState);
    }
}

uint8_t
motorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, uint8_t channel1, uint8_t channel2, osTimerId_t motorTimerHandle, int32_t motorPWMPulse, int32_t motorTimerPeriod, uint8_t switchLimiterUpStat, uint8_t switchLimiterDownStat) {
    uint32_t motorStatus            = 0;
    MotorDriverState setMotorYState = HiZ;

    HAL_TIM_PWM_Stop (htim, channel1);
    HAL_TIM_PWM_Stop (htim, channel2);
    if (motorTimerPeriod > 0) {
        if (motorPWMPulse > 0) {
            // Forward
            if (switchLimiterUpStat == 0) {
                setMotorYState = Forward;
                motorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorYState, abs (motorPWMPulse) * 10);
                HAL_TIM_PWM_Start (htim, channel1);
                motorStatus = 1;
            } else {
                HAL_TIM_PWM_Stop (htim, channel1);
            }
            HAL_TIM_PWM_Stop (htim, channel2);
        } else if (motorPWMPulse < 0) {
            // Reverse
            if (switchLimiterDownStat == 0) {
                setMotorYState = Reverse;
                motorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorYState, abs (motorPWMPulse) * 10);
                HAL_TIM_PWM_Start (htim, channel2);
                motorStatus = 1;
            } else {
                HAL_TIM_PWM_Stop (htim, channel2);
            }
            HAL_TIM_PWM_Stop (htim, channel1);
        } else {
            // Brake
            setMotorYState = Brake;
            motorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorYState, abs (motorPWMPulse) * 10);
            HAL_TIM_PWM_Start (htim, channel1);
            HAL_TIM_PWM_Start (htim, channel2);
            motorStatus = 0;
        }
        osTimerStart (motorTimerHandle, motorTimerPeriod * 1000);
    } else if ((motorTimerPeriod == 0) && (motorPWMPulse == 0)) {
        motorAction (htim, motorTimerConfigOC, channel1, channel2, HiZ, abs (motorPWMPulse) * 10);
        HAL_TIM_PWM_Stop (htim, channel1);
        HAL_TIM_PWM_Stop (htim, channel2);
        osTimerStop (motorTimerHandle);
        motorStatus = 0;
    } else if (motorTimerPeriod == -1) {
        if (motorPWMPulse > 0) {
            // Forward
            if (switchLimiterUpStat == 0) {
                setMotorYState = Forward;
                motorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorYState, abs (motorPWMPulse) * 10);
                HAL_TIM_PWM_Start (htim, channel1);
                motorStatus = 1;
            } else {
                HAL_TIM_PWM_Stop (htim, channel1);
            }
            HAL_TIM_PWM_Stop (htim, channel2);
        } else {
            // Reverse
            if (switchLimiterDownStat == 0) {
                setMotorYState = Reverse;
                motorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorYState, abs (motorPWMPulse) * 10);
                HAL_TIM_PWM_Start (htim, channel2);
                motorStatus = 1;
            } else {
                HAL_TIM_PWM_Stop (htim, channel2);
            }
            HAL_TIM_PWM_Stop (htim, channel1);
        }
    }
    return motorStatus;
}

void motorAction (TIM_HandleTypeDef* tim, TIM_OC_InitTypeDef* timerConf, uint32_t channel1, uint32_t channel2, MotorDriverState setState, uint32_t pulse) {
    timerConf->Pulse = pulse;
    switch (setState) {
    case Forward:
    case Reverse:
    case HiZ:
        timerConf->OCPolarity = TIM_OCPOLARITY_HIGH;
        if (HAL_TIM_PWM_ConfigChannel (tim, timerConf, channel1) != HAL_OK) {
            Error_Handler ();
        }
        timerConf->OCPolarity = TIM_OCPOLARITY_HIGH;
        if (HAL_TIM_PWM_ConfigChannel (tim, timerConf, channel2) != HAL_OK) {
            Error_Handler ();
        }
        break;
    case Brake:
        timerConf->OCPolarity = TIM_OCPOLARITY_LOW;
        if (HAL_TIM_PWM_ConfigChannel (tim, timerConf, channel1) != HAL_OK) {
            Error_Handler ();
        }
        timerConf->OCPolarity = TIM_OCPOLARITY_LOW;
        if (HAL_TIM_PWM_ConfigChannel (tim, timerConf, channel2) != HAL_OK) {
            Error_Handler ();
        }
        break;
    default:
        timerConf->OCPolarity = TIM_OCPOLARITY_HIGH;
        if (HAL_TIM_PWM_ConfigChannel (tim, timerConf, channel1) != HAL_OK) {
            Error_Handler ();
        }
        timerConf->OCPolarity = TIM_OCPOLARITY_HIGH;
        if (HAL_TIM_PWM_ConfigChannel (tim, timerConf, channel2) != HAL_OK) {
            Error_Handler ();
        }
        break;
    }
}