/**
* @file MOTOR_STEP_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 "MOTOR_STEP_ATY"
********************************************************************************
*/

#ifndef __MOTOR_STEP_ATY_C
#define __MOTOR_STEP_ATY_C

#include "MOTOR_STEP_ATY.h"

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

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

motorStep_t motorStep_t_Group[MOTOR_COUNT] =
{
#ifdef S_PARAM_C
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, S_PARAM_C},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, S_PARAM_C},
#else
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
#endif
};

/**
 * @brief   set motor enable or disbale
 * @param   enable 0 for disable, 1 for enable
 * @param   channel motor channel
 */
void MotorSetAble(uint8_t enable, uint8_t channel)
{
    motorStep_t_Group[channel].motorEn = enable;
#if (MOTOR_COUNT >= 1)
    if(channel == 0){
        if(enable){ GPIO_SET_L(MOTOR_EN_PORT_0, MOTOR_EN_PIN_0); }
        else{ GPIO_SET_H(MOTOR_EN_PORT_0, MOTOR_EN_PIN_0); }
    }
#endif
#if (MOTOR_COUNT >= 2)
    if(channel == 1){
        if(enable){ GPIO_SET_L(MOTOR_EN_PORT_1, MOTOR_EN_PIN_1); }
        else{ GPIO_SET_H(MOTOR_EN_PORT_1, MOTOR_EN_PIN_1); }
    }
#endif
#if (MOTOR_COUNT >= 3)
    if(channel == 2){
        if(enable){ GPIO_SET_L(MOTOR_EN_PORT_2, MOTOR_EN_PIN_2); }
        else{ GPIO_SET_H(MOTOR_EN_PORT_2, MOTOR_EN_PIN_2); }
    }
#endif
}

/**
 * @brief   set motor direction
 * @param   dir 0 for MOTOR_DIR_IN, 1 for MOTOR_DIR_OUT
 * @param   channel motor channel
 */
void MotorSetDirection(uint8_t dir, uint8_t channel)
{
    motorStep_t_Group[channel].motorDir = dir;
#if (MOTOR_COUNT >= 1)
    if(channel == 0){
        if(dir){ GPIO_SET_H(MOTOR_DIR_PORT_0, MOTOR_DIR_PIN_0); }
        else{ GPIO_SET_L(MOTOR_DIR_PORT_0, MOTOR_DIR_PIN_0); }
    }
#endif
#if (MOTOR_COUNT >= 2)
    if(channel == 1){
        if(dir){ GPIO_SET_H(MOTOR_DIR_PORT_1, MOTOR_DIR_PIN_1); }
        else{ GPIO_SET_L(MOTOR_DIR_PORT_1, MOTOR_DIR_PIN_1); }
    }
#endif
#if (MOTOR_COUNT >= 3)
    if(channel == 2){
        if(dir){ GPIO_SET_H(MOTOR_DIR_PORT_2, MOTOR_DIR_PIN_2); }
        else{ GPIO_SET_L(MOTOR_DIR_PORT_2, MOTOR_DIR_PIN_2); }
    }
#endif
}

/**
 * @brief   set motor step pin voltage level
 * @param   level 0 for low, 1 for high
 * @param   channel motor channel
 */
void MotorSetStepLevel(uint8_t level, uint8_t channel)
{
    motorStep_t_Group[channel].motorStepLevel = level;
#if (MOTOR_COUNT >= 1)
    if(channel == 0){
        if(level){ GPIO_SET_H(MOTOR_STEP_PORT_0, MOTOR_STEP_PIN_0); }
        else{ GPIO_SET_L(MOTOR_STEP_PORT_0, MOTOR_STEP_PIN_0); }
    }
#endif
#if (MOTOR_COUNT >= 2)
    if(channel == 1){
        if(level){ GPIO_SET_H(MOTOR_STEP_PORT_1, MOTOR_STEP_PIN_1); }
        else{ GPIO_SET_L(MOTOR_STEP_PORT_1, MOTOR_STEP_PIN_1); }
    }
#endif
#if (MOTOR_COUNT >= 3)
    if(channel == 2){
        if(level){ GPIO_SET_H(MOTOR_STEP_PORT_2, MOTOR_STEP_PIN_2); }
        else{ GPIO_SET_L(MOTOR_STEP_PORT_2, MOTOR_STEP_PIN_2); }
    }
#endif
}

