/**
 * @file adcs_cube_bypass.c
 *
 * @brief This file contains functionalities that tests the cube
 *        ADCS bypass test functionality
 *
 * @copyright Copyright 2023 Antaris, Inc.
 */
/*********************************************************************/
#include "adcs_cube_bypass.h"

/// Buffer to store received TLM data
uint8 adcs_test_buf[20] = {0};
uint8 adcs_test_rcv_buf[20] = {0};

/**
 * @brief This API decodes the communication interface based on the
 * received data in buf. It uses the start of frame identifier
 * to analyze whether the interface is UART or I2C. This info is
 * taken from the Section 5 of CubeADCS - Firmware Reference
 * Manual [V7.2]. If the SOF doesn't match with I2C or UART
 * the api will consider the interface as CAN.
 *
 * @param[in] buf   : pointer to data.
 *
 * @return Type of interface
 * @retval ADCS_UART or ADCS_I2C or ADCS_CAN -> Success
 *         ADCS_INTF_NULL_PTR  or ADCS_INTRFC_ERR -> Error
 */
/* commenting below function for waring clearance:defined but not used
   static e_intrfc_typ adcs_decode_cube_interface(uint8 *buf);

*/
/**
 * @brief Utility api to print the gyro configuration
 *
 * @param[in] gyro_cfg  : structure instance of CUBEACP_AdcsConfig_t.
 *
 * @return void
 * @retval void
 */
static void adcs_print_gyro_cfg(CUBEACP_AdcsConfig_t gyro_cfg);

/**
 * @brief Utility api to compare gyro configuration
 *
 * @param[in] gyro_cfg1  : CUBEACP_AdcsConfig_t struct instance
 * @param[in] gyro_cfg2  : CUBEACP_AdcsConfig_t struct instance
 *
 * @return status of comparison
 * @retval 0 -> All values are same other values-> failure
 */
static int8 adcs_compare_gyro_cfg(CUBEACP_AdcsConfig_t gyro_cfg1,
        CUBEACP_AdcsConfig_t gyro_cfg2);

/**
 * @brief Utility api to compare identification telemetry
 *
 * @param[in] id_stat  : COMMON_Identification_t struct instance
 *
 * @return status of comparison
 * @retval 0 -> All values are same other values-> failure
 */
static int8 adcs_compare_id_tlm(COMMON_Identification_t id_stat);

/**
 * @brief This API decodes the communication interface based on the
 * received data in buf. It uses the start of frame identifier to analyze
 * whether the interface is UART or I2C. This info is taken from the
 * Section 5 of CubeADCS - Firmware Reference Manual [V7.2].
 * If the SOF doesn't match with I2C or UART the api will consider
 * the interface as CAN.
 */
/* commenting below function for waring clearance:defined but not used
static e_intrfc_typ adcs_decode_cube_interface(uint8 *buf)
{
    e_intrfc_typ status = ADCS_INTRFC_ERR;
    if(NULL != buf)
    {
        if((0x1F == buf[0]) && (0x7F == buf[1]))
        {
            status = ADCS_UART;
        }
        else if(0xAE == buf[0])
        {
            status = ADCS_I2C;
        }
        else
        {
            status = ADCS_CAN;
        }
    }
    else
    {
        status = ADCS_INTF_NULL_PTR;
    }

    return status;
}*/

/**
 * @brief Internal test function to test the interface
 */
