/**
* @file TMC2209_ATY.c
*
* @param Project DEVICE_GENERAL_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2023 MZ-ATY
*       - This code follows:
*           - MZ-ATY Various Contents Joint Statement -
*               <a href="https://mengze.top/MZ-ATY_VCJS">
*                        https://mengze.top/MZ-ATY_VCJS</a>
*           - CC 4.0 BY-NC-SA -
*               <a href="https://creativecommons.org/licenses/by-nc-sa/4.0/">
*                        https://creativecommons.org/licenses/by-nc-sa/4.0/</a>
*       - Your use will be deemed to have accepted the terms of this statement.
*
* @brief Familiar functions of SteppingMotor control
*
* @version
*       - 1_01_221231 > ATY
*           -# Preliminary version, first Release
*       - 1_02_230408 > ATY
*           -# Add State Machine and so
*       - 1_03_230426 > ATY
*           -# Change name "SteppingMotor_ATY" to "TMC2209_ATY"
*       - 1_01_240111 > ATY
*           -# add multy addr and channel
*           -# add lock
*       - 1_01_240112 > ATY
*           -# change name from MOTOR_STEP to TMC2209
********************************************************************************
*/

#ifndef __TMC2209_ATY_C
#define __TMC2209_ATY_C

#include "TMC2209_ATY.h"

/******************************* For user *************************************/

/******************************************************************************/

/**
 * @brief   set motor enable or disbale
 *
 * @param   enable 0 for disable, 1 for enable
 * @param   dev
 * @return  uint8_t
 */
uint8_t TMC2209_SetAble(uint8_t en, struct TMC2209_ATY_Dev* dev)
{
    __ATY_LOCK(dev);
    dev->motorEn = en;
    if(en){ dev->enSet(_ATY_HL_L); }
    else{ dev->enSet(_ATY_HL_H); }
    __ATY_UNLOCK(dev);
    return 0;
}

/**
 * @brief   set motor direction
 *
 * @param   dir 0 for forward, 1 for reversal
 * @param   dev
 * @return  uint8_t
 */
uint8_t TMC2209_SetDirection(uint8_t dir, struct TMC2209_ATY_Dev* dev)
{
    __ATY_LOCK(dev);
    dev->motorDir = dir;
    if(dir){ dev->dirSet(_ATY_HL_H); }
    else{ dev->dirSet(_ATY_HL_L); }
    __ATY_UNLOCK(dev);
    return 0;
}

/**
 * @brief   get TMC2209 diag pin voltage level
 *
 * @param   dev
 * @return  uint8_t 0 for low, 1 fo high
 * @return  uint8_t
 */
uint8_t TMC2209_GetDiagLevel(struct TMC2209_ATY_Dev* dev)
{
    __ATY_LOCK(dev);
    dev->motorDiag = dev->diagGet();
    __ATY_UNLOCK(dev);
    return dev->motorDiag;
}


/**
 * @brief   start motor with direction set
 *
 * @param   dir direction to move
 * @param   speed direction to move
 * @param   dev
 */
void TMC2209_Start(uint8_t dir, uint32_t speed, struct TMC2209_ATY_Dev* dev)
{
    TMC2209_SetDirection(dir, dev);
    TMC2209_SetAble(1, dev);
    dev->freqSet(speed);
}

/**
 * @brief   stop motor
 *
 * @param   dev
 */
void TMC2209_Stop(struct TMC2209_ATY_Dev* dev)
{
    // TMC2209_PARAM_SET(0, 0, 0, 0, 0, 0, dev);
    if(dev->motorEn == 0)
        return;
    else{
        dev->freqSet(0);
        TMC2209_SetAble(0, dev);
        dev->motorSpeedCounter = 0;
        dev->motorStopCounter = 0;
    }
}

/**
 * @brief   process motor error
 *
 * @param   dev
 * @note    put at 1ms cycle
 */
