/**
 * @file adcs_vapi_common.c
 *
 * @brief This file contains OS abstracted Cube ADCS functions for commissioning
 *        the satellite
 *
 * @copyright Copyright 2023 Antaris, Inc.
 */
/*********************************************************************/

#include "adcs_vapi.h"
#include "adcs_vapi_common.h"
#include "adcs_va.h"
#include "logger.h"
#include "adcs_vapi_fw.h"
#include "sm_api.h"
#include "exo_io_al_sos_timer.h"

#define TEST_HOOK (1) ///< Testing Print enable

#define ADCS_MODE_ENABLE  ///< Enable run mode and yspin
                          ///< control mode

/**
 * @brief Extern variables for thread and IPC operations
 */
extern os_itc_msg_handle_t adcs_csw_trans_msg;

/// Buffer to store received TLM data
uint8 adcs_bypass_tlm_buffer[1024] = {0};


/*
 * @brief Function to request rate sensor tlm
 * @param[out] rate_sens_rates : Pointer to CUBEACP_AdcsMeasure_t
 *
 * @return Return value of the interface send/receive api
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_request_rate_sens_rates_tlm(CUBEACP_AdcsMeasure_t\
        *rate_sens_rates);

/*
 * @brief Function to request estimated rate sensor tlm
 * @param[out] est_rate_sens_rates : Pointer to CUBEACP_AdcsState_t
 *
 * @return Return value of the interface send/receive api
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_request_est_rate_sens_rates_tlm(CUBEACP_AdcsState_t\
        *est_rate_sens_rates);

/*
 * @brief Function to compare estimated and measured angular rates
 *
 * @return Return value of the interface send/receive api
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_test_angular_rate_measurement(void);

/*
 * @brief Function to request gyro configuration
 *
 * @return Return value of the interface send/receive api
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_request_gyro_cfg(void);

/*
 * @brief Function to analyse the angular rate and return the max rate
 *        in a total of 10 measurements
 * @param[out] max_rate : Pointer to max angular rate
 *
 * @return Return value of the interface send/receive api
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_analyse_mems_rate(CUBEACP_AdcsMeasure_t *max_rate);

/*
 * @brief Function that performs detumbling
 * @param[in] init_ctrl_mode : control mode to start the detumbling with
 *
 * @return Return value of the interface send/receive api and detumbling status
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_init_detumbling(CUBEACP_ConModeSelect_t \
        init_ctrl_mode);

/*
 * @brief Function to request nadir vector tlm
 * @param[out] nadir_vec : Pointer to nadir vector
 *
 * @return Return value of the interface send/receive api
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_request_nadir_vec_tlm(CUBEACP_AdcsMeasure_t* \
        nadir_vec);

/*
 * @brief Function to request Raw Star Tracker tlm
 * @param[out] raw_star : Pointer to raw star structure
 *
 * @return Return value of the interface send/receive api
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_request_raw_star_tlm(CUBEACP_RawStarTracker_t* \
        raw_star);
/*
 * @brief Function to request fine sun vector tlm
 * @param[out] fine_sun   : Pointer to fine sun vector
 *
 * @return Return value of the interface send/receive api and the test
 result
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_request_fine_sun_tlm(CUBEACP_AdcsMeasure_t* \
        fine_sun);

/*
 * @brief Function to request fine sun vector tlm
 * @param[out] coarse_sun   : Pointer to coarse sun vector
 *
 * @return Return value of the interface send/receive api and the test
 result
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_request_coarse_sun_tlm(CUBEACP_AdcsMeasure_t* \
        coarse_sun);

/*
 * @brief Function to test the fine sun sensor
 *
 * @return test result
 * @retval 0 -> Success
 *         Other values -> Error
 */
static e_adcs_status adcs_test_fine_sun_sens(void);

/*
 * @brief Function to compare the difference of a and b with precision
 */
uint8 adcs_double_compare(double a, double b, double precision)
{
    uint8 status = OPERATION_FAILURE;
    double diff = a - b;

    status = (diff < precision) && (-diff < precision);
    if(status == 1)
    {
        status = OPERATION_SUCCESS;
    }
    else
    {
        status = OPERATION_FAILURE;
    }
    return status;
}
/**
 * @brief Function to handle adcs reset command
 */
e_adcs_status vapi_adcs_reset(void)
{
    e_adcs_status status = TC_TM_ERROR;
    COMMON_ExtendedIdentification_t boot_sts;
    e_tctm_status result;
    uint16 expected_boot_ctr;
    uint16 size = 0;
    uint8 *tcBuffer = NULL;

    /*
     * This delay is needed to make sure that cube adcs doesn't go to boot
     * loader mode instead of application mode
     */
    os_delay(5000);
    result = adcs_request_boot_prog_status_tlm(&boot_sts);
    if(TLM_OK == result)
    {
        expected_boot_ctr = (boot_sts.bootCounter) + 1;
        tcBuffer = os_malloc_eram(2);
        if(NULL != tcBuffer)
        {
            size = COMMON_FormatResetCmd(tcBuffer, 0x5A);
            result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
           //Reset command response not get while using UART interface
           //if(TCMD_OK == result)
            {
                os_delay(10000);
                result = adcs_request_boot_prog_status_tlm(&boot_sts);
                if(TLM_OK == result)
                {
                    if(boot_sts.bootCounter == expected_boot_ctr)
                    {
                        status = OPERATION_SUCCESS;
                    }
                    else
                    {
                        status = OPERATION_FAILURE;
                    }
                }
            }
            os_free(tcBuffer);
        }
    }
    return status;
}

/**
 * @brief Function to handle adcs boot command
 */
e_adcs_status vapi_adcs_boot(void)
{
    e_adcs_status status = TC_TM_ERROR;
    uint8 expected_node_type = 10;
    COMMON_Identification_t id_stat;
    e_tctm_status result;

    result = adcs_request_identification_tlm(&id_stat);
    if(TLM_OK == result)
    {
        if(id_stat.nodeType == expected_node_type)
        {
            status = OPERATION_SUCCESS;
        }
        else
        {
            status = OPERATION_FAILURE;
        }
    }

    return status;
}

/**
 * @brief Function to handle adcs log configuration command
 */
e_adcs_status vapi_adcs_cfg_log(void)
{
    e_adcs_status status = OPERATION_SUCCESS;
    return status;
}

/**
 * @brief Function to handle adcs detumble command
 */
