/**************************************************************************
*
* Copyright (c) 2025-2025 Diality Inc. - All Rights Reserved.
*
* THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN
* WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER.
*
* @file    EjectorMotor.c
*
* @author  (last)      Sean
* @date    (last)      13-May-2025
*
* @author  (original)  Sean
* @date    (original)  13-May-2025
*
***************************************************************************/

#include "AlarmMgmtTD.h"
#include "EjectorMotor.h"
#include "FpgaTD.h"
#include "Messaging.h"

/**
 *  @addtogroup EjectorMotor
 *  @{
 */

// ********** private definitions **********

#define EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP  0xFFFFFFFF    ///< Ejector motor microstep toggle time setting to indicate zero speed (stopped).

#define EJECTOR_STEPS_PER_REV                         48.0F         ///< Number of steps per revolution (7.5 degrees per step).
#define EJECTOR_MICRO_STEPS_PER_STEP                  32.0F         ///< Number of micro-steps per step.
#define EJECTOR_TOGGLES_PER_STEP                      2.0F          ///< Stepper motor driver toggles per step or microstep.

#define EJECTOR_MOTOR_START_RAMP_SPEED                300000        ///< Starting speed for all ejector operations to ramp up from.
#define EJECTOR_MOTOR_RAMP_DIVISOR                    5             ///< Used for ramping profile.

/// Number of ejector motor micro steps per revolution.
#define EJECTOR_MICRO_STEPS_PER_REV                  ( EJECTOR_STEPS_PER_REV * EJECTOR_MICRO_STEPS_PER_STEP )

// Bit definitions for ejector motor control register
#define EJECTOR_MOTOR_CONTROL_SLEEP_OFF               0x40          ///< Ejector motor control register bit for sleep mode (active low).
#define EJECTOR_MOTOR_CONTROL_NOT_RESET               0x20          ///< Ejector motor control register bit for resetting stepper motor (active low).
#define EJECTOR_MOTOR_CONTROL_ENABLE                  0x00          ///< Ejector motor control register bit mask for enable.
#define EJECTOR_MOTOR_CONTROL_DISABLE                 0x10          ///< Ejector motor control register bit for enable (active low).
#define EJECTOR_MOTOR_CONTROL_REVERSE_DIR             0x00          ///< Ejector motor control register bit for direction (1=fwd, 0=rev).
#define EJECTOR_MOTOR_CONTROL_FORWARD_DIR             0x08          ///< Ejector motor control register bit mask for forward direction.
#define EJECTOR_MOTOR_CONTROL_32TH_STEP               0x03          ///< Ejector motor control register bits for 1/32 micro-stepping mode.

/// Control bits to run ejector motor in reverse direction
static const U08 EJECTOR_MOTOR_CONTROL_RUN_REVERSE = EJECTOR_MOTOR_CONTROL_SLEEP_OFF |
                                                     EJECTOR_MOTOR_CONTROL_NOT_RESET |
                                                     EJECTOR_MOTOR_CONTROL_ENABLE |
                                                     EJECTOR_MOTOR_CONTROL_REVERSE_DIR |
                                                     EJECTOR_MOTOR_CONTROL_32TH_STEP;
/// Control bits to run ejector motor in forward direction
static const U08 EJECTOR_MOTOR_CONTROL_RUN_FORWARD = EJECTOR_MOTOR_CONTROL_SLEEP_OFF |
                                                     EJECTOR_MOTOR_CONTROL_NOT_RESET |
                                                     EJECTOR_MOTOR_CONTROL_ENABLE |
                                                     EJECTOR_MOTOR_CONTROL_FORWARD_DIR |
                                                     EJECTOR_MOTOR_CONTROL_32TH_STEP;

/// Payload record structure for ejector motor set request
typedef struct
{
    F32 setSpeed;                                                   ///< Set speed for ejector motor (in RPM).  Negative RPM indicates reverse direction.
} EJECTOR_MOTOR_SET_CMD_PAYLOAD_T;