void MotorSelfCycle(struct TMC2209_ATY_Dev* dev)
{
    if(TMC2209_GetDiagLevel(dev)){
        if(dev->motorEn == 1)
            TMC2209_SetAble(0, dev);
        if(dev->motorStopCounter >= 10000)
            TMC2209_SetAble(1, dev);
    }
}


/**
 * @brief   deal step motor state
 *
 * @param   dev
 * @note    put at 1ms cycle;
 *          set motorSetState = 4 to change motor dir with speed no changed
 *          set motorSetState = 3 to start motor directly, motorSoftSteps need to be 0
 *          set motorSetState = 2 to strat motor with soft, motorDir/motorSoftSpeed/motorSpeed/motorSoftSteps/motorSteps need to be set
 *          set motorSetState = 0 to stop motor flow
 *          set motorSetState = 10 to scram motor
 *          when motorSetState = 5 means last cycle finished, set others to start a new cycle
 *          when motorSetState = 1 means motor running and state not changing
 */
void TMC2209_StateMachine_PWM(struct TMC2209_ATY_Dev* dev)
{
    if(dev->motorSoftSpeed < dev->motorSpeed)
        dev->motorSoftSpeed = dev->motorSpeed;

    if(dev->motorSetState == TMC2209_SET_REVERSE){
        dev->motorStopCounter = 0;
        dev->motorStepCounter = 0;
        dev->motorSetState = TMC2209_SET_START_SOFT;
        dev->motorDir = !dev->motorDir;
    }
    else if(dev->motorSetState == TMC2209_SET_START_DIRECTLY){
        dev->motorStepCounter = dev->motorSoftSteps;
        TMC2209_Start(dev->motorDir, dev->motorSpeed, dev);
        dev->motorSetState = TMC2209_SET_RUNNING;
    }
    else if(dev->motorSetState == TMC2209_SET_START_SOFT){
        dev->motorStepCounter = 0;
        TMC2209_Start(dev->motorDir, dev->motorSoftSpeed, dev);
        dev->motorSetState = TMC2209_SET_RUNNING;
    }
    else if(dev->motorSetState == TMC2209_SET_RUNNING){
        if(dev->motorStepCounter < dev->motorSoftSteps){
            if(dev->lineProType == 'T'){
                dev->speedIncreaseStep = ((float)(dev->motorSoftSpeed - dev->motorSpeed) / (float)dev->motorSoftSteps);  // at 1ms cycle
                TMC2209_Start(dev->motorDir,
                    dev->motorSpeed < (dev->motorSoftSpeed - (dev->speedIncreaseStep * dev->motorStepCounter)) ?
                    (dev->motorSoftSpeed - (dev->speedIncreaseStep * dev->motorStepCounter)) : dev->motorSpeed, dev);
            }
            else if(dev->lineProType == 'S'){
                // S line: y = ax^3 + cx, a in 0-1, c >= 0, lower c cause lower increase at start or end, c for users
                float S_param_A = (((float)(dev->motorSoftSpeed - dev->motorSpeed) / 2)
                    - (dev->lineProParam * (float)((float)dev->motorSoftSteps / 2) * (float)((float)dev->motorSoftSteps / 2) / 2))
                    / ((float)((float)dev->motorSoftSteps / 2) * (float)((float)dev->motorSoftSteps / 2)
                        * (float)((float)dev->motorSoftSteps / 2) * (float)((float)dev->motorSoftSteps / 2)) * 4;
                // mbG[27] = S_param_A * 10000000;
                if(dev->motorStepCounter < (dev->motorSoftSteps / 2))
                    dev->speedIncreaseStep =
                    ((float)(S_param_A * dev->motorStepCounter * dev->motorStepCounter * dev->motorStepCounter * dev->motorStepCounter) / 4)
                    + ((float)(dev->lineProParam * dev->motorStepCounter * dev->motorStepCounter) / 2);
                // (S_param_A * dev->motorStepCounter * dev->motorStepCounter * dev->motorStepCounter)
                // + (lineProParam * dev->motorStepCounter);
                else
                    dev->speedIncreaseStep =
                    (float)((float)(dev->motorSoftSpeed - dev->motorSpeed) / 2)
                    + ((float)(S_param_A * dev->motorSoftSteps * dev->motorSoftSteps * dev->motorSoftSteps * dev->motorSoftSteps) / 64)
                    + ((float)(dev->lineProParam * dev->motorSoftSteps * dev->motorSoftSteps) / 8)
                    - ((float)(S_param_A * (dev->motorSoftSteps - dev->motorStepCounter) * (dev->motorSoftSteps - dev->motorStepCounter)
                        * (dev->motorSoftSteps - dev->motorStepCounter) * (dev->motorSoftSteps - dev->motorStepCounter)) / 4)
                    - ((float)(dev->lineProParam * (dev->motorSoftSteps - dev->motorStepCounter) * (dev->motorSoftSteps - dev->motorStepCounter)) / 2);
                    // 0
                    // - (S_param_A * (float)((float)dev->motorStepCounter - (float)dev->motorSoftSteps)
                    //     * (float)((float)dev->motorStepCounter - (float)dev->motorSoftSteps)
                    //     * (float)((float)dev->motorStepCounter - (float)dev->motorSoftSteps))
                    // - (dev->lineProParam * (float)((float)dev->motorStepCounter - (float)dev->motorSoftSteps));
                TMC2209_Start(dev->motorDir,
                    dev->motorSpeed < (dev->motorSoftSpeed - dev->speedIncreaseStep) ?
                    (dev->motorSoftSpeed - dev->speedIncreaseStep) : dev->motorSpeed, dev);
            }
            else if(dev->lineProType == '2'){}
        }

        if(dev->motorStepCounter == dev->motorSoftSteps)
            dev->motorSetState = TMC2209_SET_START_DIRECTLY;
        else if(dev->motorStepCounter >= (dev->motorSoftSteps + dev->motorSteps))
            dev->motorSetState = TMC2209_SET_FINISHED;
    }
    else if(dev->motorSetState == TMC2209_SET_SCRAM){
        TMC2209_Stop(dev);
        dev->motorSetState = TMC2209_SET_STOP;
    }
    else{ TMC2209_Stop(dev); }    // 5 0

    if(dev->motorEn == 0){
        dev->motorStepCounter = 0;
        dev->motorStopCounter++;
    }
    else{
        dev->motorStepCounter++;
        dev->motorStopCounter = 0;
    }
}