e_adcs_status vapi_adcs_detumble(void)
{
    DEBUG_CPRINT(("\n\r###Starting of vapi_adcs_detumble"));
    e_adcs_status status = TC_TM_ERROR;
    e_adcs_status detumble_status = OPERATION_FAILURE;
    CUBEACP_AdcsPower_t pwr_ctrl;
    e_tctm_status result;
    uint16 size = 0;
    CUBEACP_ConModeSelect_t mode;
    double max_rate = 0;
    uint8 *tcBuffer = NULL;
    uint8 param[10] = {0x80,0x7D,0x00,0x58,0x00,0x00,0x00,0x18,0x00,0x00};

    // Generic tc buffer
    tcBuffer = os_malloc_eram(15);
    pwr_ctrl.cubeControlSignalPower = PowerSelect_PowOn;
    pwr_ctrl.cubeControlMotorPower = PowerSelect_PowOn;
    pwr_ctrl.cubeSense1Power = PowerSelectCubeSense_PowCsOff;
    pwr_ctrl.cubeSense2Power = PowerSelectCubeSense_PowCsOff;
    pwr_ctrl.cubeStarPower = PowerSelect_PowOff;
    pwr_ctrl.cubeWheel1Power = PowerSelect_PowOff;
    pwr_ctrl.cubeWheel2Power = PowerSelect_PowOff;
    pwr_ctrl.cubeWheel3Power = PowerSelect_PowOff;
    pwr_ctrl.motorPower = PowerSelect_PowOff;
    pwr_ctrl.gpsLnaPower = PowerSelect_PowOff;

    size = CUBEACP_FormatAdcsPowerCmd(tcBuffer, &pwr_ctrl);
    result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
    if(TCMD_OK == result)
    {
#ifdef ADCS_FLIGHT_READY
        //TODO: Enable for final build
        size = CUBEACP_FormatAdcsEnabledCmd(tcBuffer,\
        AdcsRunMode_AdcsEnabled);
        result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
#endif
        if(TCMD_OK == result)
        {
            adcs_log_sd_cube(param,10);
            /*
             * Determine the maximum angular rate and decide the initial
             * detumbling mode based on that
             */
            status = adcs_select_ctrl_mode(&mode, &max_rate);
#ifdef TEST_HOOK
            DEBUG_CPRINT(("CUBE ADCS TEST:Selected detumbling control mode is:"
                    "%d \r\n", mode));
            log_id_with_one_arg(LOG_INFO_L2,LOG_ADCS,\
            Adcs_selected_detumbling_ctrl_mode__u32_,mode);
            DEBUG_CPRINT(("CUBE ADCS TEST:Maximum rate  is : %lf \r\n", \
                        max_rate));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,Max_rate__u32__pt__u32_,\
            max_rate);
#endif
            if(OPERATION_SUCCESS == status)
            {
                /*
                 * Determine and set the rate sensor multiplier based on the
                 * maximum angular rate
                 */
                status = adcs_set_rate_sens_mult(max_rate);
                if(OPERATION_SUCCESS == status)
                {
#ifdef TEST_HOOK
                    adcs_request_gyro_cfg();
#endif
                    size = CUBEACP_FormatEstimModeCmd(
                            tcBuffer, EstimModeSelect_EstMemsRate);
                    result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
                    if(TCMD_OK == result)
                    {
                        if(TCMD_OK == result)
                        {
                            os_delay(300000);
                            status = adcs_test_angular_rate_measurement();
                            if(OPERATION_SUCCESS == status)
                            {
                                DEBUG_CPRINT(("CUBE ADCS TEST :STARTING TO "
                                        "DETUMBLE \n"));
                                log_id(LOG_INFO_L1,LOG_ADCS,\
                                STARTING_TO_DETUMBLE);
                                status = adcs_init_detumbling(mode);
                                if(OPERATION_SUCCESS == status)
                                {
                                    detumble_status = OPERATION_SUCCESS;
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    os_free(tcBuffer);
    status = detumble_status;

    return status;
}

/**
 * @brief Function to start detumble command
 */
static e_adcs_status adcs_init_detumbling(CUBEACP_ConModeSelect_t
        init_ctrl_mode)
{
    e_adcs_status status = OPERATION_FAILURE;
    int8 result = OPERATION_FAILURE;
    CUBEACP_ControlMode_t ctrl_mode;
    uint16 size = 0;
    // Maximum angular before Z-Thomson
    CUBEACP_AdcsMeasure_t max_rate_b_detumble;
    // Maximum angular after Z-Thomson for 600seconds
    CUBEACP_AdcsMeasure_t max_rate_a_detumble;
    // Maximum angular after Z-Thomson infinite timeout
    CUBEACP_AdcsMeasure_t max_rate_f_detumble;
    uint8 detumble_success = 0;
    uint8 tcBuffer[10] = {0};
#ifdef ADCS_MODE_ENABLE
    if(ConModeSelect_ConYspin != init_ctrl_mode)
#else
    if(ConModeSelect_ConSpecial1 != init_ctrl_mode)
#endif
    {
        ctrl_mode.controlModeSelection = init_ctrl_mode;
        ctrl_mode.timeout = 600;
        size = CUBEACP_FormatControlModeCmd(tcBuffer, &ctrl_mode);
        result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
        if(TCMD_OK == result)
        {
            // Wait for 600 seconds for the rates to reduce
            DEBUG_CPRINT(("CUBE ADCS: Waiting for 600 seconds \n"));
            log_id(LOG_INFO_L1,LOG_ADCS,Waiting_600_sec);
            os_delay(600000);
        }
        else
        {
            // TODO: Log error
            DEBUG_CPRINT(("CUBE ADCS: Failed to set control mode %d in initial "
                    "detumbling \n", init_ctrl_mode));
            log_id_with_one_arg(LOG_ERROR,LOG_ADCS,\
            Failed1_to_set_control_mode__u32__in_initial_detumbling,\
            init_ctrl_mode);
            return status;
        }
    }
    else
    {
        /*
         * init_ctrl_mode = ConModeSelect_ConSpecial1 which means the initial
         * angular rates are very low. So we can directly proceed to
         * ConModeSelect_ConSpecial1 AKA Z-Thomson
         */
    }

    // Get the angular rates before starting Z-thomson spin
    status = adcs_analyse_mems_rate(&max_rate_b_detumble);

    if(OPERATION_SUCCESS == status)
    {
        // Reset the status
#ifdef ADCS_MODE_ENABLE
        ctrl_mode.controlModeSelection = ConModeSelect_ConYspin;
#else
        ctrl_mode.controlModeSelection = ConModeSelect_ConSpecial1;
#endif
        /* Set timeout as 600 seconds to verify the X and Y rates are
         * getting reduced. Refer Table 4 in Cube ADCS Commissioning
         * manual.
         */
        ctrl_mode.timeout = 600;
        size = CUBEACP_FormatControlModeCmd(tcBuffer, &ctrl_mode);
        result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
        if(TCMD_OK == result)
        {
            // Wait for 600 seconds for the rates to reduce
            os_delay(600000);
            status = adcs_analyse_mems_rate(&max_rate_a_detumble);

            if(OPERATION_SUCCESS == status)
            {
                // Reset the status
                status = OPERATION_FAILURE;
                /*
                 * Compare the angular rates before detumble and after
                 * enabling detumbling process for 600 seconds. If the rates
                 * are reduced, set infinite time out.
                 */
#ifdef TEST_HOOK
                DEBUG_CPRINT(("CUBE ADCS: max_rate_a_detumble.rateX - %lf \n",
                        max_rate_a_detumble.rateX));
                log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
                max_rate_a_detumble_rateX__u32__pt__u32_,\
                max_rate_a_detumble.rateX);
                DEBUG_CPRINT(("CUBE ADCS: max_rate_b_detumble.rateX - %lf \n",
                        max_rate_b_detumble.rateX));
                log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
                max_rate_b_detumble_rateX__u32__pt__u32_,\
                max_rate_b_detumble.rateX);
                DEBUG_CPRINT(("CUBE ADCS: max_rate_a_detumble.rateY - %lf \n",
                        max_rate_a_detumble.rateY));
                log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
                max_rate_a_detumble_rateY__u32__pt__u32_,\
                max_rate_a_detumble.rateY);
                DEBUG_CPRINT(("CUBE ADCS: max_rate_b_detumble.rateY - %lf \n",
                        max_rate_b_detumble.rateY));
                log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
                max_rate_b_detumble_rateY__u32__pt__u32_,\
                max_rate_b_detumble.rateY);
                DEBUG_CPRINT(("CUBE ADCS: max_rate_a_detumble.rateZ - %lf \n",
                        max_rate_a_detumble.rateZ));
                log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
                max_rate_a_detumble_rateZ__u32__pt__u32_,\
                max_rate_a_detumble.rateZ);
#endif
                if(max_rate_a_detumble.rateX <= max_rate_b_detumble.rateX)
                {
                    if(max_rate_a_detumble.rateY <= max_rate_b_detumble.rateY)
                    {
                        // Check Z rate. It should be ~ equal to reference rate
                        status = adcs_double_compare(max_rate_a_detumble.rateZ,
                                -2.0, 0.5);
                    }
                }
                if(OPERATION_SUCCESS != status)
                {
                    /*
                     * Sometimes the rate may be already less than 0. So check
                     * if that's the case
                     */
                    status = adcs_double_compare(max_rate_a_detumble.rateX, \
                             0.0,0.5);
                    status |= adcs_double_compare(max_rate_a_detumble.rateY,\
                            0.0, 0.5);
                    status |= adcs_double_compare(max_rate_a_detumble.rateZ,\
                            -2.0, 0.5);
                }
                // status = OPERATION_SUCCESS;
                if(OPERATION_SUCCESS == status)
                {
#ifdef ADCS_MODE_ENABLE
                    ctrl_mode.controlModeSelection = ConModeSelect_ConYspin;
#else
                    ctrl_mode.controlModeSelection = ConModeSelect_ConSpecial1;
#endif
                    ctrl_mode.timeout = 0xffff;
                    size = CUBEACP_FormatControlModeCmd(tcBuffer, &ctrl_mode);
                    result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
                    if(TCMD_OK == result)
                    {
                        while(0 == detumble_success)
                        {
                            status =
                                adcs_analyse_mems_rate(&max_rate_f_detumble);
                            if(OPERATION_SUCCESS == status)
                            {
                                status = adcs_double_compare(
                                        max_rate_f_detumble.rateX, 0, 0.2);
                                status |= adcs_double_compare(
                                        max_rate_f_detumble.rateY, 0, 0.2);
                                status |= adcs_double_compare(
                                        max_rate_f_detumble.rateZ, -2.0, 0.2);
                            }
                            else
                            {
                                // TODO Log error
                                DEBUG_CPRINT(("CUBE ADCS: Failed to acquire "
                                    "telemetry in final detumbling setp \n"));
                                log_id(LOG_ERROR,LOG_ADCS,\
                        Failed_to_acquire_telemetry_in_final_detumbling_setp);
                                status = OPERATION_FAILURE;
                                break;
                            }
                            if(OPERATION_SUCCESS == status)
                            {
                                detumble_success = 1;
                                DEBUG_CPRINT(("CUBE ADCS: Final detumbling"
                                        " success \n"));
                                log_id(LOG_INFO_L1,LOG_ADCS,\
                                Final_detumbling_success);
                            }
                            else
                            {
                                // TODO: Log error after n attempts
                                DEBUG_CPRINT(("CUBE ADCS: rates failed to"
                                            " reduce in "
                                        "final detumbling step. waiting for "
                                        "another 600 seconds\n"));
                                log_id(LOG_ERROR,LOG_ADCS,\
        Rates_failed_to_reduce_in_final_detumbling_step_wait_another_600s);
                                os_delay(600000);
                            }
                        }
                    }
                    else
                    {
                        // TODO: Log error
                        DEBUG_CPRINT(("CUBE ADCS: Failed to set control"
                                    " mode %d in "
                                "initial detumbling \n",
                                init_ctrl_mode));
                        log_id_with_one_arg(LOG_ERROR,LOG_ADCS,\
                    Failed2_to_set_control_mode__u32__in_initial_detumbling,\
                    init_ctrl_mode);
                        status = OPERATION_FAILURE;
                    }
                }
                else
                {
                    // TODO: Log error
                    DEBUG_CPRINT(("CUBE ADCS:Angular rates failed to reduce in\
                            initial detumbling step \n"));
                    log_id(LOG_ERROR,LOG_ADCS,\
                    Angular_rates_failed_to_reduce_in_initial_detumbling_step);
                }
            }
            else
            {
                // TODO: Log error
                DEBUG_CPRINT(("CUBE ADCS:Failed to acquire telemetry in initial"
                        " detumbling step \n"));
                log_id(LOG_ERROR,LOG_ADCS,\
                Failed_to_acquire_telemetry_in_initial_detumbling_step);
                status = OPERATION_FAILURE;
            }
        }
        else
        {
            // TODO: Log error
            DEBUG_CPRINT(("CUBE ADCS: Failed to set control mode %d in initial"
                    "detumbling \n", init_ctrl_mode));
            log_id_with_one_arg(LOG_ERROR,LOG_ADCS,\
            Failed3_to_set_control_mode__u32__in_initial_detumbling,\
            init_ctrl_mode);
            status = OPERATION_FAILURE;
        }
    }
    else
    {
        // TODO Log error
        DEBUG_CPRINT(("CUBE ADCS: Failed to acquire telemetry before starting"
                " Z-Thomson spin \n"));
        log_id(LOG_ERROR,LOG_ADCS,\
        Failed_to_acquire_telemetry_before_starting_Zthomson_spin);
    }
    return status;
}

/**
 * @brief Function to handle adcs coarse_momentum
 */
e_adcs_status vapi_adcs_coarse_momentum(void)
{
    CUBEACP_ControlMode_t ctrl_mode;
    uint16 size = 0;
    uint8 tcBuffer[10] = {0};
    int8 result = -1;
    e_adcs_status status = OPERATION_FAILURE;
    CUBEACP_AdcsMeasure_t max_rate;

    ctrl_mode.controlModeSelection = ConModeSelect_ConNone;
    ctrl_mode.timeout = 0;
    size = CUBEACP_FormatControlModeCmd(tcBuffer, &ctrl_mode);
    result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
    if(TCMD_OK == result)
    {
        // Mask the usage of all sensors in estimation mode
        status = adcs_set_estim_sens_mask(0, 0, 0, 0);
        if(OPERATION_SUCCESS == status)
        {
            size = CUBEACP_FormatEstimModeCmd(tcBuffer,
                    EstimModeSelect_EstGyroEkf);
            result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
            if(TCMD_OK == result)
            {
                // TODO: Tune the delay value
                os_delay(600000);
                status = adcs_test_angular_rate_measurement();
                if(OPERATION_SUCCESS != status)
                {
                    // TODO Revert to old estimation mode
                    DEBUG_CPRINT(("CUBE ADCS: Gyro EKF estimated rates"
                                " doesn't match with actual rates \n"));
                    log_id(LOG_ERROR,LOG_ADCS,\
                    Gyro_EKF_est_rates_not_match_with_actual_rates);
                }
            }
            else
            {
                // TODO Log error
                DEBUG_CPRINT(("CUBE ADCS: Set Gyro EKF estimation"
                            "mode failed \n"));
                log_id(LOG_ERROR,LOG_ADCS,Set_Gyro_EKF_est_mode_failed);
            }
        }
        else
        {
            // TODO: Log error
            DEBUG_CPRINT(("CUBE ADCS:Masking sensor for estimation failed\n"));
            log_id(LOG_ERROR,LOG_ADCS,Masking_sens_for_est_failed);
        }
        DEBUG_CPRINT(("CUBE ADCS: Starting Reaction wheel test \n"));
        log_id(LOG_INFO_L1,LOG_ADCS,Starting_reaction_wheel_test);
        status = adcs_perform_r_wheel_test();
    }
    else
    {
        // TODO : Log error
        DEBUG_CPRINT(("CUBE ADCS: Set control mode to none failed \n"));
        log_id(LOG_ERROR,LOG_ADCS,Set_ctrl_mode_to_none_failed);
    }

    if(OPERATION_SUCCESS == status)
    {
        ctrl_mode.controlModeSelection = ConModeSelect_ConXYZwheel;
        ctrl_mode.timeout = 0xffff;
        size = CUBEACP_FormatControlModeCmd(tcBuffer, &ctrl_mode);
        result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
        if(TCMD_OK == result)
        {
            // TODO: Tune the delay value
            os_delay(1200000);
            status = adcs_analyse_mems_rate(&max_rate);
            if(OPERATION_SUCCESS == status)
            {
                status = adcs_double_compare(max_rate.rateX, 0.0, 0.1);

                status |= adcs_double_compare(max_rate.rateY, 0.0, 0.1);

                status |= adcs_double_compare(max_rate.rateZ, 0.0, 0.1);
                if(OPERATION_SUCCESS == status)
                {
                    DEBUG_CPRINT((" CUBE ADCS: Coarse momentum is success \n"));
                    log_id(LOG_INFO_L1,LOG_ADCS,Masking_sens_for_est_failed);
                }
                else
                {
                    // TODO: Log error
                    DEBUG_CPRINT((" CUBE ADCS: Coarse momentum failed \n"));
                    log_id(LOG_ERROR,LOG_ADCS,Coarse_momentum_failed);
                }
            }
            else
            {
                // TODO: Log error
                DEBUG_CPRINT(("CUBE ADCS: Mems rate analysis failed for "
                        "ConXYZwheel\n"));
                log_id(LOG_ERROR,LOG_ADCS,\
                Mems_rate_analysis_failed_for_conXYZwheel);
            }
        }
        else
        {
            // TODO: Log error
            DEBUG_CPRINT(("CUBE ADCS: Set control mode to ConXYZwheel"
                        " failed \n"));
            log_id(LOG_ERROR,LOG_ADCS,Set_ctrl_mode_to_conXYZwheel_failed);
        }
    }
    else
    {
        // TODO: Log error
        DEBUG_CPRINT((" Reaction wheel tests failed \n"));
        log_id(LOG_ERROR,LOG_ADCS,Reaction_wheel_tests_failed);
    }

    if(OPERATION_SUCCESS != status)
    {
        // revert the control mode
#ifdef ADCS_MODE_ENABLE
                    ctrl_mode.controlModeSelection = ConModeSelect_ConYspin;
#else
                    ctrl_mode.controlModeSelection = ConModeSelect_ConSpecial1;
#endif
        ctrl_mode.timeout = 0xffff;
        size = CUBEACP_FormatControlModeCmd(tcBuffer, &ctrl_mode);
        result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
    }

    return status;
}

/**
 * @brief Function to handle adcs fine_momentum
 */
e_adcs_status vapi_adcs_fine_momentum(void)
{
    CUBEACP_ControlMode_t ctrl_mode;
    uint16 size = 0;
    uint8 tcBuffer[10] = {0};
    int8 result = -1;
    e_adcs_status status = OPERATION_FAILURE;
    CUBEACP_AdcsMeasure_t max_rate={0};
    max_rate=max_rate;
    e_adcs_status  star_status;
    uint8 param[10] = {0x80,0x7D,0x00,0x58,0x00,0x00,0x00,0x18,0x00,0x00};

    status = adcs_commission_sun_sensor();
    status |= adcs_commission_nadir_sensor();
    if(OPERATION_SUCCESS == status)
    {
        // Enable the usage of sun sensor in estimation mode
        status = adcs_set_estim_sens_mask(1, 1, 1, 0);
        if(OPERATION_SUCCESS == status)
        {
            // Enable sun tracking mode
            ctrl_mode.controlModeSelection = ConModeSelect_ConRWSunTrack;
            ctrl_mode.timeout = 0xffff;
            size = CUBEACP_FormatControlModeCmd(tcBuffer, &ctrl_mode);
            result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
            if(TCMD_OK != result)
            {
                // status = OPERATION_FAILURE;
                // log error
                DEBUG_CPRINT(("CUBE ADCS: Set control mode to suntracking"
                            " failed \n"));
                log_id(LOG_ERROR,LOG_ADCS,Set_ctrl_mode_to_suntrack_failed);
            }
            else
            {
                DEBUG_CPRINT(("CUBE ADCS: Fine momentum success \n"));
                log_id(LOG_INFO_L1,LOG_ADCS,Fine_momentum_sucess);
                DEBUG_CPRINT(("CUBE ADCS: ADCS COMMISSIONING SUCCESS \n"));
                log_id(LOG_INFO_L1,LOG_ADCS,ADCS_COMMISSIONING_SUCCESS);
            }
        }
        else
        {
            // log error
            DEBUG_CPRINT(("CUBE ADCS: Enabling the usage of sun sensor"
                        " in estimation mode failed \n"));
            log_id(LOG_ERROR,LOG_ADCS,\
            Enabling_the_usage_of_sun_sens_in_est_mode_failed);
        }
    }
    else
    {
        // log error
        DEBUG_CPRINT(("CUBE ADCS: Sun or nadir sensor commissioning"
                    " failed \n"));
        log_id(LOG_ERROR,LOG_ADCS,sun_or_nadir_sens_commissioning_failed);
    }
    star_status = adcs_commission_star_sensor();
    if(OPERATION_SUCCESS == star_status)
    {
        // Enable the usage of star sensor in estimation mode
        status = adcs_set_estim_sens_mask(1,1,1,1);
    }
    else
    {
        // log error
        DEBUG_CPRINT(("CUBE ADCS: star sensor commissioning failed \n"));
        log_id(LOG_ERROR,LOG_ADCS,star_sens_commissioning_failed);
    }
    os_delay(1200000);
    //stop logging
    adcs_log_sd_cube(param,0);

    return status;
}

/*
 * @brief function to power on and commission the sun sensor
 */
e_adcs_status adcs_commission_sun_sensor(void)
{
    e_adcs_status status = TC_TM_ERROR;
    CUBEACP_AdcsPower_t pwr_ctrl;
    e_tctm_status result;
    uint8 tcBuffer[10] = {0};
    uint16 size = 0;

    pwr_ctrl.cubeControlSignalPower = PowerSelect_PowOn;
    pwr_ctrl.cubeControlMotorPower = PowerSelect_PowOn;
    pwr_ctrl.cubeSense1Power = PowerSelect_PowOn;
    pwr_ctrl.cubeSense2Power = PowerSelect_PowOn;
    pwr_ctrl.cubeStarPower = PowerSelect_PowNoChange;
    pwr_ctrl.cubeWheel1Power = PowerSelect_PowNoChange;
    pwr_ctrl.cubeWheel2Power = PowerSelect_PowNoChange;
    pwr_ctrl.cubeWheel3Power = PowerSelect_PowNoChange;
    pwr_ctrl.motorPower = PowerSelect_PowNoChange;
    pwr_ctrl.gpsLnaPower = PowerSelect_PowNoChange;

    size = CUBEACP_FormatAdcsPowerCmd(tcBuffer, &pwr_ctrl);
    result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
    if(TCMD_OK == result)
    {
        status = adcs_test_fine_sun_sens();
    }
    else
    {
        // log error
        DEBUG_CPRINT(("CUBE ADCS: Powering sun camer failed \n"));
        log_id(LOG_ERROR,LOG_ADCS,Powering_sun_camer_failed);
    }

    return status;
}

/**
 * @brief function that tests the fine sun sensor by comparing the fine
 *        sun vector with coarse sun vector
 */
static e_adcs_status adcs_test_fine_sun_sens(void)
{
    e_adcs_status status = OPERATION_SUCCESS;
    uint8 flag = 0;
    uint8 pass = 0;
    CUBEACP_AdcsMeasure_t coarse_sun_vec[10];
    CUBEACP_AdcsMeasure_t fine_sun_vec[10];
    uint8 idx = 0;

    while(idx < 10)
    {
        status |= adcs_request_coarse_sun_tlm(&coarse_sun_vec[idx]);
        status |= adcs_request_fine_sun_tlm(&fine_sun_vec[idx]);
        if(OPERATION_SUCCESS != status)
        {
            break;
        }
        idx++;
        os_delay(1000);
    }
    if(status == OPERATION_SUCCESS)
    {
        for(idx = 0; idx < 10; idx++)
        {
            flag = adcs_double_compare(coarse_sun_vec[idx].cssX,
                    fine_sun_vec[idx].sunX, 1.0);
            if(flag == OPERATION_SUCCESS)
            {
                flag = adcs_double_compare(coarse_sun_vec[idx].cssY,
                        fine_sun_vec[idx].sunY, 1.0);
                if(flag == OPERATION_SUCCESS)
                {
                    pass++;
                }
            }
        }
        if(pass >= 5)
        {
            status = OPERATION_SUCCESS;
        }
        else
        {
            status = OPERATION_FAILURE;
        }
    }
    else
    {
        // Move to error state
        DEBUG_CPRINT(("CUBE ADCS: Requesting telemetry failed in "
                "test_fine_sun_sens \n"));
        log_id(LOG_ERROR,LOG_ADCS,Req_tm_failed_in_test_fine_sun_sens);
    }

    return status;
}

/*
 * @brief function that commissions nadir sensor
 */
e_adcs_status adcs_commission_nadir_sensor(void)
{
    e_adcs_status status = TC_TM_ERROR;
    CUBEACP_AdcsPower_t pwr_ctrl;
    e_tctm_status result;
    CUBEACP_AdcsMeasure_t nadir_vec;
    uint8 tcBuffer[10] = {0};
    uint16 size = 0;

    pwr_ctrl.cubeControlSignalPower = PowerSelect_PowOn;
    pwr_ctrl.cubeControlMotorPower = PowerSelect_PowOn;
    pwr_ctrl.cubeSense1Power = PowerSelect_PowOn;
    pwr_ctrl.cubeSense2Power = PowerSelect_PowOn;
    pwr_ctrl.cubeStarPower = PowerSelect_PowNoChange;
    pwr_ctrl.cubeWheel1Power = PowerSelect_PowNoChange;
    pwr_ctrl.cubeWheel2Power = PowerSelect_PowNoChange;
    pwr_ctrl.cubeWheel3Power = PowerSelect_PowNoChange;
    pwr_ctrl.motorPower = PowerSelect_PowNoChange;
    pwr_ctrl.gpsLnaPower = PowerSelect_PowNoChange;

    size = CUBEACP_FormatAdcsPowerCmd(tcBuffer, &pwr_ctrl);
    result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
    if(TCMD_OK == result)
    {
        status = adcs_request_nadir_vec_tlm(&nadir_vec);
    }
    else
    {
        // log error
        DEBUG_CPRINT(("CUBE ADCS: Powering nadir camer failed \n"));
        log_id(LOG_ERROR,LOG_ADCS,Powering_nadir_camer_failed);
    }
    if(OPERATION_SUCCESS == status)
    {
        status = adcs_double_compare(nadir_vec.nadirX, 0.0, 1);
        if(OPERATION_SUCCESS == status)
        {
            status = adcs_double_compare(nadir_vec.nadirY, 0.0, 1);
            if(OPERATION_SUCCESS != status)
            {
                // log error
                DEBUG_CPRINT((" Nadir Y angle is not close to zero \n"));
                log_id(LOG_ERROR,LOG_ADCS,Nadir_Y_angle_is_not_close_to_zero);
            }
        }
        else
        {
            // log error
            DEBUG_CPRINT((" Nadir X angle is not close to zero \n"));
            log_id(LOG_ERROR,LOG_ADCS,Nadir_X_angle_is_not_close_to_zero);
        }
    }

    return status;
}

/*
 * @brief Function that commissions star sensor
 */
e_adcs_status adcs_commission_star_sensor(void)
{
    e_adcs_status status = TC_TM_ERROR;
    e_adcs_status star_commission = OPERATION_FAILURE;
    CUBEACP_AdcsPower_t pwr_ctrl;
    e_tctm_status result;
    CUBEACP_RawStarTracker_t raw_star;
    uint8 tcBuffer[10] = {0};
    uint16 size = 0;

    pwr_ctrl.cubeControlSignalPower = PowerSelect_PowOn;
    pwr_ctrl.cubeControlMotorPower = PowerSelect_PowOn;
    pwr_ctrl.cubeSense1Power = PowerSelect_PowNoChange;
    pwr_ctrl.cubeSense2Power = PowerSelect_PowNoChange;
    pwr_ctrl.cubeStarPower = PowerSelect_PowOn;
    pwr_ctrl.cubeWheel1Power = PowerSelect_PowNoChange;
    pwr_ctrl.cubeWheel2Power = PowerSelect_PowNoChange;
    pwr_ctrl.cubeWheel3Power = PowerSelect_PowNoChange;
    pwr_ctrl.motorPower = PowerSelect_PowNoChange;
    pwr_ctrl.gpsLnaPower = PowerSelect_PowNoChange;

    size = CUBEACP_FormatAdcsPowerCmd(tcBuffer, &pwr_ctrl);
    result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
    if(TCMD_OK == result)
    {
        status = adcs_request_raw_star_tlm(&raw_star);
    }
    else
    {
        // log error
        DEBUG_CPRINT(("CUBE ADCS: Powering star tracker failed \n"));
        log_id(LOG_ERROR,LOG_ADCS,Pwr_star_trck_failed);
    }
    if(OPERATION_SUCCESS == status)
    {
        if(1 == raw_star.imageCaptureSuccess)
        {
            if(1 == raw_star.detectionSuccess)
            {
                if(1 == raw_star.identificationSuccess)
                {
                    if((raw_star.numStarsDetected > 3) &&
                            (raw_star.numStarsDetected < 16))
                    {
                        DEBUG_CPRINT(("CUBE ADCS: Number of stars"
                                    " identified: %d \n",
                                raw_star.numberOfStarsID));
                        log_id_with_one_arg(LOG_INFO_L1,LOG_ADCS,\
                        No_of_stars_identified,raw_star.numberOfStarsID);
                        star_commission = OPERATION_SUCCESS;
                    }
                    else
                    {
                        DEBUG_CPRINT(("CUBE ADCS: Star tracker configuration "
                                "change needed \n"));
                        log_id(LOG_INFO_L1,LOG_ADCS,\
                        Star_trck_cfg_change_needed);
                    }
                }
                else
                {
                    DEBUG_CPRINT(("CUBE ADCS: No star identified \n"));
                    log_id(LOG_INFO_L1,LOG_ADCS,NO_Star_identified);
                }
            }
            else
            {
                DEBUG_CPRINT(("CUBE ADCS: No star detected \n"));
                log_id(LOG_INFO_L1,LOG_ADCS,NO_Star_detected);
            }
        }
        else
        {
            DEBUG_CPRINT(("CUBE ADCS: No star image captured \n"));
            log_id(LOG_INFO_L1,LOG_ADCS,NO_Star_image_captured);
        }
    }
    else
    {
        // log error
        DEBUG_CPRINT(("CUBE ADCS: Requesting raw star telemetry failed \n"));
        log_id(LOG_ERROR,LOG_ADCS,Req_raw_star_tm_failed);
    }

    status = star_commission;

    return status;
}

/*
 * @brief Function that performs reaction wheel test
 */
e_adcs_status adcs_perform_r_wheel_test(void)
{
    CUBEACP_WheelSpeedCmd_t setVal;
    CUBEACP_AdcsMeasure_t exp_rate;
    e_adcs_status status = OPERATION_FAILURE;
    uint8 error_pass = 0;

    // perform zwheel test
    setVal.wheelZ = -800;
    setVal.wheelX = 0;
    setVal.wheelY = 0;

    exp_rate.rateX = 0.0;
    exp_rate.rateY = 0.0;
    exp_rate.rateZ = 0.0;

    status = vapi_adcs_r_wheel_ramp_up_test(setVal, exp_rate, R_WHEEL_Z);

    if(OPERATION_SUCCESS == status)
    {
        DEBUG_CPRINT(("Z wheel test is success \n"));
        log_id(LOG_INFO_L1,LOG_ADCS,Zwheel_test_is_success);
        // perform xwheel test
        setVal.wheelZ = -800;
        setVal.wheelX = -1800;
        setVal.wheelY = 0;

        exp_rate.rateX = 2.0;
        exp_rate.rateY = 0.0;
        exp_rate.rateZ = 0.0;

        status = vapi_adcs_r_wheel_ramp_up_test(setVal, exp_rate, R_WHEEL_X);
    }
    else
    {
        // TODO Log error
        DEBUG_CPRINT((" CUBE ADCS: Z wheel test failed\n"));
        log_id(LOG_ERROR,LOG_ADCS,Zwheel_test_is_failed);
        // flag to make sure the other error prints are not printed
        error_pass = 1;
    }

    if(OPERATION_SUCCESS == status)
    {
        DEBUG_CPRINT(("X wheel test is success \n"));
        log_id(LOG_INFO_L1,LOG_ADCS,Xwheel_test_is_success);
        // perform Ywheel test
        setVal.wheelZ = -800;
        setVal.wheelX = 0;
        setVal.wheelY = -1800;

        exp_rate.rateX = 0.0;
        exp_rate.rateY = 1.0;
        exp_rate.rateZ = 0.0;

        status = vapi_adcs_r_wheel_ramp_up_test(setVal, exp_rate, R_WHEEL_Y);
    }
    else if(0 == error_pass)
    {
        // TODO Log error
        DEBUG_CPRINT((" CUBE ADCS: X wheel test failed\n"));
        log_id(LOG_ERROR,LOG_ADCS,Xwheel_test_is_failed);
        // flag to make sure the other error prints are not printed
        error_pass = 1;
    }

    if(OPERATION_SUCCESS != status)
    {
        // TODO Log error
        DEBUG_CPRINT((" CUBE ADCS: Y wheel test failed\n"));
        log_id(LOG_ERROR,LOG_ADCS,Ywheel_test_is_failed);
    }
    else
    {
        // All wheel tests passed. Return the status
    }

    return status;
}

/*
 * @brief Function to test reaction wheel
 */
e_adcs_status vapi_adcs_r_wheel_ramp_up_test(CUBEACP_WheelSpeedCmd_t setVal,
        CUBEACP_AdcsMeasure_t expected_rate, e_rwheel_axis wheel)
{
    e_adcs_status status = OPERATION_FAILURE;
    CUBEACP_AdcsPower_t pwr_ctrl;
    e_tctm_status result;
    CUBEACP_AdcsMeasure_t actual_rate;
    uint16 size = 0;
    uint8 tcBuffer[10] = {0};
    uint8 error_flag = 0;
    error_flag=error_flag;

    pwr_ctrl.cubeControlSignalPower = PowerSelect_PowOn;
    pwr_ctrl.cubeControlMotorPower = PowerSelect_PowOn;
    pwr_ctrl.cubeSense1Power = PowerSelectCubeSense_PowCsNoChange;
    pwr_ctrl.cubeSense2Power = PowerSelectCubeSense_PowCsNoChange;
    pwr_ctrl.cubeStarPower = PowerSelectCubeSense_PowCsNoChange;
    pwr_ctrl.motorPower = PowerSelectCubeSense_PowCsNoChange;
    pwr_ctrl.gpsLnaPower = PowerSelectCubeSense_PowCsNoChange;
    pwr_ctrl.cubeWheel1Power = PowerSelect_PowOn;
    pwr_ctrl.cubeWheel2Power = PowerSelect_PowOn;
    pwr_ctrl.cubeWheel3Power = PowerSelect_PowOn;

    size = CUBEACP_FormatAdcsPowerCmd(tcBuffer, &pwr_ctrl);
    result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);

    if(TCMD_OK == result)
    {
        size = CUBEACP_FormatWheelSpeedCmdCmd(tcBuffer, &setVal);
        result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
    }
    else
    {
        // TODO Log error
        DEBUG_CPRINT((" CUBE ADCS: Failed to turn on R Wheel \n"));
        log_id(LOG_ERROR,LOG_ADCS,Failed_to_turn_on_R_Wheel);
    }
    if(TCMD_OK == result)
    {
        os_delay(12000);
        status = adcs_analyse_mems_rate(&actual_rate);
    }
    else
    {
        // TODO Log error
        DEBUG_CPRINT((" CUBE ADCS: Failed to command Wheel Z speed \n"));
        log_id(LOG_ERROR,LOG_ADCS,Failed_to_cmd_Wheel_Z_speed);
    }
    if(OPERATION_SUCCESS == status)
    {
        // Check actual rate. It should be ~ equal to expected rate
        // TODO We might need to check the attitude too
        if(R_WHEEL_X == wheel)
        {
            status = adcs_double_compare(expected_rate.rateX,
                    actual_rate.rateX,
                    0.5);
        }
        else if(R_WHEEL_Y == wheel)
        {
            status = adcs_double_compare(expected_rate.rateY,
                    actual_rate.rateY,
                    0.5);
        }
        else if(R_WHEEL_Z == wheel)
        {
            status = adcs_double_compare(expected_rate.rateZ,
                    actual_rate.rateZ,
                    0.5);
        }
        else
        {
            status = OPERATION_FAILURE;
        }
    }
    else
    {
        // TODO Log error
        DEBUG_CPRINT(("CUBE ADCS: Failed to obtain rate telemetry"
                    " in R wheel test\n"));
        log_id(LOG_ERROR,LOG_ADCS,Failed_to_obtain_rate_tm_in_R__Wheel_test);
        // flag to make sure the other error prints are not printed
        error_flag = 1;
    }

    setVal.wheelZ = 0;
    setVal.wheelX = 0;
    setVal.wheelY = 0;
    size = CUBEACP_FormatWheelSpeedCmdCmd(tcBuffer, &setVal);
    result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
    os_delay(30000);

    if(OPERATION_SUCCESS != status)
    {
        // TODO Log error
        DEBUG_CPRINT((" CUBE ADCS: Rates doesn't match in R wheel test\n"));
        log_id(LOG_ERROR,LOG_ADCS,Rates_not_match_in_R_Wheel_test);
    }
    else if(TCMD_OK != result)
    {
        status = OPERATION_FAILURE;
        // TODO Log error
        DEBUG_CPRINT((" CUBE ADCS: Failed to command Wheel R speed"
                    " to 0 RPM\n"));
        log_id(LOG_ERROR,LOG_ADCS,Failed_to_cmd_Wheel_R_speed_to_0_RPM);
    }

    return status;
}

/*
 * @brief Function to analyse the angular rate and return the max rate
 *        in a total of 10 measurements
 */
static e_adcs_status adcs_analyse_mems_rate(CUBEACP_AdcsMeasure_t *max_rate)
{
    CUBEACP_AdcsMeasure_t rate_sens_rates[10];
    uint8 idx = 0;
    e_adcs_status status = OPERATION_FAILURE;

    if(NULL != max_rate)
    {
        status = OPERATION_SUCCESS;
        while(idx < 10)
        {
            status |= adcs_request_est_rate_sens_rates_tlm(
                    (CUBEACP_AdcsState_t*)&rate_sens_rates[idx]);
            idx++;
            os_delay(1000);
        }

        if(OPERATION_SUCCESS == status)
        {
            // Calculate the largest angular rate in each of the axis
            adcs_largest_rate(rate_sens_rates, 10, max_rate);
#ifdef TEST_HOOK
            DEBUG_CPRINT(("max_rate.rateX : %lf \n", max_rate->rateX));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
            ang_max_rate_X__u32__pt__u32_,max_rate->rateX);
            DEBUG_CPRINT(("max_rate.rateY : %lf \n", max_rate->rateY));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
            ang_max_rate_Y__u32__pt__u32_,max_rate->rateY);
            DEBUG_CPRINT(("max_rate.rateZ : %lf \n", max_rate->rateZ));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
            ang_max_rate_Z__u32__pt__u32_,max_rate->rateZ);
#endif
        }
    }

    return status;
}

