/**
* @file MOTOR_STEP_SPEED_T_C_ATY.c
*
* @param Project DEVICE_GENERAL_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2026 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 Trapezoidal acceleration and deceleration control implementation for stepper motor
*
* @note  import angle to use COMPENSATE
*
* @version
*       - 1_00_260119 > ATY
*           -# Initial version
********************************************************************************
*/

#ifndef __MOTOR_STEP_SPEED_T_C_ATY_C
#define __MOTOR_STEP_SPEED_T_C_ATY_C

#include "MOTOR_STEP_SPEED_T_C_ATY.h"
#include <math.h>

#define MOTOR_STEP_SPEED_T_C_ATY_TAG "\r\n[MOTOR_STEP_SPEED_T_C_ATY] "

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

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


uint8_t MSSTC_PulseToggle = 0;          // pulse toggle counter (for complete pulse)
int32_t MSSTC_CelCount = 0;             // ac/deceleration step counter
uint32_t MSSTC_FastStepDelay = 0;       // minimum delay at fullSpeed
int32_t MSSTC_StepDelay = 0;            // current step delay (timer ticks)
uint32_t MSSTC_NewStepDelay = 0;    // calculated step delay for next step from last step
int32_t MSSTC_Rest = 0;                 // Remainder for precision
int32_t MSSTC_LastDelay = 0;            // Last acceleration delay

// for angle compensation ------------------------------------------------------
float MSSTC_StartAngle = 0;
uint32_t MSSTC_CurrentTurn = 0;
float MSSTC_CurrentAngle = 0;
uint32_t MSSTC_TargetTurn = 0;
float MSSTC_TargetAngle = 0;
float MSSTC_AnglePerStep = 0;
float MSSTC_TargetAngleL = 0;
float MSSTC_TargetAngleH = 0;
float MSSTC_AngleDiff = 0;
int32_t MSSTC_StepDiff = 0;
uint8_t MSSTC_CelCountErrCount = 0;


float K = 0.676;
/**
 * @brief Move motor with trapezoidal dev->fullSpeed profile
 * @param dev Pointer to motor device structure
 * @param steps Number of steps to move (positive=CW, negative=CCW)
 * @return 0=success, 1=busy, 2=invalid parameters
 */