// ********** private data **********

static F32 currentEjectorMotorSetSpeed;                             ///< Current ejector motor set speed (in RPM).
static U32 ejectorMotorSetToggleTime;                               ///< Time (in uSec) between microstep toggles.
static U32 ejectorMotorRampUpToggleTime;                            ///< Current ramp time (in uSec) between microstep toggles.
static U32 ejectorMotorRampTimerCtr;                                ///< Used to track ramp up time.
static BOOL ejectorMotorRampUpInProgress;                           ///< Flag indicating whether a ramp up is in progress.

// ********** private function prototypes **********

static U32 calcEjectorStepToggleTimeFromSetSpeed( F32 rpm );

/*********************************************************************//**
 * @brief
 * The initEjectorMotor function initializes the ejector motor driver unit.
 * @details \b Inputs: none
 * @details \b Outputs: EjectorMotor unit initialized.
 * @return none
 *************************************************************************/
void initEjectorMotor(void)
{
    currentEjectorMotorSetSpeed = 0.0F;
    ejectorMotorSetToggleTime = EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP;
    ejectorMotorRampUpToggleTime = EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP;
    ejectorMotorRampTimerCtr = 0;
    ejectorMotorRampUpInProgress = FALSE;

    setH5StepToggleTime( ejectorMotorSetToggleTime );
    setH5ControlFlags( EJECTOR_MOTOR_CONTROL_RUN_FORWARD );
}

/*********************************************************************//**
 * @brief
 * The setEjectorMotorSpeed function sets the ejector motor to the given
 * set speed and direction.
 * @details \b Inputs: currentEjectorMotorSetSpeed
 * @details \b Outputs: currentEjectorMotorSetSpeed, ejectorMotorSetToggleTime
 * ejectorMotorRampUpToggleTime, ejectorMotorRampUpInProgress
 * @param rpm Speed (rpm) to set the ejector motor to (negative value indicates reverse direction)
 * @return TRUE if ejector motor speed set successfully, FALSE if not.
 *************************************************************************/