#endif /* __TMC2209_ATY_C */

/************************************ etc *************************************/
/* init */
// void MOTOR_1_EN_SET(uint8_t level) {
//     if(level == _ATY_HL_L)
//         GPIO_SET_L(STEP_MOTOR_EN_GPIO_Port, STEP_MOTOR_EN_Pin);
//     else if(level == _ATY_HL_H)
//         GPIO_SET_H(STEP_MOTOR_EN_GPIO_Port, STEP_MOTOR_EN_Pin);
// }
// void MOTOR_1_DIR_SET(uint8_t level) {
//     if(level == _ATY_HL_L)
//         GPIO_SET_L(STEP_MOTOR_DIR_GPIO_Port, STEP_MOTOR_DIR_Pin);
//     else if(level == _ATY_HL_H)
//         GPIO_SET_H(STEP_MOTOR_DIR_GPIO_Port, STEP_MOTOR_DIR_Pin);
// }
// void MOTOR_1_DIAG_GET(void) { }
// void MOTOR_1_FREQ_SET(uint16_t freq) {
//     HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_3);
//     __HAL_TIM_SET_AUTORELOAD(&htim3, freq);
//     __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, (uint16_t)(freq * 0.5f));
//     HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
// }
// struct TMC2209_ATY_Dev TMC2209_ATY_Dev_1 = {
//     .id = 'M',

//     .motorEn = 0,
//     .motorDir = 0,
//     .motorStepLevel = 0,
//     .motorDiag = 0,