uint8_t MSSTC_Move(struct MOTOR_STEP_SPEED_T_C_ATY_Dev* dev,
    uint32_t steps, float startAngle){

    if(MSSTC_IsRunning(dev) == 1){
        return 1;
    }

    if(steps == 0 || dev->fullSpeed == 0){
        dev->stop();
        return 0;
    }
    if(dev->unitDivision == 0)
        dev->unitDivision = 1;

    if(dev->powerOnlyRun == 1){
        dev->enable = 1;
    }
    dev->enSet(dev->enable);
    dev->dirSet(dev->direction);

    /* Handle single step */
    if(steps == 1){
        dev->runState = MSSTC_STATE_DECEL;
        MSSTC_CelCount = -1;
        MSSTC_StepDelay = 1000;
        dev->start();
        return 0;
    }

    /* Calculate motor parameters */
    // alpha = 2 * PI / stepsPerRevolution [rad/step]
    float alpha = 2.0 * 3.14159265359 / (dev->fullSteps * dev->microstepping);
    /* Calculate minimum delay (maximum dev->fullSpeed) */
    // wmax = fullSpeed [rad/sec]
    // tmin = alpha / wmax [sec/step] (t = distance / velocity)
    // fastStepDelay = cmin = tmin * timerFreq = alpha * timerFreq / fullSpeed
    MSSTC_FastStepDelay = (uint32_t)(alpha * dev->timerFreq * dev->unitDivision / dev->fullSpeed);
    if(MSSTC_FastStepDelay <= 1){
        MSSTC_FastStepDelay = 2;
    }

    /* Calculate initial step delay (first step) */
    // t0 = sqrt(2 * alpha / (acceleration / unit)) = sqrt(2 * alpha * unit / acceleration)
    // stepDelay = K * timerFreq * t0
    if(dev->acceleration != 0){
        MSSTC_StepDelay = (uint32_t)(K * dev->timerFreq
            * sqrt(2.0 * alpha * dev->unitDivision / dev->acceleration));
    }
    if(dev->acceleration == 0){
        // no acceleration and no deceleration
        MSSTC_StepDelay = MSSTC_FastStepDelay;
    }

    /* Calculate steps to reach fullSpeed */
    // accelSteps = (fullSpeed / unit)^2 / (2 * alpha * (acceleration / unit))
    // accelSteps = fullSpeed^2 / (2 * alpha * acceleration * unit)
    dev->accelSteps = 1;
    if(dev->acceleration != 0){
        dev->accelSteps = (uint32_t)((double)dev->fullSpeed
            / (2.0 * alpha * dev->acceleration * dev->unitDivision)
            * dev->fullSpeed);
    }
    if(dev->accelSteps == 0 || dev->acceleration == 0){
        dev->accelSteps = 1;
    }

    /* Calculate steps before deceleration */
    dev->accelLimit = 1;
    if(dev->acceleration != 0 && dev->deceleration != 0){
        dev->accelLimit = (uint32_t)(steps * dev->deceleration
            / (dev->acceleration + dev->deceleration));
    }
    else if(dev->deceleration == 0){
        dev->accelLimit = steps - 1;
    }
    if(dev->accelLimit == 0 || dev->acceleration == 0){
        dev->accelLimit = 1;
    }

    /* Calculate steps deceleration */
    dev->decelSteps = 1;
    if(dev->acceleration != 0 && dev->deceleration != 0){
        if(dev->accelLimit <= dev->accelSteps){
            // not to fullSpeed
            dev->decelSteps = steps - dev->accelLimit;
            dev->accelSteps = dev->accelLimit;
        }
        else{
            // accelerate to fullSpeed
            dev->decelSteps = (uint32_t)(dev->accelSteps
                * dev->acceleration / dev->deceleration);
        }
    }
    else if(dev->acceleration == 0 && dev->deceleration != 0){
        dev->decelSteps = (uint32_t)((double)dev->fullSpeed
            / (2.0 * alpha * dev->deceleration * dev->unitDivision)
            * dev->fullSpeed);
        if(dev->decelSteps >= steps - 1){
            dev->decelSteps = steps - 1;
        }
    }
    if(dev->decelSteps == 0 || dev->deceleration == 0){
        dev->decelSteps = 1;
    }
    /* Calculate deceleration start position */
    dev->decelStart = steps - dev->decelSteps;
    if(dev->mode == MSSTC_MODE_SPEED_OPEN){
        dev->decelSteps = (uint32_t)((double)dev->fullSpeed
            / (2.0 * alpha * dev->deceleration * dev->unitDivision)
            * dev->fullSpeed);
    }

    /* Set initial state */
    if(MSSTC_StepDelay <= MSSTC_FastStepDelay){
        // fullSpeed is very low, no acceleration
        MSSTC_StepDelay = MSSTC_FastStepDelay;
        dev->runState = MSSTC_STATE_FULL;
    }
    else{
        dev->runState = MSSTC_STATE_ACCEL;
    }

    /* Reset counters */
    dev->stepCountCurrent = 0;
    MSSTC_PulseToggle = 0;
    MSSTC_CelCount = 0;
    MSSTC_NewStepDelay = 0;
    MSSTC_Rest = 0;
    MSSTC_LastDelay = MSSTC_StepDelay;
    MSSTC_CelCountErrCount = 0;

    MSSTC_StartAngle = startAngle;
    MSSTC_CurrentTurn = 0;
    MSSTC_CurrentAngle = startAngle;
    MSSTC_TargetTurn = steps / (dev->fullSteps * dev->microstepping);
    MSSTC_TargetAngle = ((steps % (dev->fullSteps * dev->microstepping)) * 360.0)
        / (dev->fullSteps * dev->microstepping) + startAngle;
    while(MSSTC_TargetAngle >= 360.0){
        MSSTC_TargetAngle -= 360.0;
        MSSTC_TargetTurn++;
    }
    MSSTC_AnglePerStep = 360.0 / (dev->fullSteps * dev->microstepping);
    MSSTC_TargetAngleL = MSSTC_TargetAngle - MSSTC_AnglePerStep;
    MSSTC_TargetAngleH = MSSTC_TargetAngle + MSSTC_AnglePerStep;
    MSSTC_AngleDiff = 0;
    MSSTC_StepDiff = 0;

    /* Start PWM and timer */
    dev->setTimerCompare(MSSTC_StepDelay);
    dev->start();

    return 0;
}