/*
 * @brief Function to select control mode based on sensor rate
 */
e_adcs_status adcs_select_ctrl_mode(CUBEACP_ConModeSelect_t *mode,
        double *max_rate_ptr)
{
    e_adcs_status status = OPERATION_SUCCESS;
    uint8 flag = 0;
    uint8 pass = 0;
    pass=pass;
    flag=flag;
    CUBEACP_AdcsMeasure_t max_mems_rate;
    CUBEACP_AdcsState_t estim_rate={0};
    estim_rate=estim_rate;
    CUBEACP_AdcsMeasure_t rate_sens_rates[10];
    CUBEACP_AdcsState_t est_rate_sens_rates[3];
    est_rate_sens_rates[3]=est_rate_sens_rates[3];
    uint16 recv_length=0;
    recv_length=recv_length;
    uint16 time_Val=0;
    time_Val=time_Val;
    uint8 idx = 0;
    double abs_max_rate, max_rate = 0;
    CUBEACP_ConModeSelect_t ctrl_mode = ConModeSelect_ConNone;

    if((NULL != mode) && (NULL != max_rate_ptr))
    {
        // Get rate sensor data for 10 iterations
        while(idx < 10)
        {
            status |= adcs_request_rate_sens_rates_tlm(&rate_sens_rates[idx]);
            idx++;
            os_delay(1000);
        }

        if(OPERATION_SUCCESS == status)
        {
            // Calculate the largest angular rate in each of the axis
            max_rate = adcs_largest_rate(rate_sens_rates, 3, &max_mems_rate);
            abs_max_rate = fabs(max_rate);

            /*
             * Determine the initial detumbling control mode based on the
             * maximum angular rate
             */
            if(abs_max_rate > 40)
            {
                if((abs_max_rate > 40) && (abs_max_rate < 100))
                {
                    ctrl_mode = ConModeSelect_ConFastDetumbling;
                }
                else
                {
                    ctrl_mode = ConModeSelect_ConVeryFastDetumbling;
                }
            }
            else
            {
                ctrl_mode = ConModeSelect_ConBdot;
            }

            *mode = ctrl_mode;
            *max_rate_ptr = max_rate;
        }
    }
    else
    {
        status = OPERATION_FAILURE;
    }

    return status;
}

