/**
* @file MOTOR_STEP_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 functions of closed loop stepper motor control for C platform
*
* @note Slave Mode: Disable
*       Trigger Source: Disable
*       Clock Source: Internal Clock
*       Channel2: PWM Generation CH2
*       Prescaler: 71
*       Counter Mode: Up
*       Counter Period: 999
*       Internal Clock Division: No Division
*       auto-reload preload: Disable
*       Master/Slave Mode (MSM bit): Disable
*       Trigger Event Selection: Update Event
*       PWM Generation Channel 2 Mode: PWM mode 2
*       Pulse: 499
*       Output compare preload: Enable
*       Fast Mode: Disable
*       CH Polarity: High
*       CH Idle State: Reset
*
*       htim1.Init.Prescaler = 71;
*       htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
*       htim1.Init.Period = 999;
*       htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
*       htim1.Init.RepetitionCounter = 0;
*       htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
*       sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
*       sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE;
*       sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
*       sConfigOC.OCMode = TIM_OCMODE_PWM2;
*       sConfigOC.Pulse = 499;
*       sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
*       sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
*       sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
*       sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
*       sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
*       htim2.Init.Prescaler = 0;
*       htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
*       htim2.Init.Period = 65535;
*       htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
*       htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
*       sSlaveConfig.SlaveMode = TIM_SLAVEMODE_GATED;
*       sSlaveConfig.InputTrigger = TIM_TS_ITR0;
*       sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
*       sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
*
*       import angle to use COMPENSATE
*
* @version
*       - 1_00_260109 > ATY
*           -# Preliminary version
********************************************************************************
*/

#ifndef __MOTOR_STEP_C_ATY_C
#define __MOTOR_STEP_C_ATY_C

#include "MOTOR_STEP_C_ATY.h"
#define MOTOR_STEP_C_ATY_TAG "\r\n[MOTOR_STEP_C_ATY] "

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

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

uint8_t MSC_MoveToAngleSteps(struct MOTOR_STEP_C_ATY_Dev* dev,
    float targetAngle, float currentAngle){
    uint8_t dir = __ATY_PN_P;
    float angleSpace = (360.0 / dev->fullSteps / dev->microstepping);
    if(currentAngle > targetAngle + angleSpace){
        if(currentAngle - targetAngle > 180.0){
            dev->stepCount = (targetAngle + 360.0 - currentAngle) / angleSpace;
        }
        else{
            dir = __ATY_PN_N;
            dev->stepCount = (currentAngle - targetAngle) / angleSpace;
        }
        dev->dirSet(dir);
        dev->pwmSetCount(dev->frequency, dev->dutyCycle, dev->stepCount);
        return 1;
    }
    else if(targetAngle > currentAngle + angleSpace){
        if(targetAngle - currentAngle > 180.0){
            dir = __ATY_PN_N;
            dev->stepCount = (currentAngle + 360.0 - targetAngle) / angleSpace;
        }
        else{
            dev->stepCount = (targetAngle - currentAngle) / angleSpace;
        }
        dev->dirSet(dir);
        dev->pwmSetCount(dev->frequency, dev->dutyCycle, dev->stepCount);
        return 1;
    }
    else{
        return 0;
    }
}