/**
 * @brief Timer interrupt handler - dev->fullSpeed decision algorithm
 * @param dev Pointer to motor device structure
 * @note Call this function in timer compare interrupt
 */
void MSSTC_IRQHandler(struct MOTOR_STEP_SPEED_T_C_ATY_Dev* dev){
    /* Update timer compare value */
    dev->setTimerCompare(MSSTC_StepDelay);

    /* Toggle counter for complete pulse generation */
    MSSTC_PulseToggle++;
    if(MSSTC_PulseToggle >= 2){
        MSSTC_PulseToggle = 0;

        /* State machine for dev->fullSpeed control */
        switch(dev->runState){
            case MSSTC_STATE_STOP:
            {
                dev->stop();
                if(dev->mode == MSSTC_MODE_DISTANCE_CLOSE){
                    dev->runState = MSSTC_STATE_COMPENSATE;
                }
                else{
                    dev->runState = MSSTC_STATE_STANDBY;
                    if(dev->powerOnlyRun == 1){
                        dev->enable = 0;
                        dev->enSet(dev->enable);
                    }
                }
                break;
            }
            case MSSTC_STATE_ACCEL:
            {
                dev->stepCountCurrent++;
                MSSTC_CelCount++;

                /* Calculate new step delay */
                MSSTC_NewStepDelay = MSSTC_StepDelay -
                    ((2 * MSSTC_StepDelay + MSSTC_Rest) /
                        (4 * MSSTC_CelCount + 1));
                MSSTC_Rest = ((2 * MSSTC_StepDelay + MSSTC_Rest) %
                    (4 * MSSTC_CelCount + 1));

                /* Check if should start deceleration */
                if(dev->mode != MSSTC_MODE_SPEED_OPEN
                    && dev->stepCountCurrent >= dev->decelStart){
                    MSSTC_PulseToggle = 1;
                    MSSTC_CelCount = -1 * (int32_t)dev->decelSteps;
                    dev->runState = MSSTC_STATE_DECEL;
                }
                /* Check if reached maximum dev->fullSpeed */
                else if(MSSTC_NewStepDelay <= MSSTC_FastStepDelay){
                    MSSTC_LastDelay = MSSTC_NewStepDelay;
                    MSSTC_CelCount = -1 * (int32_t)dev->decelSteps;
                    dev->runState = MSSTC_STATE_FULL;
                }
                break;
            }
            case MSSTC_STATE_FULL:
            {
                if(dev->mode == MSSTC_MODE_SPEED_OPEN){
                    dev->stepCountCurrent = 0;
                }
                else{
                    dev->stepCountCurrent++;
                }
                MSSTC_NewStepDelay = MSSTC_FastStepDelay;
                // MSSTC_LastDelay = MSSTC_NewStepDelay;

                /* Check if should start deceleration */
                if(dev->stepCountCurrent >= dev->decelStart){
                    MSSTC_PulseToggle = 1;
                    MSSTC_Rest = 0;
                    MSSTC_CelCount = -1 * (int32_t)dev->decelSteps;
                    MSSTC_NewStepDelay = MSSTC_LastDelay;
                    dev->runState = MSSTC_STATE_DECEL;
                }
                break;
            }
            case MSSTC_STATE_DECEL:
            {
                dev->stepCountCurrent++;
                MSSTC_CelCount++;

                /* Check if this is the last step */
                if(MSSTC_CelCount >= 0){
                    dev->runState = MSSTC_STATE_STOP;
                    break;
                }

                if(MSSTC_StepDiff == 0){
                    MSSTC_NewStepDelay = MSSTC_StepDelay -
                        ((2 * MSSTC_StepDelay + MSSTC_Rest) /
                            (4 * MSSTC_CelCount + 1));
                    MSSTC_Rest = ((2 * MSSTC_StepDelay + MSSTC_Rest) %
                        (4 * MSSTC_CelCount + 1));
                    MSSTC_LastDelay = MSSTC_NewStepDelay;
                }
                break;
            }
            default:
            {
                if(dev->powerOnlyRun == 1){
                    dev->enable = 0;
                    dev->enSet(dev->enable);
                }
                break;
            }
        }
        if(MSSTC_NewStepDelay <= 1){
            MSSTC_NewStepDelay = 2;
        }
        MSSTC_StepDelay = MSSTC_NewStepDelay;
    }
}