/*
 * @brief Function to set multiplier for rate sensor
 */
e_adcs_status adcs_set_rate_sens_mult(double max_rate)
{
    e_adcs_status status = OPERATION_FAILURE;
    e_tctm_status tc_tm_sts;
    uint8 *tlm_buffer = NULL;
    uint8 *tcBuffer = NULL;
    uint8 rate_sens_mult = 1;
    CUBEACP_AdcsConfig_t gyro_cfg;
    uint16 size = 0;
    double abs_max_rate = fabs(max_rate);

    if(abs_max_rate > 75)
    {
        if((abs_max_rate > 75) && (abs_max_rate < 150))
        {
            rate_sens_mult = 2;
        }
        else if((abs_max_rate > 150) && (abs_max_rate < 300))
        {
            rate_sens_mult = 3;
        }
        else if((abs_max_rate > 300) && (abs_max_rate < 900))
        {
            rate_sens_mult = 4;
        }
        else
        {
            rate_sens_mult = 12;
        }

        size = COMMON_GetTlmLen(GyroConfig);
        tlm_buffer = os_malloc_eram(size);
        if(NULL != tlm_buffer)
        {
            tc_tm_sts = adcs_requesttlm(GyroConfig, tlm_buffer, size);
            if(TLM_OK == tc_tm_sts)
            {
                CUBEACP_ExtractGyroConfigTlm(tlm_buffer, &gyro_cfg);
                gyro_cfg.rateSensorMult = rate_sens_mult;
                tcBuffer = os_malloc_eram(11);
                size = CUBEACP_FormatGyroConfigCmd(tcBuffer, &gyro_cfg);
                tc_tm_sts = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
                if(TCMD_OK == tc_tm_sts)
                {
                    status = OPERATION_SUCCESS;
                }
                else
                {
                    //log error
                    DEBUG_CPRINT(("CUBE ADCS: ADCS_SENDTC_CAN API "
                                "RETURNS FAILURE FOR RATE_GYRO_CFG \n"));
                    log_id(LOG_ERROR,LOG_ADCS,\
                    ADCS_SENDTC_CAN_API_RETURNS_FAILURE_FOR_RATE_GYRO_CFG);
                }
            }
            else
            {
                DEBUG_CPRINT((" Error receving TLM %d \n", status));
                log_id_with_one_arg(LOG_ERROR,LOG_ADCS,Error_receving_tlm_s32,\
                status);
            }
            os_free(tlm_buffer);
            os_free(tcBuffer);
        }
    }
    else
    {
        // Nothing to do if the max rate is less than 75 dps
        status = OPERATION_SUCCESS;
    }
    return status;
}