/**
 * @brief   get TMC2209 diag pin voltage level
 * @param   channel motor channel
 * @return  uint8_t 0 for low, 1 fo high
 */
uint8_t MotorGetDiagLevel(uint8_t channel)
{
#if (MOTOR_COUNT >= 1)
    if(channel == 0){ motorStep_t_Group[channel].motorDiag = GPIO_GET(MOTOR_DIAG_PORT_0, MOTOR_DIAG_PIN_0); }
#endif
#if (MOTOR_COUNT >= 2)
    if(channel == 1){ motorStep_t_Group[channel].motorDiag = GPIO_GET(MOTOR_DIAG_PORT_1, MOTOR_DIAG_PIN_1); }
#endif
#if (MOTOR_COUNT >= 3)
    if(channel == 2){ motorStep_t_Group[channel].motorDiag = GPIO_GET(MOTOR_DIAG_PORT_2, MOTOR_DIAG_PIN_2); }
#endif
    return motorStep_t_Group[channel].motorDiag;
}


/**
 * @brief   start motor with direction set
 * @param   dir direction to move
 * @param   speed direction to move
 * @param   channel motor channel
 */
void MotorStart(uint8_t dir, uint32_t speed, uint8_t channel)
{
    MotorSetDirection(dir, channel);
    MotorSetAble(1, channel);
#if (MOTOR_COUNT >= 1)
    if(channel == 0){ PwmFreqSet(speed, MOTOR_PWM_Channel_0); }
#endif
#if (MOTOR_COUNT >= 2)
    if(channel == 1){ PwmFreqSet(speed, MOTOR_PWM_Channel_1); }
#endif
#if (MOTOR_COUNT >= 3)
    if(channel == 2){ PwmFreqSet(speed, MOTOR_PWM_Channel_2); }
#endif
}

/**
 * @brief   start motor with only speed
 * @param   speed direction to move
 * @param   channel motor channel
 */
void MotorStartSpeed(uint32_t speed, uint8_t channel)
{
    MotorSetAble(1, channel);
#if (MOTOR_COUNT >= 1)
    if(channel == 0){ PwmFreqSet(speed, MOTOR_PWM_Channel_0); }
#endif
#if (MOTOR_COUNT >= 2)
    if(channel == 1){ PwmFreqSet(speed, MOTOR_PWM_Channel_0); }
#endif
#if (MOTOR_COUNT >= 3)
    if(channel == 2){ PwmFreqSet(speed, MOTOR_PWM_Channel_0); }
#endif
}

/**
 * @brief   stop motor
 * @param   channel motor channel
 */
void MotorStop(uint8_t channel)
{
    // MOTOR_PARAM_SET(MOTOR_DIR_OUT, 0, 0, 0, 0, motorSetState);
    if(motorStep_t_Group[channel].motorEn == 0)
        return;
    else{
#if (MOTOR_COUNT >= 1)
        if(channel == 0){ PWM_Stop(0, MOTOR_PWM_Channel_0); }
#endif
#if (MOTOR_COUNT >= 2)
        if(channel == 1){ PWM_Stop(0, MOTOR_PWM_Channel_0); }
#endif
#if (MOTOR_COUNT >= 3)
        if(channel == 2){ PWM_Stop(0, MOTOR_PWM_Channel_0); }
#endif
        MotorSetAble(0, channel);;
        motorStep_t_Group[channel].motorSpeedCounter = 0;
        motorStep_t_Group[channel].motorStopCounter = 0;
    }
}

/**
 * @brief   process motor error
 * @param   channel motor channel
 * @note    put at 1ms cycle
 */