/**
 * @brief Stop motor immediately but not clear params data
 * @param dev Pointer to motor device structure
 */
void MSSTC_Scram(struct MOTOR_STEP_SPEED_T_C_ATY_Dev* dev){
    dev->runState = MSSTC_STATE_STANDBY;
    if(dev->powerOnlyRun == 1){
        dev->enable = 0;
        dev->enSet(dev->enable);
    }
    dev->stop();
}

void MSSTC_Stop(struct MOTOR_STEP_SPEED_T_C_ATY_Dev* dev){
    MSSTC_CelCount = -1 * (int32_t)dev->decelSteps;
    dev->runState = MSSTC_STATE_DECEL;
    dev->start();
    MSSTC_IRQHandler(dev);
}

void MSSTC_UpdateFullSpeed(struct MOTOR_STEP_SPEED_T_C_ATY_Dev* dev){
    float alpha = 2.0 * 3.14159265359 / (dev->fullSteps * dev->microstepping);
    MSSTC_FastStepDelay = (uint32_t)(alpha * dev->timerFreq * dev->unitDivision / dev->fullSpeed);
    dev->setTimerCompare(MSSTC_FastStepDelay);
}

/**
 * @brief Check if motor is running
 * @param dev Pointer to motor device structure
 * @return 1=running, 0=stopped
 */
uint8_t MSSTC_IsRunning(struct MOTOR_STEP_SPEED_T_C_ATY_Dev* dev){
    return (dev->runState == MSSTC_STATE_STANDBY) ? 0 : 1;
}

/**
 * @brief MSSTC_Compensate
 *
 * @param dev
 */
void MSSTC_Compensate(struct MOTOR_STEP_SPEED_T_C_ATY_Dev* dev){
    if(dev->runState != MSSTC_STATE_COMPENSATE){
        return;
    }

    uint32_t publicTurns = ((MSSTC_TargetTurn < MSSTC_CurrentTurn) ? MSSTC_TargetTurn : MSSTC_CurrentTurn);
    float targetAngleTotal = 360.0 * (MSSTC_TargetTurn - publicTurns) + MSSTC_TargetAngle;
    float currentAngleTotal = 360.0 * (MSSTC_CurrentTurn - publicTurns) + MSSTC_CurrentAngle;
    MSSTC_StepDiff = (int32_t)((targetAngleTotal - currentAngleTotal) / MSSTC_AnglePerStep);
    if(MSSTC_StepDiff > 1 || MSSTC_StepDiff < -1){
        if(MSSTC_StepDiff < 0){
            if(dev->turnBackAllowed == 1){
                MSSTC_CelCount = MSSTC_StepDiff;
                dev->dirSet(dev->direction ^ 0x01);
            }
            else{
                dev->runState = MSSTC_STATE_STANDBY;
                if(dev->powerOnlyRun == 1){
                    dev->enable = 0;
                    dev->enSet(dev->enable);
                }
            }
        }
        else{
            dev->dirSet(dev->direction);
            MSSTC_CelCount = -1 * MSSTC_StepDiff;
        }
        dev->runState = MSSTC_STATE_DECEL;

        MSSTC_CelCountErrCount++;
        if(MSSTC_CelCountErrCount > 5){
            dev->runState = MSSTC_STATE_STANDBY;
            if(dev->powerOnlyRun == 1){
                dev->enable = 0;
                dev->enSet(dev->enable);
            }
        }
        else{
            dev->start();
        }
    }
    else{
        dev->runState = MSSTC_STATE_STANDBY;
        if(dev->powerOnlyRun == 1){
            dev->enable = 0;
            dev->enSet(dev->enable);
        }
    }
}