/*
 * @brief Function to Calculate the largest angular rate in each of the axis
 */
double adcs_largest_rate(CUBEACP_AdcsMeasure_t *data, uint16 sample,
        CUBEACP_AdcsMeasure_t *max_rate)
{
    uint16 itr;
    double max, max_x, max_y, max_z = 0;

    max_x = data[0].rateX;
    max_y = data[0].rateY;
    max_z = data[0].rateZ;

    for(itr = 0; itr < sample; itr++)
    {
        if(fabs(data[itr].rateX) > fabs(max_x))
        {
            max_x = data[itr].rateX;
        }
        if(fabs(data[itr].rateY) > fabs(max_y))
        {
            max_y = data[itr].rateY;
        }
        if(fabs(data[itr].rateZ) > fabs(max_z))
        {
            max_z = data[itr].rateZ;
        }
    }
    max_rate->rateX = max_x;
    max_rate->rateY = max_y;
    max_rate->rateZ = max_z;

    // Find the largest of 3 max rates and return it
    max = max_x;

    if(fabs(max_y) > fabs(max))
    {
        max = max_y;
    }
    if(fabs(max_z) > fabs(max))
    {
        max = max_z;
    }

    return max;
}

/*
 * @brief Function to set Estimation Parameters
 */
e_adcs_status adcs_set_estim_sens_mask(uint8 use_sun, uint8 use_nadir,
        uint8 use_css, uint8 use_star)
{
    CUBEACP_AdcsConfig_t est_param;
    e_adcs_status status = OPERATION_FAILURE;
    e_tctm_status result;
    uint8 tlmId = 0;
    uint16 size = 0;
    uint8 *tlm_buffer = NULL;
    uint8 *tcBuffer = NULL;

    tlmId = EstimParams;
    size = CUBEACP_GetTlmLen(EstimParams);
    tlm_buffer = os_malloc_eram(size);
    if(NULL != tlm_buffer)
    {
        result = adcs_requesttlm(tlmId, tlm_buffer, size);
        if(TLM_OK == result)
        {
            CUBEACP_ExtractEstimParamsTlm(tlm_buffer, &est_param);
            // Don't change the state of the value is other than 0 or 1
            if((use_sun >= 0) && (use_sun < 2))
            {
                est_param.useSunEst = use_sun;
            }
            if((use_nadir >= 0) && (use_nadir < 2))
            {
                est_param.useNadirEst = use_nadir;
            }
            if((use_css >= 0) && (use_css < 2))
            {
                est_param.useCssEst = use_css;
            }
            if((use_star >= 0) && (use_star < 2))
            {
                est_param.useStarEst = use_star;
            }
            os_free(tlm_buffer);
            tcBuffer = os_malloc_eram(32);
            if(NULL != tcBuffer)
            {
                size = CUBEACP_FormatEstimParamsCmd(tcBuffer, &est_param);
                result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);

                if(TCMD_OK == result)
                {
                    status = OPERATION_SUCCESS;
                }
                else
                {
                    // TODO: Log error
                    DEBUG_CPRINT(("CUBE ADCS: Set estimation params"
                                " failed \n"));
                    log_id(LOG_ERROR,LOG_ADCS,Set_est_params_failed);
                }
                os_free(tcBuffer);
            }
            else
            {
                // TODO: Log error
                DEBUG_CPRINT(("CUBE ADCS: os_malloc_eram failed for TC in set"
                        "estim param \n"));
                log_id(LOG_ERROR,LOG_ADCS,\
                osmalloc_failed_for_TC_in_set_est_param);
            }
        }
        else
        {
            // TODO: Log error
            DEBUG_CPRINT(("CUBE ADCS: Request estim param TLM failed \n"));
            log_id(LOG_ERROR,LOG_ADCS,Req_est_params_tlm_failed);
        }
    }
    else
    {
        // TODO: Log error
        DEBUG_CPRINT(("CUBE ADCS: os_malloc_eram failed for TLM in set"
                    " estim param \n"));
        log_id(LOG_ERROR,LOG_ADCS,osmalloc_failed_for_TLM_in_set_est_param);
    }

    return status;
}

/**
 * @brief Function to compare measured and estimated angular rates
 */
static e_adcs_status adcs_test_angular_rate_measurement(void)
{
    e_adcs_status status = OPERATION_SUCCESS;
    uint8 flag = 0;
    uint8 pass = 0;
    CUBEACP_AdcsMeasure_t rate_sens_rates[10];
    CUBEACP_AdcsState_t est_rate_sens_rates[10];
    uint8 idx = 0;

    while(idx < 10)
    {
        status |= adcs_request_rate_sens_rates_tlm(&rate_sens_rates[idx]);
        status |=
            adcs_request_est_rate_sens_rates_tlm(&est_rate_sens_rates[idx]);
        if(OPERATION_SUCCESS != status)
        {
            break;
        }
        idx++;
        os_delay(1000);
    }
    if(status == OPERATION_SUCCESS)
    {
        for(idx = 0; idx < 10; idx++)
        {
            flag = adcs_double_compare(rate_sens_rates[idx].rateX,
                    est_rate_sens_rates[idx].rateX, 1.0);
            if(flag == OPERATION_SUCCESS)
            {
                flag = adcs_double_compare(rate_sens_rates[idx].rateY,
                        est_rate_sens_rates[idx].rateY, 1.0);
                if(flag == OPERATION_SUCCESS)
                {
                    flag = adcs_double_compare(rate_sens_rates[idx].rateZ,
                            est_rate_sens_rates[idx].rateZ,
                            1.0);
                    if(flag == OPERATION_SUCCESS)
                    {
                        pass++;
                    }
                }
            }
        }
        if(pass >= 5)
        {
            status = OPERATION_SUCCESS;
        }
        else
        {
            status = OPERATION_FAILURE;
        }
    }
    else
    {
        // Move to error state
        DEBUG_CPRINT(("CUBE ADCS: Requesting telemetry failed in "
                "test_angular_rate_measurement \n"));
        log_id(LOG_ERROR,LOG_ADCS,Req_tlm_failed_in_test_ang_rate_measure);
    }

    return status;
}

/**
 * @brief Function to handle adcs sensor init command
 */
e_adcs_status vapi_adcs_sensor_init(void)
{
    e_adcs_status status = OPERATION_SUCCESS;
    /*
     * There is no need to initialize the sensor in a separate step for CUBE
     * SPACE ADCS. It should be done during the commissioning process as per
     * CUBE SPACE ADCS commissioning manual
     */
    return status;
}

/**
 * @brief Function to handle adcs actuator init command
 */
e_adcs_status vapi_adcs_actuator_init(void)
{
    e_adcs_status status = OPERATION_SUCCESS;
    /*
     * There is no need to initialize the actuator in a separate step for CUBE
     * SPACE ADCS. It should be done during the commissioning process as per
     * CUBE SPACE ADCS commissioning manual
     */
    return status;
}

/**
 * @brief Function to handle set coarse momentum paramter command
 */
e_adcs_status vapi_adcs_set_coarse_momentum_params(void)
{
    e_adcs_status status = OPERATION_SUCCESS;
    return status;
}

/**
 * @brief Function to handle set fine momentum paramter command
 */
e_adcs_status vapi_adcs_set_fine_momentum_params(void)
{
    e_adcs_status status = OPERATION_SUCCESS;
    return status;
}

/**
 * @brief Function to handle ground tracking control command
 */
e_adcs_status vapi_adcs_ground_tracking_control(float longRef, float latRef,
        float altRef)
{
    e_adcs_status status = OPERATION_SUCCESS;
    return status;
}

/**
 * @brief Function to handle adcs bypass command
 */
e_adcs_status vapi_adcs_bypass_mode(uint8 *tx_data, uint8 *rx_data,
        uint16 tx_size, uint16 *rx_size)
{
    e_adcs_status status = TC_TM_ERROR;
    e_intrfc_typ interface={0};
    interface=interface;
    uint16 size = 0;

    if((NULL != tx_data) && (NULL != rx_data) && (NULL != rx_size))
    {
        if((tx_data[0] >= 0) && (tx_data[0] < 128))
        {
            size = tx_size - 1;
            status = adcs_send_cmd(tx_data[0], &tx_data[1],size);
            rx_data[1] = tx_data[0];
            rx_data[0] = status;
            *rx_size = 2;
        }
        else if((tx_data[0] > 127) && (tx_data[0] <= 255))
        {
            size = adcs_get_tlm_size(tx_data[0]);
            status = adcs_requesttlm(tx_data[0], adcs_bypass_tlm_buffer, size);
            rx_data[0] = status;
            rx_data[1] = tx_data[0];
            *rx_size =  2;
            if(status == OPERATION_SUCCESS)
            {
                os_memcpy(&rx_data[2], adcs_bypass_tlm_buffer, size);
                *rx_size = *rx_size + size;
            }
        }
        else
        {
            /**
             *  TC or TM id cannot be greater than 255 for cube adcs
             *  Return the status as TC_TM_ERROR
             */
        }
    }

    return status;
}

