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


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