//     .motorSoftSpeed = 0,
//     .motorSpeed = 0,
//     .motorSpeedCounter = 0,

//     .motorSoftSteps = 0,
//     .motorSteps = 0,
//     .motorStepCounter = 0,

//     .motorStopCounter = 0,

//     .motorSetState = 0,

//     .speedIncreaseStep = 0,
//     .lineProType = 'T',
//     .lineProParam = TMC2209_S_PARAM_C,

//     .enSet = MOTOR_1_EN_SET,
//     .dirSet = MOTOR_1_DIR_SET,
//     .diagGet = MOTOR_1_DIAG_GET,

//     .freqSet = MOTOR_1_FREQ_SET,

//     .lock = _ATY_UNLOCKED,
//     .debugEnable = 0,
//     .LOG = printf
// };



// SetMotorDefault(&TMC2209_ATY_Dev_1){
//     TMC2209_PARAM_SET(
//         (uint8_t)mbP_TMC2209_DIR,
//         (uint32_t)mbP_TMC2209_SOFT_SPEED,
//         (uint32_t)mbP_TMC2209_RUN_SPEED,
//         (uint32_t)mbP_TMC2209_SOFT_TIME,
//         (uint32_t)mbP_TMC2209_RUN_TIME,
//         (uint8_t)mbP_TMC2209_SET_STATE,
//         dev);
//     HAL_GPIO_WritePin(STEP_MOTOR_DIR_GPIO_Port, STEP_MOTOR_DIR_Pin, GPIO_PIN_RESET);
//     HAL_GPIO_WritePin(STEP_MOTOR_EN_GPIO_Port, STEP_MOTOR_EN_Pin, GPIO_PIN_SET);
// }
// HAL_TIM_Base_Start(&htim3);
// __HAL_TIM_SET_AUTORELOAD(&htim3, STEP_MOTOR_PARAM_RUN_SPEED);
// __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 0);
// HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);

/* use */
// // in 1ms loop
// StepMotorProcess();
// TMC2209_StateMachine_PWM(&TMC2209_ATY_Dev_1);
// TMC2209_PARAM_GET(
//     mbP_TMC2209_DIR,
//     mbP_TMC2209_SOFT_SPEED,
//     mbP_TMC2209_RUN_SPEED,
//     mbP_TMC2209_SOFT_TIME,
//     mbP_TMC2209_RUN_TIME,
//     mbP_TMC2209_SET_STATE,
//     TMC2209_ATY_Dev_1);

// // uart change(in uart process)
// TMC2209_PARAM_SET(
//     (uint8_t)mbP_TMC2209_DIR,
//     (uint32_t)mbP_TMC2209_SOFT_SPEED,
//     (uint32_t)mbP_TMC2209_RUN_SPEED,
//     (uint32_t)mbP_TMC2209_SOFT_TIME,
//     (uint32_t)mbP_TMC2209_RUN_TIME,
//     (uint8_t)mbP_TMC2209_SET_STATE,
//     (&TMC2209_ATY_Dev_1));

// // set state or others to use
// if((uint8_t)mbP_TMC2209_FAST_CMD == STEP_MOTOR_CMD_SCRAM){
//     mbP_TMC2209_SET_STATE = TMC2209_SET_SCRAM;
// }
// else if((uint8_t)mbP_TMC2209_FAST_CMD == STEP_MOTOR_CMD_UP){
//     mbP_TMC2209_DIR = STEP_MOTOR_PARAM_DIR_UP;
//     mbP_TMC2209_SET_STATE = TMC2209_SET_START_SOFT;
// }
// else if((uint8_t)mbP_TMC2209_FAST_CMD == STEP_MOTOR_CMD_DOWN){
//     mbP_TMC2209_DIR = STEP_MOTOR_PARAM_DIR_DOWN;
//     mbP_TMC2209_SET_STATE = TMC2209_SET_START_SOFT;
// }

/******************************************************************************/

/******************************** End Of File *********************************/