/**
 * @brief This API calculates and returns the size of the telemetry based
 * on the cubespace provided API.
 */
int16 adcs_get_tlm_size(uint8 tlm)
{
    int16 size = 0;

#ifndef ADCS_HW_SIM
    size = COMMON_GetTlmLen(tlm);
    if(size == -1)
    {
        size = CUBEACP_GetTlmLen(tlm);
        if(size == -1)
        {
            size = BOOTLOADER_GetTlmLen(tlm);
        }
    }
#else
    size = bsk_get_tlm_len(tlm);
#endif

    return size;
}

/*
 * @brief Function to request Boot And Running Program Status tlm
 */
int8 adcs_request_boot_prog_status_tlm(COMMON_ExtendedIdentification_t\
        *prog_sts)
{
    int8 status = -1;
    uint8 tlmId = 0;
    uint16 size = 0;
    uint8 *tlm_buffer = NULL;

    //  Boot And Running Program Status Telemetry
    tlmId = ExtendedIdentification;
    size = COMMON_GetTlmLen(ExtendedIdentification);
    tlm_buffer = os_malloc_eram(size);
    if(NULL != tlm_buffer)
    {
        status = adcs_requesttlm(tlmId, tlm_buffer, size);
        if(TLM_OK == status)
        {
            COMMON_ExtractExtendedIdentificationTlm(tlm_buffer, prog_sts);
        }
        else
        {
            DEBUG_CPRINT((" Error receving TLM %d \n", status));
            log_id_with_one_arg(LOG_ERROR,LOG_ADCS,Error_recv_tlm__i8_,status);
        }
        os_free(tlm_buffer);
    }
    return status;
}

/*
 * @brief Function to request identification tlm
 */
int8 adcs_request_identification_tlm(COMMON_Identification_t *id_stat)
{
    int8 status = -1;
    uint8 tlmId = 0;
    uint16 size = 0;
    uint8 *tlm_buffer = NULL;

    // Request Identification telemetry
    tlmId = Identification;
    size = COMMON_GetTlmLen(Identification);
    tlm_buffer = os_malloc_eram(size);
    if(NULL != tlm_buffer)
    {
        status = adcs_requesttlm(tlmId, tlm_buffer, size);
        if(TLM_OK == status)
        {
            COMMON_ExtractIdentificationTlm(tlm_buffer, id_stat);
        }
        else
        {
            // Move to error state
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("CUBE ADCS: Error receving TLM in "
                    "request_identification_tlm %ld \n",
                    status));
            log_id_with_one_arg(LOG_ERROR,LOG_ADCS,\
            Error_recv_tlm_in_req_identification_tlm_i8_,status);
#pragma GCC diagnostic pop
            status = TC_TM_ERROR;
        }
        os_free(tlm_buffer);
    }
    else
    {
        // Move to error state
        DEBUG_CPRINT(("CUBE ADCS: os_malloc_eram failed in"
                    " request_identification_tlm \n"));
        log_id(LOG_ERROR,LOG_ADCS,OSmalloc_failed_in_req_identification_tlm);
    }
    return status;
}

/*
 * @brief Function to request rate sensor tlm
 */
static e_adcs_status adcs_request_rate_sens_rates_tlm(CUBEACP_AdcsMeasure_t*\
        rate_sens_rates)
{
    e_adcs_status status = OPERATION_FAILURE;
    e_tctm_status tlm_sts;
    uint8 tlmId = 0;
    uint16 size = 0;
    uint8 *tlm_buffer = NULL;
    CUBEACP_AdcsMeasure_t raw_sens_rates={0};
    raw_sens_rates=raw_sens_rates;

    //  Rate Sensor Rates Telemetry
#ifdef TEST_HOOK
    DEBUG_CPRINT(("\n ******** Rate Sensor Rates Telemetry test ********\n"));
    log_id(LOG_INFO_L2,LOG_ADCS,Rate_sens_rates_tlm_test);
#endif
    tlmId = RateSensor;
    size = CUBEACP_GetTlmLen(RateSensor);
    tlm_buffer = os_malloc_eram(size);
    if(NULL != tlm_buffer)
    {
        tlm_sts = adcs_requesttlm(tlmId, tlm_buffer, size);
        if(TLM_OK == tlm_sts)
        {
            CUBEACP_ExtractRateSensorTlm(tlm_buffer, rate_sens_rates);
            status = OPERATION_SUCCESS;
#ifdef TEST_HOOK
            DEBUG_CPRINT(("rateX is %lf \n", rate_sens_rates->rateX));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,rateX_is__u32__pt__u32_,\
            rate_sens_rates->rateX);
            DEBUG_CPRINT(("rateY is %lf \n", rate_sens_rates->rateY));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,rateY_is__u32__pt__u32_,\
            rate_sens_rates->rateY);
            DEBUG_CPRINT(("rateZ is %lf \n", rate_sens_rates->rateZ));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,rateZ_is__u32__pt__u32_,\
            rate_sens_rates->rateZ);
#endif
        }
        else
        {
            // Move to error state
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("CUBE ADCS: Error receving TLM in "
                    "request_rate_sens_rates_tlm %ld \n",
                    status));
            log_id_with_one_arg(LOG_ERROR,LOG_ADCS,\
            Error_recv_TLM_in_req_rate_sens_rate_tlm__s32_,status);
            status = TC_TM_ERROR;
#pragma GCC diagnostic pop
        }
        os_free(tlm_buffer);
    }
    else
    {
        // Move to error state
        DEBUG_CPRINT(("CUBE ADCS: os_malloc_eram failed in"
                    " request_rate_sens_rates_tlm\n"));
        log_id(LOG_ERROR,LOG_ADCS,OSmalloc1_failed_in_req_rate_sens_rates_tlm);
    }

    return status;
}

/*
 * @brief Function to request estimation rate sensor tlm
 */
static e_adcs_status adcs_request_est_rate_sens_rates_tlm(CUBEACP_AdcsState_t*\
        est_rate_sens_rates)
{
    e_adcs_status status = OPERATION_FAILURE;
    e_tctm_status tlm_sts;
    uint8 tlmId = 0;
    uint16 size = 0;
    uint8 *tlm_buffer = NULL;
    CUBEACP_AdcsMeasure_t raw_sens_rates={0};
    raw_sens_rates=raw_sens_rates;

    //  Rate Sensor Rates Telemetry
#ifdef TEST_HOOK
    DEBUG_CPRINT(("\n ******* Rate Sensor Estimated Rates Telemetry"
                " test *******\n"));
    log_id(LOG_INFO_L2,LOG_ADCS,Rate_Sensor_Estimated_Rates_Telemetry_test);
#endif
    tlmId = EstimRates;
    size = CUBEACP_GetTlmLen(EstimRates);
    tlm_buffer = os_malloc_eram(size);
    if(NULL != tlm_buffer)
    {
        tlm_sts = adcs_requesttlm(tlmId, tlm_buffer, size);
        if(TLM_OK == tlm_sts)
        {
            CUBEACP_ExtractEstimRatesTlm(tlm_buffer, est_rate_sens_rates);
            // apply mems transformtion if it is memsgyro estimation mode
            // est_rate_sens_rates->rateX = (-1 * (raw_sens_rates.rateZ));
            // est_rate_sens_rates->rateY = (-1 * (raw_sens_rates.rateX));
            // est_rate_sens_rates->rateZ = raw_sens_rates.rateY;
            status = OPERATION_SUCCESS;
#ifdef TEST_HOOK
            DEBUG_CPRINT(("Estimated rateX is %lf \n", 
                        est_rate_sens_rates->rateX));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
            Est_rateX_is__u32__pt__u32_,est_rate_sens_rates->rateX);
            DEBUG_CPRINT(("Estimated rateY is %lf \n", 
                        est_rate_sens_rates->rateY));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
            Est_rateY_is__u32__pt__u32_,est_rate_sens_rates->rateY);
            DEBUG_CPRINT(("Estimated rateZ is %lf \n", 
                        est_rate_sens_rates->rateZ));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,\
            Est_rateZ_is__u32__pt__u32_,est_rate_sens_rates->rateZ);
#endif
        }
        else
        {
            // Move to error state
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("CUBE ADCS: Error receving TLM in "
                    "request_rate_sens_rates_tlm %ld \n",
                    status));
            log_id_with_one_arg(LOG_ERROR,LOG_ADCS,\
            Error_recv_TLM_in_req_rate_sens_rates_tlm__s32_,status);
#pragma GCC diagnostic pop
            status = TC_TM_ERROR;
        }
        os_free(tlm_buffer);
    }
    else
    {
        // Move to error state
        DEBUG_CPRINT(("CUBE ADCS: os_malloc_eram failed in"
                    " request_rate_sens_rates_tlm\n"));
        log_id(LOG_ERROR,LOG_ADCS,OSmalloc2_failed_in_req_rate_sens_rates_tlm);
    }

    return status;
}

/*
 * @brief Function to request gyro configuration
 */
static e_adcs_status adcs_request_gyro_cfg(void)
{
    e_adcs_status status = OPERATION_FAILURE;
    e_tctm_status tlm_sts;
    uint8 tlmId = 0;
    uint16 size = 0;
    uint8 *tlm_buffer = NULL;
    CUBEACP_AdcsConfig_t gyro_cfg;

    //  Rate Sensor Rates Telemetry
#ifdef TEST_HOOK
    DEBUG_CPRINT(("\n ******** Rate Sensor Gyro configuration "
            "Telemetry test ********\n"));
    log_id(LOG_INFO_L2,LOG_ADCS,Rate_Sensor_Gyro_cfg_Tlm_test);
#endif
    tlmId = GyroConfig;
    size = CUBEACP_GetTlmLen(GyroConfig);
    tlm_buffer = os_malloc_eram(size);
    if(NULL != tlm_buffer)
    {
        tlm_sts = adcs_requesttlm(tlmId, tlm_buffer, size);
        if(TLM_OK == tlm_sts)
        {
            CUBEACP_ExtractGyroConfigTlm(tlm_buffer, &gyro_cfg);
            status = OPERATION_SUCCESS;
#ifdef TEST_HOOK
            DEBUG_CPRINT(("CUBE ADCS TEST :gyro_cfg.rateSensorMult"
                    " : %d \r\n",
                    gyro_cfg.rateSensorMult));
            log_id_with_one_arg(LOG_INFO_L2,LOG_ADCS,\
            gyro_cfg_RateSensMult__u32_,gyro_cfg.rateSensorMult);
#endif
        }
        else
        {
            // Move to error state
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("CUBE ADCS: Error receving TLM in "
                    "request_gyro_cfg %ld \n",
                    status));
            log_id_with_one_arg(LOG_ERROR,LOG_ADCS,\
            Error_recv_TLM_in_req_gyro_cfg__s32_,status);
#pragma GCC diagnostic pop
            status = TC_TM_ERROR;
        }
        os_free(tlm_buffer);
    }
    else
    {
        // Move to error state
        DEBUG_CPRINT(("CUBE ADCS: os_malloc_eram failed in"
                    " request_gyro_cfg \n"));
        log_id(LOG_ERROR,LOG_ADCS,OSmalloc_failed_in_req_gyro_cfg);
    }

    return status;
}

/*
 * @brief Function to request nadir vector tlm
 */