int32 adcs_test_bypass(void)
{
    int8 cmpr_sts = 0;
    COMMON_Identification_t id_stat;
    uint16 size = 0;
    uint16 rx_size = 0;
    CUBEACP_AdcsConfig_t org_gyro_cfg;
    CUBEACP_AdcsConfig_t set_gyro_cfg;
    CUBEACP_AdcsConfig_t get_gyro_cfg;

    adcs_test_buf[0] = 0x1f;
    adcs_test_buf[1] = 0x7f;
    adcs_test_buf[2] = Identification;
    size = COMMON_GetTlmLen(Identification);
    vapi_adcs_bypass_mode(adcs_test_buf, adcs_test_rcv_buf, size, &rx_size);
    COMMON_ExtractIdentificationTlm(&adcs_test_rcv_buf[3], &id_stat);

    cmpr_sts |= adcs_compare_id_tlm(id_stat);

    adcs_test_buf[2] = GyroConfig;
    size = CUBEACP_GetTlmLen(GyroConfig);
    vapi_adcs_bypass_mode(adcs_test_buf, adcs_test_rcv_buf, size, &rx_size);
    CUBEACP_ExtractGyroConfigTlm(&adcs_test_rcv_buf[3], &org_gyro_cfg);
    adcs_print_gyro_cfg(org_gyro_cfg);

    // Set maximum values and compare with get
    set_gyro_cfg.gyro1Conf = AxisSelect_PosXPosY45deg;
    set_gyro_cfg.gyro2Conf = AxisSelect_PosXPosY45deg;
    set_gyro_cfg.gyro3Conf = AxisSelect_PosXPosY45deg;
    set_gyro_cfg.xRateOffset = 32.767;
    set_gyro_cfg.yRateOffset = 32.767;
    set_gyro_cfg.zRateOffset = 32.767;
    set_gyro_cfg.rateSensorMult = 12;
    size = CUBEACP_FormatGyroConfigCmd(&adcs_test_buf[2], &set_gyro_cfg);
    vapi_adcs_bypass_mode(adcs_test_buf, adcs_test_rcv_buf, size + 4, &rx_size);

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wimplicit-function-declaration"
    HAL_Delay(1);
#pragma GCC diagnostic pop
    adcs_test_buf[2] = GyroConfig;
    size = CUBEACP_GetTlmLen(GyroConfig);
    vapi_adcs_bypass_mode(adcs_test_buf, adcs_test_rcv_buf, size, &rx_size);
    CUBEACP_ExtractGyroConfigTlm(&adcs_test_rcv_buf[3], &get_gyro_cfg);
    adcs_print_gyro_cfg(get_gyro_cfg);

    cmpr_sts |= adcs_compare_gyro_cfg(set_gyro_cfg, get_gyro_cfg);

    size = CUBEACP_FormatGyroConfigCmd(&adcs_test_buf[2], &org_gyro_cfg);
    vapi_adcs_bypass_mode(adcs_test_buf, adcs_test_rcv_buf, size + 4, &rx_size);

    HAL_Delay(1);

    adcs_test_buf[2] = GyroConfig;
    size = CUBEACP_GetTlmLen(GyroConfig);
    vapi_adcs_bypass_mode(adcs_test_buf, adcs_test_rcv_buf, size, &rx_size);
    CUBEACP_ExtractGyroConfigTlm(&adcs_test_rcv_buf[3], &get_gyro_cfg);
    adcs_print_gyro_cfg(get_gyro_cfg);

    cmpr_sts |= adcs_compare_gyro_cfg(org_gyro_cfg, get_gyro_cfg);

    if(0 == cmpr_sts)
    {
        DEBUG_CPRINT((" ADCS interface sanity test is SUCCESS \r\n"));
    }
    else
    {
        DEBUG_CPRINT((" ADCS interface sanity test FAILED \r\n"));
    }

    return cmpr_sts;
}

/**
 * @brief Utility api to print the gyro configuration
 */
static void adcs_print_gyro_cfg(CUBEACP_AdcsConfig_t gyro_cfg)
{
    DEBUG_CPRINT((" \r\n gyro_cfg.gyro1Conf : %d \r\n", gyro_cfg.gyro1Conf));
    DEBUG_CPRINT(("gyro_cfg.gyro2Conf : %d \r\n", gyro_cfg.gyro2Conf));
    DEBUG_CPRINT(("gyro_cfg.gyro3Conf : %d \r\n", gyro_cfg.gyro3Conf));
    DEBUG_CPRINT(("gyro_cfg.rateSensorMult : %d \r\n", gyro_cfg.rateSensorMult));
}

/**
 * @brief Utility api to compare gyro configuration
 */
static int8 adcs_compare_gyro_cfg(CUBEACP_AdcsConfig_t gyro_cfg1,
        CUBEACP_AdcsConfig_t gyro_cfg2)
{
    int8 status = -1;

    if(gyro_cfg1.gyro1Conf != gyro_cfg2.gyro1Conf)
    {
    }
    else if(gyro_cfg1.gyro2Conf != gyro_cfg2.gyro2Conf)
    {
    }
    else if(gyro_cfg1.gyro3Conf != gyro_cfg2.gyro3Conf)
    {
    }
    else if(0 != adcs_double_compare(gyro_cfg1.xRateOffset,
                gyro_cfg2.xRateOffset, 1.0))
    {
    }
    else if(0 != adcs_double_compare(gyro_cfg1.yRateOffset,
                gyro_cfg2.yRateOffset, 1.0))
    {
    }
    else if(0 != adcs_double_compare(gyro_cfg1.zRateOffset,
                gyro_cfg2.zRateOffset, 1.0))
    {
    }
    else if(gyro_cfg1.rateSensorMult != gyro_cfg2.rateSensorMult)
    {
    }
    else
    {
        // All values matches
        status = 0;
    }

    return status;
}

/**
 * @brief Utility api to compare identification telemetry
 */
static int8 adcs_compare_id_tlm(COMMON_Identification_t id_stat)
{
    int8 status = -1;
    DEBUG_CPRINT(("NodeType: %d  \r\n", id_stat.nodeType));
    if(10 != id_stat.nodeType)
    {
    }
    else if(7 != id_stat.interfaceVersion)
    {
    }
    else if(7 != id_stat.firmwareMajorVersion)
    {
    }
    else if(9 != id_stat.firmwareMinorVersion)
    {
    }
    else
    {
        status = 0;
        DEBUG_CPRINT(("Identification test is success \r\n"));
        DEBUG_CPRINT(("interfaceVersion is %d \r\n", id_stat.interfaceVersion));
        DEBUG_CPRINT(("firmwareMajorVersion is %d \r\n", id_stat.firmwareMajorVersion));
        DEBUG_CPRINT(("firmwareMinorVersion is %d \r\n", id_stat.firmwareMinorVersion));
        DEBUG_CPRINT(("runtimeSeconds is %d \r\n", id_stat.runtimeSeconds));
        DEBUG_CPRINT(("runtimeMilliseconds is %d \r\n", id_stat.runtimeMilliseconds));
    }

    return status;
}