/**
 * @brief Update current angle
 * @param dev Pointer to motor device structure
 * @param turn Turn count
 * @param angle Current angle
 */
void MSSTC_UpdateAngle(struct MOTOR_STEP_SPEED_T_C_ATY_Dev* dev,
    uint32_t turn, float angle){
    MSSTC_CurrentTurn = turn;
    MSSTC_CurrentAngle = angle;
    MSSTC_Compensate(dev);
}

#endif /* __MOTOR_STEP_SPEED_T_C_ATY_C */

/************************************ etc *************************************/
/* init
#define MS_1_TIM htim1
#define MS_1_TIM_CHANNEL TIM_CHANNEL_1
#define MS_1_TIM_IT_CC TIM_IT_CC1
// MSSTC -----------------------------------------------------------------------
#include "MOTOR_STEP_SPEED_T_C_ATY.h"
uint8_t MSSTC_1_EnSet(uint8_t en){
    HAL_GPIO_WritePin(MS_EN_GPIO_Port, MS_EN_Pin,
        (en == 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
    return 0;
}
uint8_t MSSTC_1_DirSet(uint8_t dir){
    HAL_GPIO_WritePin(MS_DIR_GPIO_Port, MS_DIR_Pin,
        (dir == __ATY_PN_P) ? GPIO_PIN_SET : GPIO_PIN_RESET);
    return 0;
}
void MSSTC_1_SetTimerCompare(uint32_t value){
    __HAL_TIM_SET_COMPARE(&MS_1_TIM, MS_1_TIM_CHANNEL, value);
    __HAL_TIM_SET_COUNTER(&MS_1_TIM, 0);
}
void MSSTC_1_Start(void){
    HAL_TIM_OC_Start_IT(&MS_1_TIM, MS_1_TIM_CHANNEL);
}
void MSSTC_1_Stop(void){
    HAL_TIM_OC_Stop_IT(&MS_1_TIM, MS_1_TIM_CHANNEL);
}
struct MOTOR_STEP_SPEED_T_C_ATY_Dev MSSTC_Dev_1 = {
    .enSet = MSSTC_1_EnSet,
    .dirSet = MSSTC_1_DirSet,
    .start = MSSTC_1_Start,
    .stop = MSSTC_1_Stop,
    .setTimerCompare = MSSTC_1_SetTimerCompare,

    .timerFreq = 1000000,
    .fullSteps = 200,
    .microstepping = 8,

    .enable = 0,
    .direction = __ATY_PN_P,
    .fullSpeed = 20,
    .unitDivision = 1,
    .acceleration = 100,
    .deceleration = 200,

    .mode = 0,
    .angleLock = 0,
    .powerOnlyRun = 1,
    .runState = MSSTC_STATE_STANDBY,

    .lock = __ATY_UNLOCKED
};
void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef* htim){
    if(htim->Instance == TIM1 && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1){
        if(__HAL_TIM_GET_IT_SOURCE(&MS_1_TIM, MS_1_TIM_IT_CC) != RESET){
            __HAL_TIM_CLEAR_IT(&MS_1_TIM, MS_1_TIM_IT_CC);
            MSSTC_IRQHandler(&MSSTC_Dev_1);
        }
    }
}
void MSSTC_1_Init(void){
    HAL_GPIO_WritePin(MS_SP_GPIO_Port, MS_SP_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(MS_MS1_GPIO_Port, MS_MS1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(MS_MS2_GPIO_Port, MS_MS2_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(MS_EN_GPIO_Port, MS_EN_Pin, GPIO_PIN_SET);

    RGF_MOTOR_FREQUENCY = (1600.0 / 60.0);
    RGF_MOTOR_DUTY_CYCLE = 50.0;

    RGF_MOTOR_FULL_STEPS = MSSTC_Dev_1.fullSteps;
    RGF_MOTOR_MICROSTEPPING = MSSTC_Dev_1.microstepping;

    RGF_MOTOR_ENABLE = MSSTC_Dev_1.enable;
    RGF_MOTOR_DIRECTION = MSSTC_Dev_1.direction;
    RGF_MOTOR_SPEED = MSSTC_Dev_1.fullSpeed;
    RGF_MOTOR_UNIT_DIVISION = MSSTC_Dev_1.unitDivision;
    RGF_MOTOR_ACCELERATION = MSSTC_Dev_1.acceleration;
    RGF_MOTOR_DECELERATION = MSSTC_Dev_1.deceleration;

    RGF_MOTOR_MODE = MSSTC_Dev_1.mode;
    RGF_MOTOR_ANGLE_LOCK_TARGET = MSSTC_Dev_1.angleLock;
    RGF_MOTOR_AUTO_POWER = MSSTC_Dev_1.powerOnlyRun;
    RGF_MOTOR_RUN_STATE = MSSTC_Dev_1.runState;
    RGF_MOTOR_RUN = 1;
    RGF_MOTOR_STEP_COUNT = 1600;
}
void MSSTC_1_Cycle(void){
    if(MSSTC_IsRunning(&MSSTC_Dev_1) != 1){
        MSSTC_Dev_1.fullSteps = (uint16_t)RGF_MOTOR_FULL_STEPS;
        if(MSSTC_Dev_1.microstepping != (uint16_t)RGF_MOTOR_MICROSTEPPING){
            MSSTC_Dev_1.microstepping = (uint16_t)RGF_MOTOR_MICROSTEPPING;
            MSSTC_Dev_1.enSet(0);
            if(MSSTC_Dev_1.microstepping == 8){
                HAL_GPIO_WritePin(MS_MS1_GPIO_Port, MS_MS1_Pin, GPIO_PIN_RESET);
                HAL_GPIO_WritePin(MS_MS2_GPIO_Port, MS_MS2_Pin, GPIO_PIN_RESET);
            }
            else if(MSSTC_Dev_1.microstepping == 16){
                HAL_GPIO_WritePin(MS_MS1_GPIO_Port, MS_MS1_Pin, GPIO_PIN_SET);
                HAL_GPIO_WritePin(MS_MS2_GPIO_Port, MS_MS2_Pin, GPIO_PIN_SET);
            }
            else if(MSSTC_Dev_1.microstepping == 32){
                HAL_GPIO_WritePin(MS_MS1_GPIO_Port, MS_MS1_Pin, GPIO_PIN_RESET);
                HAL_GPIO_WritePin(MS_MS2_GPIO_Port, MS_MS2_Pin, GPIO_PIN_SET);
            }
            else if(MSSTC_Dev_1.microstepping == 64){
                HAL_GPIO_WritePin(MS_MS1_GPIO_Port, MS_MS1_Pin, GPIO_PIN_SET);
                HAL_GPIO_WritePin(MS_MS2_GPIO_Port, MS_MS2_Pin, GPIO_PIN_RESET);
            }
            if(MSSTC_Dev_1.powerOnlyRun == 0){
                MSSTC_Dev_1.enSet(MSSTC_Dev_1.enable);
            }
        }

        if(MSSTC_Dev_1.enable != (uint8_t)RGF_MOTOR_ENABLE){
            MSSTC_Dev_1.enable = (uint8_t)RGF_MOTOR_ENABLE;
            if(MSSTC_Dev_1.powerOnlyRun == 0){
                MSSTC_Dev_1.enSet(MSSTC_Dev_1.enable);
            }
        }
        if(MSSTC_Dev_1.direction != (uint8_t)RGF_MOTOR_DIRECTION){
            MSSTC_Dev_1.direction = (uint8_t)RGF_MOTOR_DIRECTION;
            MSSTC_Dev_1.dirSet(MSSTC_Dev_1.direction);
        }
        MSSTC_Dev_1.fullSpeed = (uint16_t)RGF_MOTOR_SPEED;
        MSSTC_Dev_1.unitDivision = (uint8_t)RGF_MOTOR_UNIT_DIVISION;
        MSSTC_Dev_1.acceleration = (uint16_t)RGF_MOTOR_ACCELERATION;
        MSSTC_Dev_1.deceleration = (uint16_t)RGF_MOTOR_DECELERATION;

        MSSTC_Dev_1.mode = (uint8_t)RGF_MOTOR_MODE;
        if(MSSTC_Dev_1.angleLock != (uint8_t)RGF_MOTOR_ANGLE_LOCK_TARGET){
            MSSTC_Dev_1.angleLock = (uint8_t)RGF_MOTOR_ANGLE_LOCK_TARGET;
            if(MSSTC_Dev_1.mode == 1){

            }
        }
        if(MSSTC_Dev_1.powerOnlyRun != (uint8_t)RGF_MOTOR_AUTO_POWER){
            MSSTC_Dev_1.powerOnlyRun = (uint8_t)RGF_MOTOR_AUTO_POWER;
            if(MSSTC_Dev_1.powerOnlyRun == 0){
                MSSTC_Dev_1.enSet(MSSTC_Dev_1.enable);
            }
            else{
                MSSTC_Dev_1.enSet(0);
            }
        }
    }


    MT6816_1_UpdateCycle();
    MSSTC_UpdateAngle(&MSSTC_Dev_1,
        MT6816_ATY_Dev_1.turnCount,
        MT6816_ATY_Dev_1.angleTotal + MT6816_ATY_Dev_1.angleStart);
    if((uint8_t)RGF_MOTOR_RUN == 1){
        MT6816_TurnCountOffsetStart(&MT6816_ATY_Dev_1, MSSTC_Dev_1.direction);
        MSSTC_Move(&MSSTC_Dev_1, RGF_MOTOR_STEP_COUNT, MT6816_ATY_Dev_1.angleStart);
    }
    else if((uint8_t)RGF_MOTOR_RUN == 2){
        MSSTC_Scram(&MSSTC_Dev_1);
    }
    RGF_MOTOR_RUN = 0;

    if(MSSTC_Dev_1.runState == MSSTC_STATE_STANDBY
        && MT6816_ATY_Dev_1.start == 1){
        MT6816_TurnCountOffsetStop(&MT6816_ATY_Dev_1);
    }


    RGF_MOTOR_ENABLE = MSSTC_Dev_1.enable;
    RGF_MOTOR_DIRECTION = MSSTC_Dev_1.direction;

    RGF_MOTOR_RUN_STATE = MSSTC_Dev_1.runState;

    RGF_MOTOR_CURRENT_STEPS = MSSTC_Dev_1.stepCountCurrent;
    RGF_MOTOR_ACCEL_STEPS = MSSTC_Dev_1.accelSteps;
    RGF_MOTOR_ACCEL_LIMIT = MSSTC_Dev_1.accelLimit;
    RGF_MOTOR_DECEL_STEPS = MSSTC_Dev_1.decelSteps;
    RGF_MOTOR_DECEL_START = MSSTC_Dev_1.decelStart;
}
*/