static e_adcs_status adcs_request_nadir_vec_tlm(CUBEACP_AdcsMeasure_t*
        nadir_vec)
{
    e_adcs_status status = OPERATION_FAILURE;
    e_tctm_status tlm_sts;
    uint8 tlmId = 0;
    uint16 size = 0;
    uint8 *tlm_buffer = NULL;

    //  Nadir vector TLM
#ifdef TEST_HOOK
    DEBUG_CPRINT(("\n ******** Nadir Telemetry test ********\n"));
    log_id(LOG_INFO_L2,LOG_ADCS,Nadir_TLM_Test);
#endif
    tlmId = NadirVec;
    size = CUBEACP_GetTlmLen(NadirVec);
    tlm_buffer = os_malloc_eram(size);
    if(NULL != tlm_buffer)
    {
        tlm_sts = adcs_requesttlm(tlmId, tlm_buffer, size);
        if(TLM_OK == tlm_sts)
        {
            CUBEACP_ExtractNadirVecTlm(tlm_buffer, nadir_vec);
            status = OPERATION_SUCCESS;
#ifdef TEST_HOOK
            DEBUG_CPRINT(("nadirX is %lf \n", nadir_vec->nadirX));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,NadirX_is__u32__pt__u32_,\
            nadir_vec->nadirX);
            DEBUG_CPRINT(("nadirY is %lf \n", nadir_vec->nadirY));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,NadirY_is__u32__pt__u32_,\
            nadir_vec->nadirY);
            DEBUG_CPRINT(("nadirZ is %lf \n", nadir_vec->nadirZ));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,NadirZ_is__u32__pt__u32_,\
            nadir_vec->nadirZ);
#endif
        }
        else
        {
            // Move to error state
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("CUBE ADCS: Error receving TLM in "
                    "request_nadir_vec_tlm %ld \n",
                    status));
            log_id_with_one_arg(LOG_INFO_L1,LOG_ADCS,\
            Error_recv_TLM_in_req_nadir_vec_tlm__s32_,status);
#pragma GCC diagnostic pop
            status = TC_TM_ERROR;
        }
        os_free(tlm_buffer);
    }
    else
    {
        // Move to error state
        DEBUG_CPRINT(("CUBE ADCS: os_malloc_eram failed in"
                    " request_nadir_vec_tlm \n"));
        log_id(LOG_INFO_L1,LOG_ADCS,OSmalloc1_failed_in_req_nadir_vec_tlm);
    }

    return status;
}

/*
 * @brief Function to request coarse sun vector tlm
 */
static e_adcs_status adcs_request_coarse_sun_tlm(CUBEACP_AdcsMeasure_t*
        coarse_sun)
{
    e_adcs_status status = OPERATION_FAILURE;
    e_tctm_status tlm_sts;
    uint8 tlmId = 0;
    uint16 size = 0;
    uint8 *tlm_buffer = NULL;

    //  coarse_sun vector TLM
#ifdef TEST_HOOK
    DEBUG_CPRINT(("\n ******** coarse_sun Telemetry test ********\n"));
    log_id(LOG_INFO_L2,LOG_ADCS,Coarse_sun_TLM_Test); 
#endif
    tlmId = CoarseSunVec;
    size = CUBEACP_GetTlmLen(CoarseSunVec);
    tlm_buffer = os_malloc_eram(size);
    if(NULL != tlm_buffer)
    {
        tlm_sts = adcs_requesttlm(tlmId, tlm_buffer, size);
        if(TLM_OK == tlm_sts)
        {
            CUBEACP_ExtractCoarseSunVecTlm(tlm_buffer, coarse_sun);
            status = OPERATION_SUCCESS;
#ifdef TEST_HOOK
            DEBUG_CPRINT(("cssX is %lf \n", coarse_sun->cssX));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,cssX_is__u32__pt__u32_,\
            coarse_sun->cssX);
            DEBUG_CPRINT(("cssY is %lf \n", coarse_sun->cssY));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,cssY_is__u32__pt__u32_,\
            coarse_sun->cssY);
            DEBUG_CPRINT(("cssZ is %lf \n", coarse_sun->cssZ));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,cssZ_is__u32__pt__u32_,\
            coarse_sun->cssZ);
#endif
        }
        else
        {
            // Move to error state
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("CUBE ADCS: Error receving TLM in "
                    "request_coarse_sun_tlm %ld \n",
                    status));
            log_id_with_one_arg(LOG_INFO_L1,LOG_ADCS,\
            Error_recv_TLM_in_req_css_tlm__s32_,status);
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
            status = TC_TM_ERROR;
        }
        os_free(tlm_buffer);
    }
    else
    {
        // Move to error state
        DEBUG_CPRINT(("CUBE ADCS: os_malloc_eram failed in"
                    " request_nadir_vec_tlm \n"));
        log_id(LOG_INFO_L1,LOG_ADCS,OSmalloc2_failed_in_req_nadir_vec_tlm);
    }

    return status;
}

/*
 * @brief Function to request fine sun vector tlm
 */
static e_adcs_status adcs_request_fine_sun_tlm(CUBEACP_AdcsMeasure_t *fine_sun)
{
    e_adcs_status status = OPERATION_FAILURE;
    e_tctm_status tlm_sts;
    uint8 tlmId = 0;
    uint16 size = 0;
    uint8 *tlm_buffer = NULL;

    //  fine_sun vector TLM
#ifdef TEST_HOOK
    DEBUG_CPRINT(("\n ******** fine_sun Telemetry test ********\n"));
    log_id(LOG_INFO_L2,LOG_ADCS,Fine_sun_TLM_Test); 
#endif
    tlmId = FineSunVec;
    size = CUBEACP_GetTlmLen(FineSunVec);
    tlm_buffer = os_malloc_eram(size);
    if(NULL != tlm_buffer)
    {
        tlm_sts = adcs_requesttlm(tlmId, tlm_buffer, size);
        if(TLM_OK == tlm_sts)
        {
            CUBEACP_ExtractFineSunVecTlm(tlm_buffer, fine_sun);
            status = OPERATION_SUCCESS;
#ifdef TEST_HOOK
            DEBUG_CPRINT(("sunX is %lf \n", fine_sun->sunX));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,sunX_is__u32__pt__u32_,\
            fine_sun->sunX);
            DEBUG_CPRINT(("sunY is %lf \n", fine_sun->sunY));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,sunY_is__u32__pt__u32_,\
            fine_sun->sunY);
            DEBUG_CPRINT(("sunZ is %lf \n", fine_sun->sunZ));
            log_id_with_float(LOG_INFO_L2,LOG_ADCS,sunZ_is__u32__pt__u32_,\
            fine_sun->sunZ);
#endif
        }
        else
        {
            // Move to error state
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("CUBE ADCS: Error receving TLM in "
                    "request_fine_sun_tlm %ld \n",
                    status));
            log_id_with_one_arg(LOG_INFO_L1,LOG_ADCS,\
            Error_recv_TLM_in_req_fine_sun_tlm__s32_,status);
#pragma GCC diagnostic pop
            status = TC_TM_ERROR;
        }
        os_free(tlm_buffer);
    }
    else
    {
        // Move to error state
        DEBUG_CPRINT(("CUBE ADCS: os_malloc_eram failed in"
                    " request_fine_sun_tlm \n"));
        log_id(LOG_INFO_L1,LOG_ADCS,OSmalloc_failed_in_req_fine_sun_tlm);
    }

    return status;
}

/*
 * @brief Function to request Raw Star Tracker tlm
 */
static e_adcs_status adcs_request_raw_star_tlm(CUBEACP_RawStarTracker_t* \
       raw_star)
{
    e_adcs_status status = OPERATION_FAILURE;
    e_tctm_status tlm_sts;
    uint8 tlmId = 0;
    uint16 size = 0;
    uint8 *tlm_buffer = NULL;

    //  Nadir vector TLM
    tlmId = RawStarTracker;
    size = CUBEACP_GetTlmLen(RawStarTracker);
    tlm_buffer = os_malloc_eram(size);
    if(NULL != tlm_buffer)
    {
        tlm_sts = adcs_requesttlm(tlmId, tlm_buffer, size);
        if(TLM_OK == tlm_sts)
        {
            CUBEACP_ExtractRawStarTrackerTlm(tlm_buffer, raw_star);
            status = OPERATION_SUCCESS;
#ifdef TEST_HOOK
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("imageCaptureSuccess is %lf \n",
                    raw_star->imageCaptureSuccess));
            log_id_with_one_arg(LOG_INFO_L2,LOG_ADCS,\
            imageCaptureSuccess_is__u8_,raw_star->imageCaptureSuccess);
            DEBUG_CPRINT(("detectionSuccess is %lf \n",
                        raw_star->detectionSuccess));
            log_id_with_one_arg(LOG_INFO_L2,LOG_ADCS,detectionSuccess_is_u8_,\
            raw_star->detectionSuccess);
            DEBUG_CPRINT(("identificationSuccess is %lf \n",
                    raw_star->identificationSuccess));
            log_id_with_one_arg(LOG_INFO_L2,LOG_ADCS,\
            identificationSuccess_is_u8_,raw_star->identificationSuccess);
#pragma GCC diagnostic pop
#endif
        }
        else
        {
            // Move to error state
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("CUBE ADCS: Error receving TLM in "
                    "request_raw_star_tlm %ld \n",
                    status));
            log_id_with_one_arg(LOG_INFO_L1,LOG_ADCS,\
            Error_recv_TLM_in_req_raw_star_tlm__s32_,status);
#pragma GCC diagnostic pop
            status = TC_TM_ERROR;
        }
        os_free(tlm_buffer);
    }
    else
    {
        // Move to error state
        DEBUG_CPRINT(("CUBE ADCS: os_malloc_eram failed in"
                    " request_raw_star_tlm \n"));
        log_id(LOG_INFO_L1,LOG_ADCS,OSmalloc_failed_in_req_raw_star_tlm);
    }

    return status;
}

/**
 * @brief Function to handle adcs shutdown command
 */
e_adcs_status vapi_adcs_shutdown(void)
{
    e_adcs_status status = TC_TM_ERROR;
    CUBEACP_AdcsPower_t pwr_ctrl;
    e_tctm_status result;
    uint16 size = 0;
    uint8 *tcBuffer = NULL;

    // Generic tc buffer
    tcBuffer = os_malloc_eram(15);
    pwr_ctrl.cubeControlSignalPower = PowerSelect_PowOff;
    pwr_ctrl.cubeControlMotorPower = PowerSelect_PowOff;
    pwr_ctrl.cubeSense1Power = PowerSelectCubeSense_PowCsOff;
    pwr_ctrl.cubeSense2Power = PowerSelectCubeSense_PowCsOff;
    pwr_ctrl.cubeStarPower = PowerSelect_PowOff;
    pwr_ctrl.cubeWheel1Power = PowerSelect_PowOff;
    pwr_ctrl.cubeWheel2Power = PowerSelect_PowOff;
    pwr_ctrl.cubeWheel3Power = PowerSelect_PowOff;
    pwr_ctrl.motorPower = PowerSelect_PowOff;
    pwr_ctrl.gpsLnaPower = PowerSelect_PowOff;

    size = CUBEACP_FormatAdcsPowerCmd(tcBuffer, &pwr_ctrl);
    result = adcs_send_cmd(tcBuffer[0], &tcBuffer[1], size-1);
    if(TCMD_OK == result)
    {
        status = OPERATION_SUCCESS;
    }
    os_free(tcBuffer);

    return status;
}

/**
 * @brief Function to handle adcs test command
 */
e_adcs_status vapi_adcs_test_mode(void)
{
    e_adcs_status status = OPERATION_SUCCESS;
    status = adcs_test_angular_rate_measurement();
    status |= adcs_test_fine_sun_sens();
    status |= adcs_perform_r_wheel_test();
    if(status != OPERATION_SUCCESS)
    {
        DEBUG_CPRINT(("CUBE ADCS: Test command failed\n"));
        log_id(LOG_INFO_L1,LOG_ADCS,Test_command_failed);
    }
    else
    {
        DEBUG_CPRINT(("CUBE ADCS: Test success\n"));
        log_id(LOG_INFO_L1,LOG_ADCS,Test_success);
    }
    return status;
}