void MotorSelfCycle(uint8_t channel)
{
    if(MotorGetDiagLevel(channel)){
        if(motorStep_t_Group[channel].motorEn == 1)
            MotorSetAble(0, channel);
        if(motorStep_t_Group[channel].motorStopCounter >= 10000)
            MotorSetAble(1, channel);;
    }
}


/**
 * @brief   deal step motor state
 * @note    put at 1ms cycle;
 *          set motorSetState = 4 to change motor dir with speed no change
 *          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
 *          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 MotorStateMachine_Step_PWM(void)
{
    uint8_t i = 0;
    for(i = 0; i < MOTOR_COUNT; i++){
        if(motorStep_t_Group[i].motorSoftSpeed < motorStep_t_Group[i].motorSpeed)
            motorStep_t_Group[i].motorSoftSpeed = motorStep_t_Group[i].motorSpeed;

        if(motorStep_t_Group[i].motorSetState == 4){
            motorStep_t_Group[i].motorStopCounter = 0;
            motorStep_t_Group[i].motorStepCounter = 0;
            motorStep_t_Group[i].motorSetState = 2;
            motorStep_t_Group[i].motorDir = !motorStep_t_Group[i].motorDir;
        }
        else if(motorStep_t_Group[i].motorSetState == 3){
            motorStep_t_Group[i].motorStepCounter = motorStep_t_Group[i].motorSoftSteps;
            MotorStart(motorStep_t_Group[i].motorDir, motorStep_t_Group[i].motorSpeed, i);
            motorStep_t_Group[i].motorSetState = 1;
        }
        else if(motorStep_t_Group[i].motorSetState == 2){
            motorStep_t_Group[i].motorStepCounter = 0;
            MotorStart(motorStep_t_Group[i].motorDir, motorStep_t_Group[i].motorSoftSpeed, i);
            motorStep_t_Group[i].motorSetState = 1;
        }
        else if(motorStep_t_Group[i].motorSetState == 1){
            if(motorStep_t_Group[i].motorStepCounter < motorStep_t_Group[i].motorSoftSteps){
#if defined (Motor_StartLine_T)
                motorStep_t_Group[i].speedIncreaseStep = ((float)(motorStep_t_Group[i].motorSoftSpeed - motorStep_t_Group[i].motorSpeed) / (float)motorStep_t_Group[i].motorSoftSteps);  // at 1ms cycle
                MotorStart(motorStep_t_Group[i].motorDir,
                    motorStep_t_Group[i].motorSpeed < (motorStep_t_Group[i].motorSoftSpeed - (motorStep_t_Group[i].speedIncreaseStep * motorStep_t_Group[i].motorStepCounter)) ?
                    (motorStep_t_Group[i].motorSoftSpeed - (motorStep_t_Group[i].speedIncreaseStep * motorStep_t_Group[i].motorStepCounter)) : motorStep_t_Group[i].motorSpeed, i);
#elif defined (Motor_StartLine_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
                motorStep_t_Group[i].S_paramA = (((float)(motorStep_t_Group[i].motorSoftSpeed - motorStep_t_Group[i].motorSpeed) / 2)
                    - (motorStep_t_Group[i].S_paramC * (float)((float)motorStep_t_Group[i].motorSoftSteps / 2) * (float)((float)motorStep_t_Group[i].motorSoftSteps / 2) / 2))
                    / ((float)((float)motorStep_t_Group[i].motorSoftSteps / 2) * (float)((float)motorStep_t_Group[i].motorSoftSteps / 2)
                        * (float)((float)motorStep_t_Group[i].motorSoftSteps / 2) * (float)((float)motorStep_t_Group[i].motorSoftSteps / 2)) * 4;
                // mbG[27] = motorStep_t_Group[i].S_paramA * 10000000;
                if(motorStep_t_Group[i].motorStepCounter < (motorStep_t_Group[i].motorSoftSteps / 2))
                    motorStep_t_Group[i].speedIncreaseStep =
                    ((float)(motorStep_t_Group[i].S_paramA * motorStep_t_Group[i].motorStepCounter * motorStep_t_Group[i].motorStepCounter * motorStep_t_Group[i].motorStepCounter * motorStep_t_Group[i].motorStepCounter) / 4)
                    + ((float)(motorStep_t_Group[i].S_paramC * motorStep_t_Group[i].motorStepCounter * motorStep_t_Group[i].motorStepCounter) / 2);
                // (motorStep_t_Group[i].S_paramA * motorStep_t_Group[i].motorStepCounter * motorStep_t_Group[i].motorStepCounter * motorStep_t_Group[i].motorStepCounter)
                // + (motorStep_t_Group[i].S_paramC * motorStep_t_Group[i].motorStepCounter);
                else
                    motorStep_t_Group[i].speedIncreaseStep =
                    (float)((float)(motorStep_t_Group[i].motorSoftSpeed - motorStep_t_Group[i].motorSpeed) / 2)
                    + ((float)(motorStep_t_Group[i].S_paramA * motorStep_t_Group[i].motorSoftSteps * motorStep_t_Group[i].motorSoftSteps * motorStep_t_Group[i].motorSoftSteps * motorStep_t_Group[i].motorSoftSteps) / 64)
                    + ((float)(motorStep_t_Group[i].S_paramC * motorStep_t_Group[i].motorSoftSteps * motorStep_t_Group[i].motorSoftSteps) / 8)
                    - ((float)(motorStep_t_Group[i].S_paramA * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorStepCounter) * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorStepCounter)
                        * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorStepCounter) * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorStepCounter)) / 4)
                    - ((float)(motorStep_t_Group[i].S_paramC * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorStepCounter) * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorStepCounter)) / 2);
                    // 0
                    // - (motorStep_t_Group[i].S_paramA * (float)((float)motorStep_t_Group[i].motorStepCounter - (float)motorStep_t_Group[i].motorSoftSteps)
                    //     * (float)((float)motorStep_t_Group[i].motorStepCounter - (float)motorStep_t_Group[i].motorSoftSteps)
                    //     * (float)((float)motorStep_t_Group[i].motorStepCounter - (float)motorStep_t_Group[i].motorSoftSteps))
                    // - (motorStep_t_Group[i].S_paramC * (float)((float)motorStep_t_Group[i].motorStepCounter - (float)motorStep_t_Group[i].motorSoftSteps));
                MotorStart(motorStep_t_Group[i].motorDir,
                    motorStep_t_Group[i].motorSpeed < (motorStep_t_Group[i].motorSoftSpeed - motorStep_t_Group[i].speedIncreaseStep) ?
                    (motorStep_t_Group[i].motorSoftSpeed - motorStep_t_Group[i].speedIncreaseStep) : motorStep_t_Group[i].motorSpeed, i);
#else
#endif
            }

            if(motorStep_t_Group[i].motorStepCounter == motorStep_t_Group[i].motorSoftSteps)
                motorStep_t_Group[i].motorSetState = 3;
            else if(motorStep_t_Group[i].motorStepCounter == (motorStep_t_Group[i].motorSoftSteps + motorStep_t_Group[i].motorSteps))
                motorStep_t_Group[i].motorSetState = 5;
        }
        else if(motorStep_t_Group[i].motorSetState == 10){
            MotorStop(i);
            motorStep_t_Group[i].motorSetState = 0;
        }
        else{ MotorStop(i); }    // 5 0

        if(motorStep_t_Group[i].motorEn == 0){
            motorStep_t_Group[i].motorStepCounter = 0;
            motorStep_t_Group[i].motorStopCounter++;
        }
        else{
            motorStep_t_Group[i].motorStepCounter++;
            motorStep_t_Group[i].motorStopCounter = 0;
        }
    }
}


uint8_t motorLastState[2] = {0};
/**
 * @brief   deal step motor state
 * @note    put at 1us cycle;
 *          set motorSetState = 4 to change motor dir with speed no change
 *          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
 *          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 MotorStateMachine_Step(void)
{
    uint8_t i = 0;
    for(i = 0; i < MOTOR_COUNT; i++){
        if(motorStep_t_Group[i].motorSoftSpeed < motorStep_t_Group[i].motorSpeed)
            motorStep_t_Group[i].motorSoftSpeed = motorStep_t_Group[i].motorSpeed;

        if(motorStep_t_Group[i].motorSetState == 4){
            motorStep_t_Group[i].motorSpeedCounter = 0;
            motorStep_t_Group[i].motorStepCounter = 0;
            motorStep_t_Group[i].motorSetState = 2;
            motorStep_t_Group[i].motorDir = !motorStep_t_Group[i].motorDir;
        }
        else if(motorStep_t_Group[i].motorSetState == 3){
            motorStep_t_Group[i].motorEn = 1;
            motorStep_t_Group[i].motorSpeedCounter = 0;
            motorStep_t_Group[i].motorStepCounter = 0;
            motorLastState[i] = 3;
            motorStep_t_Group[i].motorSetState = 1;
        }
        else if(motorStep_t_Group[i].motorSetState == 2){
            motorStep_t_Group[i].motorEn = 1;
            motorStep_t_Group[i].motorSpeedCounter = 0;
            motorStep_t_Group[i].motorStepCounter = 0;
            motorLastState[i] = 2;
            motorStep_t_Group[i].motorSetState = 1;
        }
        else if(motorStep_t_Group[i].motorSetState == 1){
        }
        else if(motorStep_t_Group[i].motorSetState == 10){
            motorStep_t_Group[i].motorEn = 0;
            motorStep_t_Group[i].motorSetState = 0;
        }
        else{   // 5 0
            motorStep_t_Group[i].motorEn = 0;
            motorStep_t_Group[i].motorSetState = 0;
        }


//         if(motorStep_t_Group[i].motorSetState == 4){
//             motorStep_t_Group[i].motorSpeedCounter = 0;
//             motorStep_t_Group[i].motorStopCounter = 0;
//             motorStep_t_Group[i].motorSetState = 2;
//             motorStep_t_Group[i].motorDir = !motorStep_t_Group[i].motorDir;
//         }
//         else if(motorStep_t_Group[i].motorSetState == 3){
//             motorStep_t_Group[i].motorSpeedCounter = motorStep_t_Group[i].motorSoftSteps;
//             MotorStart(motorStep_t_Group[i].motorDir, motorStep_t_Group[i].motorSpeed, i);
//             motorStep_t_Group[i].motorSetState = 1;
//         }
//         else if(motorStep_t_Group[i].motorSetState == 2){
//             motorStep_t_Group[i].motorSpeedCounter = 0;
//             MotorStart(motorStep_t_Group[i].motorDir, motorStep_t_Group[i].motorSoftSpeed, i);
//             motorStep_t_Group[i].motorSetState = 1;
//         }
//         else if(motorStep_t_Group[i].motorSetState == 1){
//             if(motorStep_t_Group[i].motorSpeedCounter < motorStep_t_Group[i].motorSoftSteps){
// #if defined (Motor_StartLine_T)
//                 motorStep_t_Group[i].speedIncreaseStep = ((float)(motorStep_t_Group[i].motorSoftSpeed - motorStep_t_Group[i].motorSpeed) / (float)motorStep_t_Group[i].motorSoftSteps);  // at 1ms cycle
//                 MotorStart(motorStep_t_Group[i].motorDir,
//                     motorStep_t_Group[i].motorSpeed < (motorStep_t_Group[i].motorSoftSpeed - (motorStep_t_Group[i].speedIncreaseStep * motorStep_t_Group[i].motorSpeedCounter)) ?
//                     (motorStep_t_Group[i].motorSoftSpeed - (motorStep_t_Group[i].speedIncreaseStep * motorStep_t_Group[i].motorSpeedCounter)) : motorStep_t_Group[i].motorSpeed, i);
// #elif defined (Motor_StartLine_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
//                 motorStep_t_Group[i].S_paramA = (((float)(motorStep_t_Group[i].motorSoftSpeed - motorStep_t_Group[i].motorSpeed) / 2)
//                     - (motorStep_t_Group[i].S_paramC * (float)((float)motorStep_t_Group[i].motorSoftSteps / 2) * (float)((float)motorStep_t_Group[i].motorSoftSteps / 2) / 2))
//                     / ((float)((float)motorStep_t_Group[i].motorSoftSteps / 2) * (float)((float)motorStep_t_Group[i].motorSoftSteps / 2)
//                         * (float)((float)motorStep_t_Group[i].motorSoftSteps / 2) * (float)((float)motorStep_t_Group[i].motorSoftSteps / 2)) * 4;
//                 // mbG[27] = motorStep_t_Group[i].S_paramA * 10000000;
//                 if(motorStep_t_Group[i].motorSpeedCounter < (motorStep_t_Group[i].motorSoftSteps / 2))
//                     motorStep_t_Group[i].speedIncreaseStep =
//                     ((float)(motorStep_t_Group[i].S_paramA * motorStep_t_Group[i].motorSpeedCounter * motorStep_t_Group[i].motorSpeedCounter * motorStep_t_Group[i].motorSpeedCounter * motorStep_t_Group[i].motorSpeedCounter) / 4)
//                     + ((float)(motorStep_t_Group[i].S_paramC * motorStep_t_Group[i].motorSpeedCounter * motorStep_t_Group[i].motorSpeedCounter) / 2);
//                 // (motorStep_t_Group[i].S_paramA * motorStep_t_Group[i].motorSpeedCounter * motorStep_t_Group[i].motorSpeedCounter * motorStep_t_Group[i].motorSpeedCounter)
//                 // + (motorStep_t_Group[i].S_paramC * motorStep_t_Group[i].motorSpeedCounter);
//                 else
//                     motorStep_t_Group[i].speedIncreaseStep =
//                     (float)((float)(motorStep_t_Group[i].motorSoftSpeed - motorStep_t_Group[i].motorSpeed) / 2)
//                     + ((float)(motorStep_t_Group[i].S_paramA * motorStep_t_Group[i].motorSoftSteps * motorStep_t_Group[i].motorSoftSteps * motorStep_t_Group[i].motorSoftSteps * motorStep_t_Group[i].motorSoftSteps) / 64)
//                     + ((float)(motorStep_t_Group[i].S_paramC * motorStep_t_Group[i].motorSoftSteps * motorStep_t_Group[i].motorSoftSteps) / 8)
//                     - ((float)(motorStep_t_Group[i].S_paramA * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorSpeedCounter) * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorSpeedCounter)
//                         * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorSpeedCounter) * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorSpeedCounter)) / 4)
//                     - ((float)(motorStep_t_Group[i].S_paramC * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorSpeedCounter) * (motorStep_t_Group[i].motorSoftSteps - motorStep_t_Group[i].motorSpeedCounter)) / 2);
//                     // 0
//                     // - (motorStep_t_Group[i].S_paramA * (float)((float)motorStep_t_Group[i].motorSpeedCounter - (float)motorStep_t_Group[i].motorSoftSteps)
//                     //     * (float)((float)motorStep_t_Group[i].motorSpeedCounter - (float)motorStep_t_Group[i].motorSoftSteps)
//                     //     * (float)((float)motorStep_t_Group[i].motorSpeedCounter - (float)motorStep_t_Group[i].motorSoftSteps))
//                     // - (motorStep_t_Group[i].S_paramC * (float)((float)motorStep_t_Group[i].motorSpeedCounter - (float)motorStep_t_Group[i].motorSoftSteps));
//                 MotorStart(motorStep_t_Group[i].motorDir,
//                     motorStep_t_Group[i].motorSpeed < (motorStep_t_Group[i].motorSoftSpeed - motorStep_t_Group[i].speedIncreaseStep) ?
//                     (motorStep_t_Group[i].motorSoftSpeed - motorStep_t_Group[i].speedIncreaseStep) : motorStep_t_Group[i].motorSpeed, i);
// #else
// #endif
//             }

//             if(motorStep_t_Group[i].motorSpeedCounter == motorStep_t_Group[i].motorSoftSteps)
//                 motorStep_t_Group[i].motorSetState = 3;
//             else if(motorStep_t_Group[i].motorSpeedCounter == (motorStep_t_Group[i].motorSoftSteps + motorStep_t_Group[i].motorSteps))
//                 motorStep_t_Group[i].motorSetState = 5;
//         }
//         else if(motorStep_t_Group[i].motorSetState == 10){
//             MotorStop(i);
//             motorStep_t_Group[i].motorSetState = 0;
//         }
//         else{ MotorStop(i); }    // 5 0

//         if(motorStep_t_Group[i].motorEn == 0){
//             motorStep_t_Group[i].motorSpeedCounter = 0;
//             motorStep_t_Group[i].motorStopCounter++;
//         }
//         else{
//             motorStep_t_Group[i].motorSpeedCounter--;
//             if(motorStep_t_Group[i].motorSpeedCounter == 0){
//                 motorStep_t_Group[i].motorSpeedCounter = 0;
//                 motorStep_t_Group[i].motorStepCounter++;
//             }
//             motorStep_t_Group[i].motorStopCounter = 0;
//         }

        if(motorStep_t_Group[i].motorEn){
            motorStep_t_Group[i].motorStopCounter = 0;
            MotorSetDirection(motorStep_t_Group[i].motorDir, i);
            MotorSetAble(motorStep_t_Group[i].motorEn, i);
            motorStep_t_Group[i].motorSpeedCounter++;
            mbP_HSTV = motorStep_t_Group[i].motorSpeedCounter;

            if((motorLastState[i] == 3 && motorStep_t_Group[i].motorSpeedCounter >= motorStep_t_Group[i].motorSpeed)
                || (motorLastState[i] == 2 && motorStep_t_Group[i].motorSpeedCounter >= motorStep_t_Group[i].motorSoftSpeed)){
                motorStep_t_Group[i].motorSpeedCounter = 0;
                motorStep_t_Group[i].motorStepLevel = !motorStep_t_Group[i].motorStepLevel;
                MotorSetStepLevel(motorStep_t_Group[i].motorStepLevel, i);
                mbP_HSHS = motorStep_t_Group[i].motorStepLevel;
                mbP_HSTW = motorStep_t_Group[i].motorStepCounter;

                if(motorStep_t_Group[i].motorStepLevel == STATIC_STEP_LEVEL){
                    motorStep_t_Group[i].motorStepCounter++;

                    if((motorLastState[i] == 3 && motorStep_t_Group[i].motorStepCounter >= motorStep_t_Group[i].motorSteps)
                        || (motorLastState[i] == 2 && motorStep_t_Group[i].motorStepCounter >= motorStep_t_Group[i].motorSoftSteps)){
                        motorStep_t_Group[i].motorSpeedCounter = 0;
                        motorStep_t_Group[i].motorStepCounter = 0;
                        motorStep_t_Group[i].motorStepLevel = STATIC_STEP_LEVEL;
                        MotorSetStepLevel(motorStep_t_Group[i].motorStepLevel, i);
                        // motorStep_t_Group[i].motorSpeedCounter = 0;
                        if(motorLastState[i] == 3)
                            motorStep_t_Group[i].motorSetState = 5;
                        else if(motorLastState[i] == 2)
                            motorStep_t_Group[i].motorSetState = 3;
                    }
                }
            }
        }
        else{
            motorStep_t_Group[i].motorSpeedCounter = 0;
            motorStep_t_Group[i].motorStepCounter = 0;
            motorStep_t_Group[i].motorStopCounter++;
            motorStep_t_Group[i].motorStepLevel = STATIC_STEP_LEVEL;
            MotorSetStepLevel(motorStep_t_Group[i].motorStepLevel, i);
            MotorSetAble(motorStep_t_Group[i].motorEn, i);
        }
    }
}


#endif /* __MOTOR_STEP_ATY_C */

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