/* use
MSSTC_1_Init();
MSSTC_1_Cycle();


// Reg -------------------------------------------------------------------------
// motor fan
#define RGF_MOTOR_FAN_EN rgf[20]
#define RGF_MOTOR_FAN_BASE rgf[21]
#define RGF_MOTOR_FAN_LOW_T rgf[22]
#define RGF_MOTOR_FAN_HIGH_T rgf[23]
#define RGF_MOTOR_FAN_T_SPACE rgf[24]
// motor angle base
#define RGF_MOTOR_ANGLE_SET_ZERO rgf[25]
#define RGF_MOTOR_ANGLE_ZERO rgf[26]
#define RGF_MOTOR_ANGLE rgf[27]
#define RGF_MOTOR_ANGLE_ERR rgf[28]
// motor angle correction
#define RGF_MOTOR_ANGLE_ANGLE_START rgf[30]
#define RGF_MOTOR_ANGLE_TURN_COUNT rgf[31]
#define RGF_MOTOR_ANGLE_ANGLE_TOTAL rgf[32]
#define RGF_MOTOR_ANGLE_START rgf[33]

// useless motor params
#define RGF_MOTOR_FREQUENCY rgf[35]
#define RGF_MOTOR_DUTY_CYCLE rgf[36]

#define RGF_MOTOR_FULL_STEPS rgf[38]
#define RGF_MOTOR_MICROSTEPPING rgf[39]

// motor base
#define RGF_MOTOR_ENABLE rgf[40]
#define RGF_MOTOR_DIRECTION rgf[41]
#define RGF_MOTOR_SPEED rgf[42]
#define RGF_MOTOR_UNIT_DIVISION rgf[43]
#define RGF_MOTOR_ACCELERATION rgf[44]
#define RGF_MOTOR_DECELERATION rgf[45]

#define RGF_MOTOR_MODE rgf[46]
#define RGF_MOTOR_ANGLE_LOCK_TARGET rgf[47]
#define RGF_MOTOR_AUTO_POWER rgf[48]
#define RGF_MOTOR_RUN_STATE rgf[49]
#define RGF_MOTOR_RUN rgf[50]
#define RGF_MOTOR_STEP_COUNT rgf[51]

#define RGF_MOTOR_CURRENT_STEPS rgf[52]
#define RGF_MOTOR_ACCEL_STEPS rgf[53]
#define RGF_MOTOR_ACCEL_LIMIT rgf[54]
#define RGF_MOTOR_DECEL_STEPS rgf[55]
#define RGF_MOTOR_DECEL_START rgf[56]

// FAN -------------------------------------------------------------------------
uint8_t fanLastLevel = 0;
void FAN_ControlCycle(void){
    if((uint8_t)RGF_MOTOR_FAN_EN == 1 && RGF_BOARD_TEMP > RGF_MOTOR_FAN_LOW_T){
        if((uint8_t)((RGF_BOARD_TEMP - RGF_MOTOR_FAN_LOW_T) / RGF_MOTOR_FAN_T_SPACE) + 1 != fanLastLevel){
            fanLastLevel = (uint8_t)((RGF_BOARD_TEMP - RGF_MOTOR_FAN_LOW_T) / RGF_MOTOR_FAN_T_SPACE) + 1;
            HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
            __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1,
                RGF_MOTOR_FAN_BASE + ((1000 - RGF_MOTOR_FAN_BASE) * fanLastLevel
                    / ((RGF_MOTOR_FAN_HIGH_T - RGF_MOTOR_FAN_LOW_T) / RGF_MOTOR_FAN_T_SPACE)));
            HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
        }
    }
    else{
        __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, 0);
        HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
        fanLastLevel = 0;
    }
}
void FAN_Init(void){
    RGF_MOTOR_FAN_EN = 1;
    RGF_MOTOR_FAN_BASE = 200;
    RGF_MOTOR_FAN_LOW_T = 50;
    RGF_MOTOR_FAN_HIGH_T = 80;
    RGF_MOTOR_FAN_T_SPACE = 5;
}
*/

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

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