uint8_t MSC_Cycle(struct MOTOR_STEP_C_ATY_Dev* dev,
    uint8_t mode, uint8_t enable, uint8_t direction,
    uint16_t stepCount,
    float frequency, float dutyCycle, float speed,
    float currentAngle, float targetAngle){
    float angleSpace = (360.0 / dev->fullSteps / dev->microstepping);
    uint8_t regUpdateFlag = 0;

    if(mode != dev->mode){
        dev->mode = mode;
        regUpdateFlag = 1;
    }
    if(enable != dev->enable){
        dev->enable = enable;
        if(dev->autoPower == 0) dev->enSet(dev->enable);
    }
    if(direction != dev->direction){
        dev->direction = direction;
        dev->dirSet(dev->direction);
    }
    if(targetAngle != dev->targetAngle){
        while(targetAngle >= 360.0) targetAngle -= 360.0;
        dev->targetAngle = targetAngle;
        if(regUpdateFlag != 1) regUpdateFlag = 2;
    }

    if(stepCount != dev->stepCount){
        dev->stepCount = stepCount;
        if(dev->mode == MOTOR_STEP_C_MODE_POST_MOTION)
            regUpdateFlag = 1;
    }

    if(speed != dev->speed){
        dev->speed = speed;
        dev->frequency = (dev->speed * (dev->fullSteps
            * dev->microstepping) / 60.0);
        dev->dutyCycle = 50.0;

        if(dev->mode == MOTOR_STEP_C_MODE_NOCOUNT){
            regUpdateFlag = 1;
        }
        else{
            if(regUpdateFlag != 1) regUpdateFlag = 2;
        }
    }
    else if(frequency != dev->frequency
        || dutyCycle != dev->dutyCycle){
        dev->frequency = frequency;
        dev->dutyCycle = dutyCycle;
        if(dev->dutyCycle < 0) dev->dutyCycle = 0;
        else if(dev->dutyCycle > 100) dev->dutyCycle = 100;
        dev->speed = (dev->frequency * 60.0
            / (dev->fullSteps * dev->microstepping));

        if(dev->mode == MOTOR_STEP_C_MODE_NOCOUNT){
            regUpdateFlag = 1;
        }
        else{
            if(regUpdateFlag != 1) regUpdateFlag = 2;
        }
    }

    if(dev->mode == MOTOR_STEP_C_MODE_POST_MOTION
        && dev->stepCount == 0){
        if(dev->angleLock == 1){
            dev->positionFixFlag++;
        }
        if(dev->positionFixFlag != 0){
            if(MSC_MoveToAngleSteps(dev, dev->targetAngle, currentAngle) == 0){
                dev->positionFixFlag = 0;
                // dev->startAngle = 0.0;
                // dev->targetAngle = 0.0;
            }
            else{
                dev->positionFixFlag++;
            }
        }
    }

    if(dev->autoPower == 1){
        if(regUpdateFlag == 0
            && dev->stepCount == 0
            && dev->positionFixFlag == 0
            && dev->mode != MOTOR_STEP_C_MODE_NOCOUNT){
            dev->enSet(0);
        }
        else{
            dev->enSet(1);
        }
    }
    else{
        dev->enSet(dev->enable);
    }
    if(regUpdateFlag == 1 && dev->mode == MOTOR_STEP_C_MODE_POST_MOTION
        && dev->positionFixFlag == 0){
        dev->startAngle = currentAngle;
        dev->targetAngle = ((dev->stepCount % (dev->fullSteps * dev->microstepping)) * 360.0)
            / (dev->fullSteps * dev->microstepping) + dev->startAngle;
        while(dev->targetAngle >= 360.0) dev->targetAngle -= 360.0;

        dev->pwmSetCount(dev->frequency, dev->dutyCycle, dev->stepCount);
        dev->positionFixFlag++;
    }
    else if(regUpdateFlag == 1 && dev->mode == MOTOR_STEP_C_MODE_NOCOUNT){
        dev->pwmSet(dev->frequency, dev->dutyCycle);
    }
    return regUpdateFlag;
}

#endif /* __MOTOR_STEP_C_ATY_C */