BOOL setEjectorMotorSpeed( F32 rpm )
{
    BOOL result = FALSE;

    // if starting motor, motor must be stopped first
    if ( ( fabs(rpm) > 0.0F ) && ( fabs( currentEjectorMotorSetSpeed ) < NEARLY_ZERO ) )
    {
        U32 stepTime = calcEjectorStepToggleTimeFromSetSpeed( rpm );
        U08 ctrl     = EJECTOR_MOTOR_CONTROL_RUN_FORWARD;

        currentEjectorMotorSetSpeed = rpm;
        if ( rpm < 0.0F )
        {
            ctrl = EJECTOR_MOTOR_CONTROL_RUN_REVERSE;
        }
        ejectorMotorSetToggleTime = stepTime;
        ejectorMotorRampUpToggleTime = EJECTOR_MOTOR_START_RAMP_SPEED;
        setH5ControlFlags( ctrl );
        setH5StepToggleTime( ejectorMotorRampUpToggleTime );
        ejectorMotorRampUpInProgress = TRUE;

        result = TRUE;
    }
    // if stopping motor, motor must be running first
    else if ( ( fabs(rpm) < NEARLY_ZERO ) && ( fabs( currentEjectorMotorSetSpeed ) > 0.0F ) )
    {
        ejectorMotorRampUpInProgress = FALSE;
        ejectorMotorSetToggleTime = EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP;
        ejectorMotorRampUpToggleTime = EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP;
        currentEjectorMotorSetSpeed = 0.0F;
        setH5StepToggleTime( ejectorMotorSetToggleTime );
        result = TRUE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getEjectorMotorSetSpeed function gets the current set ejector motor
 * speed (in RPM).  If the direction is reverse, the reported speed will be
 * negative.
 * @details \b Inputs: currentEjectorMotorSetSpeed
 * @details \b Outputs: none
 * @return currentEjectorMotorSetSpeed
 *************************************************************************/
F32 getEjectorMotorSetSpeed( void )
{
    return currentEjectorMotorSetSpeed;
}

/*********************************************************************//**
 * @brief
 * The execEjectorMotorRamping function manages the ejector motor ramping.
 * @note This ramping function is executed within the priority task while
 * other ejector related functions occur within the general task.  Therefore
 * care should be taken when setting the ramp in progress flag.
 * @details \b Inputs: ejectorMotorRampUpInProgress
 * @details \b Outputs: none
 * @return none
 *************************************************************************/
void execEjectorMotorRamping( void )
{
    if ( TRUE == ejectorMotorRampUpInProgress )
    {
        ejectorMotorRampTimerCtr++;
        if ( ejectorMotorRampUpToggleTime > ejectorMotorSetToggleTime )
        {
            ejectorMotorRampUpToggleTime = (U32)( (F32)EJECTOR_MOTOR_START_RAMP_SPEED /
                                                  ( (F32)( ejectorMotorRampTimerCtr * ejectorMotorRampTimerCtr * ejectorMotorRampTimerCtr ) /
                                                    (F32)EJECTOR_MOTOR_RAMP_DIVISOR ) );
            if ( ejectorMotorRampUpToggleTime > ejectorMotorSetToggleTime )
            {
                setH5StepToggleTime( ejectorMotorRampUpToggleTime );
            }
            else
            {
                setH5StepToggleTime( ejectorMotorSetToggleTime );
                ejectorMotorRampUpInProgress = FALSE;
            }
        }
    }
}

/*********************************************************************//**
 * @brief
 * The calcEjectorStepToggleTimeFromSetSpeed function calculates the stepper
 * toggle period for a given set speed (in RPM).
 * @details Inputs: none
 * @details Outputs: none
 * @param rpm Speed to convert to step toggle time (in uSec)
 * @return toggle time for set speed
 *************************************************************************/
static U32 calcEjectorStepToggleTimeFromSetSpeed( F32 rpm )
{
    U32 result = EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP;
    F64 temp;
    F32 conv;

    // Toggle time is direction agnostic, so ignore direction implied by sign.
    rpm = fabs( rpm );

    // Convert given speed to stepper toggle period
    temp = (F64)EJECTOR_MICRO_STEPS_PER_REV * (F64)rpm;      // = uSteps/min
    temp /= (F64)(SEC_PER_MIN);                              // = uSteps/sec
    temp /= (F64)US_PER_SECOND;                              // = uSteps/uSec
    conv = (F32)temp * EJECTOR_TOGGLES_PER_STEP;             // = toggles/uSec
    conv = 1.0F / conv;                                      // = uSec/toggle
    result = (U32)FLOAT_TO_INT_WITH_ROUND( conv );

    ejectorMotorRampUpToggleTime = EJECTOR_MOTOR_START_RAMP_SPEED;

    return result;
}


/*************************************************************************
 *                       TEST SUPPORT FUNCTIONS
 *************************************************************************/


/*********************************************************************//**
 * @brief
 * The testSetEjectorMotorSpeed function sets the ejector motor to a given
 * set speed.
 * @details \b Inputs: none
 * @details \b Outputs: none
 * @param message set message from Dialin which includes the set speed being
 * requested.
 * @return TRUE if set request is successful, FALSE if not
 *************************************************************************/
BOOL testSetEjectorMotorSpeed( MESSAGE_T *message )
{
    BOOL result = FALSE;

    // Verify tester has logged in with TD and override type is valid
    if ( TRUE == isTestingActivated() )
    {
        // Verify payload length is valid
        if ( sizeof( F32 ) == message->hdr.payloadLen )
        {
            EJECTOR_MOTOR_SET_CMD_PAYLOAD_T payload;

            memcpy( &payload, message->payload, sizeof(EJECTOR_MOTOR_SET_CMD_PAYLOAD_T) );
            result = setEjectorMotorSpeed( payload.setSpeed );
        }
    }

    return result;
}

/**@}*/