/**
 * @brief Function to log adcs data to SD card
 */
e_adcs_status adcs_log_sd_cube(uint8* param, uint16 period)
{
    uint8 tc_buffer[14] = {0};
    uint16 size = 0;
    e_adcs_status status = OPERATION_FAILURE;
    int8 result;
    size = CUBEACP_FormatLogSdConfig1Cmd(tc_buffer, param, period, 0);
    result = adcs_send_cmd(tc_buffer[0], &tc_buffer[1], size-1);
    if(TCMD_OK == result)
    {
        status = OPERATION_SUCCESS;
    }
    return status;
}
/*
 * @brief # This function executes the fallback mechanism
 */
void adcs_execute_fallback(void)
{
    // Sequence to enable SD card logging of Rate sensor rates,
    // estimated rates and current adcs state
    uint8 sd_log_Seq[18] = {0x1f, 0x7f, 0x68, 0x0, 0x20, 0x0, 0x48, 0x0,  0x0,
        0x0,  0x0,  0x0,  0x0, 0xa,  0x0, 0x0,  0x1f, 0xff};
    // Sequence to disable SD card logging of Rate sensor rates,
    // estimated rates and current adcs state
    uint8 sd_dlog_Seq[18] = {0x1f, 0x7f, 0x68, 0x0, 0x20, 0x0,
        0x48, 0x0,  0x0,  0x0, 0x0,  0x0,
        0x0,  0x0,  0x0,  0x0, 0x1f, 0xff};
    // ADCS Run Mode to Enabled
    uint8 run_mode_enbld[6] = {0x1f, 0x7f, 0xa, 0x1, 0x1f, 0xff};
    // Sequence to Enable cubeControlSignalPower and cubeControlMotorPower
    uint8 pwr_seq[8] = {0x1f, 0x7f, 0xb, 0x5, 0x0, 0x0, 0x1f, 0xff};
    // Set control mode to Z thomson
    uint8 ctrl_seq[8] = {0x1f, 0x7f, 0xd, 0xa, 0x70, 0x17, 0x1f, 0xff};
    // Sequence to set estimation mode to MEMS rate sensing
    uint8 est_seq[6] = {0x1f, 0x7f, 0xe, 0x1, 0x1f, 0xff};
    adcs_exec_seq(sd_dlog_Seq, 18);
    os_delay(5000);
    adcs_exec_seq(run_mode_enbld, 6);
    os_delay(5000);
    adcs_exec_seq(pwr_seq, 8);
    os_delay(5000);
    adcs_exec_seq(est_seq, 6);
    os_delay(5000);
    adcs_exec_seq(ctrl_seq, 8);
    os_delay(5000);
    adcs_exec_seq(sd_log_Seq, 18);
}
/*
 * @brief This API execute the ADCS sequences
 */
e_adcs_status adcs_execute_sequence(uint8 *seq, uint16 size)
{
    e_adcs_status status = OPERATION_SUCCESS;
#ifndef LINUX_TEMP_PORT
    status = OPERATION_FAILURE;
    uint8 *tmp_ptr;
    uint16 rx_size = 0;
    static uint16 seq_no = 0;
    status = adcs_bypass_mode(&seq[2], &adcs_bypass_tlm_buffer[7], size-4, &rx_size);
    // Copy the sequence number back to the TM buffer
    os_memcpy(&adcs_bypass_tlm_buffer, &seq_no, sizeof(seq_no));
    /*
     * Append the TM_CTRL data. 0x70 represents enable
     * encryption, authentication and message type as
     * response. Refer TC/TM manager document for more
     * information
     */
    adcs_bypass_tlm_buffer[2] = 0x70;
#ifdef ADCS_FLIGHT_READY
    adcs_bypass_tlm_buffer[3] = COMMS_SBAND_IQS;
#else
    adcs_bypass_tlm_buffer[3] = GS2_BACK_DOOR_SOC;
#endif
    adcs_bypass_tlm_buffer[4] = ADCS_TCTM_SOF1;
    adcs_bypass_tlm_buffer[5] = ADCS_TCTM_SOF2;
    adcs_bypass_tlm_buffer[6] = 0x7f;
    adcs_bypass_tlm_buffer[7 + rx_size] = ADCS_TCTM_EOF1;
    adcs_bypass_tlm_buffer[8 + rx_size] = ADCS_TCTM_EOF2;
    rx_size = rx_size + 5;
    adcs_csw_trans_msg.Msg_id = adcs_bypass_tlm_buffer[6];
    adcs_csw_trans_msg.Msg_len = rx_size;
    tmp_ptr = os_malloc_eram(rx_size + 4);
    os_memcpy(tmp_ptr, adcs_bypass_tlm_buffer, rx_size + 4);
    adcs_csw_trans_msg.pld.pld_ptr = (uint8 *)tmp_ptr;
    status = os_itc_msg_send(&adcs_csw_trans_msg, TM_STORAGE, \
            os_send_itc_wait);
    seq_no++;
#endif
    return status;
}
/*
 * @brief This function execute the Initial level ADCS sequence
 */
void adcs_init_sequence()
{
    // ADCS Run Mode to Enabled
    uint8 run_mode_enbld[6] = {0x1f, 0x7f, 0xa, 0x1, 0x1f, 0xff};
    // Sequence to Enable cubeControlSignalPower and cubeControlMotorPower
    uint8 pwr_seq[8] = {0x1f, 0x7f, 0xb, 0x5, 0x0, 0x0, 0x1f, 0xff};
    // Sequence to set estimation mode to MEMS rate sensing
    uint8 est_seq[6] = {0x1f, 0x7f, 0xe, 0x1, 0x1f, 0xff};
    /*
     *Sequence to enable SD card logging of Rate sensor rates, estimated rates
     *and current adcs state
     */
    uint8 sd_log_Seq[18] = {0x1f, 0x7f, 0x68, 0x0, 0x20, 0x0, 0x48, 0x0,  0x0,
        0x0,  0x0,  0x0,  0x0, 0xa,  0x0, 0x0,  0x1f, 0xff};
    adcs_exec_seq(run_mode_enbld, 6);
    os_delay(2000);
    adcs_exec_seq(pwr_seq, 8);
    os_delay(2000);
    adcs_exec_seq(est_seq, 6);
    os_delay(2000);
    adcs_exec_seq(sd_log_Seq, 18);
    os_delay(2000);
}

/**
 * @brief Function to get ADCS beacon data
 */
void adcs_get_beacon_data(uint8_t* beacon_data)
{
    CUBEACP_AdcsPower_t pwr = {0};
    s_adcs_beacon_info adcs_beacon_info = {0};
    e_adcs_status status = 0;
    s_adcs_ctx adcs_ctx = {0};
    s_adcs_id *ident = NULL;
    s_adcs_pwr_cmd *pwr_va = NULL;
    s_adcs_rw_speed *msr = NULL;
    s_adcs_mgntrqr_out *mag = NULL;
    uint8 crnt_state = 0;

    //Read current ADCS State
    crnt_state = sm_get_state(ADCS_FSM);
    adcs_beacon_info.fsm_state = crnt_state;

    //Read ADCS Identification for current mode
    ident = os_malloc_eram(sizeof(s_adcs_id));
    status = adcs_vapi_get_id(&adcs_ctx, ident);
    os_delay(10);
    if(status == 0)
    {
        if(ident->id == 10)
        {
            adcs_beacon_info.mode = 0;
        }
        else if(ident->id == 110)
        {
            adcs_beacon_info.mode = 1;
        }
        else
        {
            adcs_beacon_info.mode = 2;
        }
    }
    else
    {
        adcs_beacon_info.mode = 2;
    }
    os_free(ident);

    //Read sensor and actuator (ON/OFF)status
    pwr_va = os_malloc_eram(sizeof(s_adcs_pwr_cmd));
    status = adcs_vapi_get_power(&adcs_ctx, pwr_va);
    os_delay(10);
    CUBEACP_ExtractAdcsPowerTlm((uint8_t*)pwr_va, &pwr);

    if(status == 0)
    {
        adcs_beacon_info.sns_actr_pwr_sts |= pwr.cubeControlSignalPower;
        adcs_beacon_info.sns_actr_pwr_sts |= pwr.cubeControlMotorPower << 1;
        adcs_beacon_info.sns_actr_pwr_sts |= pwr.cubeSense1Power << 2;
        adcs_beacon_info.sns_actr_pwr_sts |= pwr.cubeSense2Power << 4;
        adcs_beacon_info.sns_actr_pwr_sts |= pwr.cubeStarPower << 6;
        adcs_beacon_info.sns_actr_pwr_sts |= pwr.cubeWheel1Power << 7;
        adcs_beacon_info.sns_actr_pwr_sts |= pwr.cubeWheel2Power << 8;
        adcs_beacon_info.sns_actr_pwr_sts |= pwr.cubeWheel3Power << 9;
        adcs_beacon_info.sns_actr_pwr_sts |= pwr.motorPower << 10;
        adcs_beacon_info.sns_actr_pwr_sts |= pwr.gpsLnaPower << 11;
        adcs_beacon_info.sns_actr_pwr_sts |= 0 <<12;
    }
    else
    {
        //5 Status fail
        adcs_beacon_info.sns_actr_pwr_sts = 0;
        adcs_beacon_info.sns_actr_pwr_sts |= 1 <<12;
    }
    os_free(pwr_va);

#define NO_OF_RW    3
    //Read Reaction Wheel Speed and Actuator
    msr = os_malloc_eram(sizeof(s_adcs_rw_speed)*NO_OF_RW);
    status = adcs_vapi_get_rw_speed_measure(&adcs_ctx, NO_OF_RW, msr);
    os_delay(10);
    if(status == 0)
    {
        if(msr[0].rw_speed !=0)
        {
            adcs_beacon_info.actrs_run_sts |= 1<<0;
        }
        else
        {
            adcs_beacon_info.actrs_run_sts |= 0<<0;
        }

        if(msr[1].rw_speed !=0)
        {
            adcs_beacon_info.actrs_run_sts |= 1<<1;
        }
        else
        {
            adcs_beacon_info.actrs_run_sts |= 0<<1;
        }

        if(msr[2].rw_speed !=0)
        {
            adcs_beacon_info.actrs_run_sts |= 1<<2;
        }
        else
        {
            adcs_beacon_info.actrs_run_sts |= 0<<2;
        }
    }
    os_free(msr);

    //Read ADCS magnetorquer out
    mag = os_malloc_eram(sizeof(s_adcs_mgntrqr_out));
#ifndef ADCS_HW_SIM
    status = adcs_vapi_get_mgntrqr_cmd(&adcs_ctx, mag);
    os_delay(10);
#endif
    if(status == 0)
    {
        if(mag->torquer_cmd_x!=0)
        {
            adcs_beacon_info.actrs_run_sts |= 1<<3;
        }
        else
        {
            adcs_beacon_info.actrs_run_sts |= 0<<3;
        }

        if(mag->torquer_cmd_y!=0)
        {
            adcs_beacon_info.actrs_run_sts |= 1<<4;
        }
        else
        {
            adcs_beacon_info.actrs_run_sts |= 0<<4;
        }

        if(mag->torquer_cmd_z!=0)
        {
            adcs_beacon_info.actrs_run_sts |= 1<<5;
        }
        else
        {
            adcs_beacon_info.actrs_run_sts |= 0<<5;
        }
    }
    os_free(mag);
    //Copy the readed data into 4byte
    beacon_data[0] = adcs_beacon_info.fsm_state;
    beacon_data[0] |= adcs_beacon_info.mode<<4;
    beacon_data[1] = adcs_beacon_info.sns_actr_pwr_sts & 0xff;
    beacon_data[2] = (adcs_beacon_info.sns_actr_pwr_sts >> 8) & 0x1f;
    beacon_data[3] = adcs_beacon_info.actrs_run_sts;
}

