/**
* @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 *************************************/

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


uint8_t motorEn = 0;
uint8_t motorDir = MOTOR_DIR_OUT;
uint32_t motorSoftSpeed = 0;
uint32_t motorSpeed = 0;
uint32_t motorSoftTime = 0;
uint32_t motorTime = 0;

uint32_t motorStartCounter = 0;
uint32_t motorStopCounter = 0;      // for stall detection or driver error(TMC2209)

uint8_t motorSetState = 0;          // 0: stop, 1: start hold, 2: start soft, 3: run speed, 4: change dir, 5: stop state, soft set 0 after use, 10: scram motor

float speedIncreaseStep = 0.0f;
float S_paramA = 0.0f;
#ifdef S_PARAM_C
float S_paramC = S_PARAM_C;
#else
float S_paramC = 0.0f;
#endif


/**
 * @brief   start motor with direction set
 * @param   dir direction to move
 * @param   speed direction to move
 */
void MotorStart(uint8_t dir, uint32_t speed)
{
    if(dir == MOTOR_DIR_IN)
        MOTOR_DIR_SET_IN;
    else if(dir == MOTOR_DIR_OUT)
        MOTOR_DIR_SET_OUT;
    MOTOR_EN_SET_ENABLE;
    PwmFreqSet(speed, Motor_Channel);
#ifdef __DEBUG_MOTOR_STEP_ATY
    printf("\r\nMotor Start: %d - %d", dir, speed);
#endif /* __DEBUG_MOTOR_STEP_ATY */
}

/**
 * @brief   start motor with only speed
 * @param   speed direction to move
 */
void MotorStartSpeed(uint32_t speed)
{
    MOTOR_EN_SET_ENABLE;
    PwmFreqSet(speed, Motor_Channel);
#ifdef __DEBUG_MOTOR_STEP_ATY
    printf("\r\nMotor Start: %d", speed);
#endif /* __DEBUG_MOTOR_STEP_ATY */
}

/**
 * @brief   stop motor
 */
void MotorStop(void)
{
    // MOTOR_PARAM_SET(MOTOR_DIR_OUT, 0, 0, 0, 0, motorSetState);
    if(motorEn == 0)
        return;
    else{
        PWM_Stop(0, Motor_Channel);
        MOTOR_EN_SET_DISABLE;
        motorStartCounter = 0;
        motorStopCounter = 0;
#ifdef __DEBUG_MOTOR_STEP_ATY
        printf("\r\nMotor Stop");
#endif /* __DEBUG_MOTOR_STEP_ATY */
    }
}

/**
 * @brief   process motor error
 * @note    put at 1ms cycle
 */
void MotorSelfCycle(void)
{
    if(MOTOR_DIAG_GET_H)
    {
        if(motorEn == 1)
            MOTOR_EN_SET_DISABLE;
        if(motorStopCounter >= 100)
            MOTOR_EN_SET_ENABLE;
    }
}


/**
 * @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, motorSoftTime need to be 0
 *          set motorSetState = 2 to strat motor with soft, motorDir/motorSoftSpeed/motorSpeed/motorSoftTime/motorTime 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)
{
    if(motorSoftSpeed < motorSpeed)
        motorSoftSpeed = motorSpeed;

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

        if(motorStartCounter == motorSoftTime)
            motorSetState = 3;
        else if(motorStartCounter == (motorSoftTime + motorTime))
            motorSetState = 5;
    }
    else if(motorSetState == 10){
        motorSetState = 0;
        MotorStop();
    }
    else{ MotorStop(); }    // 5 0

    if(motorEn == 0){
        motorStartCounter = 0;
        motorStopCounter++;
    }
    else{
        motorStartCounter++;
        motorStopCounter = 0;
    }
}


#endif /* __MOTOR_STEP_ATY_C */

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