/************************************ etc *************************************/
/* init
// MOTOR Close -----------------------------------------------------------------
#include "MOTOR_STEP_C_ATY.h"
uint8_t MOTOR_STEP_C_1_EnSet(uint8_t en){
    HAL_GPIO_WritePin(MS_EN_GPIO_Port, MS_EN_Pin,
        (en == 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
    return __ATY_OK;
}
uint8_t MOTOR_STEP_C_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 __ATY_OK;
}
void MOTOR_STEP_C_1_PWM_NoCount(float freq, float dutycycle){
    uint32_t ticksPeriod = 1000000U / freq;
    uint32_t ticksPulse = ticksPeriod * dutycycle / 100.0;
    HAL_TIM_PWM_Stop(&MS_1_TIM, MS_1_TIM_CHANNEL);
    __HAL_TIM_SET_COUNTER(&MS_1_TIM, 0);
    __HAL_TIM_SET_AUTORELOAD(&MS_1_TIM, ticksPeriod - 1);
    __HAL_TIM_SET_COMPARE(&MS_1_TIM, MS_1_TIM_CHANNEL, ticksPulse - 1);
    HAL_TIM_PWM_Start(&MS_1_TIM, MS_1_TIM_CHANNEL);
}
void MOTOR_STEP_C_1_PWM_Count(float freq, float dutycycle, uint16_t step){
    uint32_t ticksPeriod = 1000000U / freq;
    uint32_t ticksPulse = ticksPeriod * dutycycle / 100.0;
    HAL_TIM_PWM_Stop(&MS_1_TIM, MS_1_TIM_CHANNEL);
    HAL_TIM_Base_Stop_IT(&htim2);
    if(step != 0){
        __HAL_TIM_SET_COUNTER(&MS_1_TIM, 0);
        // Set 1 to make sure the system can generate one pulse
        __HAL_TIM_SET_COUNTER(&htim2, 1);
        // init counter 1, then do not need -1
        __HAL_TIM_SET_AUTORELOAD(&htim2, step);
        __HAL_TIM_SET_AUTORELOAD(&MS_1_TIM, ticksPeriod - 1);
        __HAL_TIM_SET_COMPARE(&MS_1_TIM, MS_1_TIM_CHANNEL, ticksPulse - 1);
        HAL_TIM_Base_Start_IT(&htim2);
        HAL_TIM_PWM_Start(&MS_1_TIM, MS_1_TIM_CHANNEL);
    }
}
struct MOTOR_STEP_C_ATY_Dev MOTOR_STEP_C_ATY_Dev_1 = {
    .enSet = MOTOR_STEP_C_1_EnSet,
    .dirSet = MOTOR_STEP_C_1_DirSet,

    .pwmSet = MOTOR_STEP_C_1_PWM_NoCount,
    .pwmSetCount = MOTOR_STEP_C_1_PWM_Count,

    .mode = 0xFF,
    .enable = 0xFF,
    .direction = 0xFF,
    .stepCount = 0xFFFF,
    .frequency = -1.0,
    .dutyCycle = -1.0,
    .speed = -1.0,

    .autoPower = 0,
    .positionFixFlag = 0,
    .startAngle = 0.0,
    .targetAngle = 0.0,
    .forbidReverseCorrection = 0,
    .angleLock = 0,

    .fullSteps = 200,
    .microstepping = 8,

    .lock = __ATY_UNLOCKED
};
void MOTOR_STEP_C_1_ValueUpdateBack(struct MOTOR_STEP_C_ATY_Dev* dev){
    RGF_MOTOR_STEP_COUNT = dev->stepCount;
    RGF_MOTOR_FREQUENCY = dev->frequency;
    RGF_MOTOR_DUTY_CYCLE = dev->dutyCycle;
    RGF_MOTOR_SPEED = dev->speed;
    RGF_MOTOR_ANGLE_LOCK_TARGET = dev->targetAngle;
}
void MOTOR_STEP_C_1_Init(void){
    RGF_MOTOR_MODE = MOTOR_STEP_C_MODE_POST_MOTION;
    RGF_MOTOR_ENABLE = 0;
    RGF_MOTOR_DIRECTION = __ATY_PN_P;
    RGF_MOTOR_STEP_COUNT = 6;
    RGF_MOTOR_FREQUENCY = (1600.0 / 60.0);
    RGF_MOTOR_DUTY_CYCLE = 50.0;
    RGF_MOTOR_SPEED = 60.0;
    RGF_MOTOR_ANGLE_LOCK_TARGET = 0;
    // Prevent from getting into an interruption from the very beginning
    __HAL_TIM_CLEAR_FLAG(&htim2, TIM_FLAG_UPDATE);
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim){
    if(htim == &htim4){
        UserTimerLoop_Cycle1ms(&HW_TIMER_ATY_Dev_4);
    }
    if((uint8_t)RGF_MOTOR_MODE == 0 && htim == &htim2){
        // HAL_TIM_PWM_Stop(&MS_1_TIM, MS_1_TIM_CHANNEL);
        // HAL_TIM_Base_Stop_IT(&htim2);
        // RGF_MOTOR_STEP_COUNT = 0;
        // MOTOR_STEP_C_ATY_Dev_1.stepCount = 0;
        // MOTOR_STEP_C_ATY_Dev_1.dirSet(MOTOR_STEP_C_ATY_Dev_1.direction);
    }
}
*/

/* use
MOTOR_STEP_C_1_Init();

if(MSC_Cycle(&MOTOR_STEP_C_ATY_Dev_1,
    (uint8_t)RGF_MOTOR_MODE,
    (uint8_t)RGF_MOTOR_ENABLE,
    (uint8_t)RGF_MOTOR_DIRECTION,
    (uint16_t)RGF_MOTOR_STEP_COUNT,
    RGF_MOTOR_FREQUENCY,
    RGF_MOTOR_DUTY_CYCLE,
    RGF_MOTOR_SPEED,
    RGF_MOTOR_ANGLE,
    RGF_MOTOR_ANGLE_LOCK,
    RGF_MOTOR_ANGLE_LOCK_TARGET) != 0){
    MOTOR_STEP_C_1_ValueUpdateBack(&MOTOR_STEP_C_ATY_Dev_1);
}
*/

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

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