/**
 * @file mfcc_tc_tm_adcs.c
 *
 * @brief #TODO
 *
 *
 * #TODO
 *
 */
/*********************************************************************/
#include <string.h>
#include <stdio.h>
#include <stdint.h>
#include "cubeacp.h"
#include "adcs_common.h"
#include "mfcc_tc_tm_sample.h"
#include "adcs_bsk.h"

#define ADCS_TLM_PRINT

uint8_t adcs_tc_data[1024] = {0};
extern s_sock_info tctm_soc_inf;
/*void print_array(uint8_t* data, uint16_t size);
void mfcc_adcs_tc_send(uint32 tc_id, uint8 *adcs_data, uint16 tc_len);
void health_metric_print(uint8_t hm_id, uint8_t hm_buffer_cnt, uint8_t *hm_data);*/
static void cube_tlm_data_extract(uint8* data, uint16 size, uint16 tm_id);
extern uint8_t radio_mod_id;
void parse_adcs_tc_tm_seq_cmd(void)
{
    int32_t port = atoi(tctm_soc_inf.port_num);
    int32_t addr = inet_addr(tctm_soc_inf.ip_addr);
    FILE *fp = fopen("adcs_tc_tm_sequence.txt", "r");
    // reading line by line, max 1024 bytes
    const unsigned MAX_LENGTH = 1024;
    char buffer[MAX_LENGTH];
    char *token;
    uint itr = 0;
    int num = 1;
    int delay_val=0;

    if (fp == NULL)
    {
        printf("MFCC ADCS APP Error: could not open file :adcs_tc_tm_sequence.txt");
        return;
    }

    while (fgets(buffer, MAX_LENGTH, fp))
    {
        if((num%2)!=0)
        {
            /* get the first token */
            token = strtok(buffer, ",");

            /* walk through other tokens */
            while( token != NULL )
            {
                //printf( "%s\n", token );
                adcs_tc_data[itr] = (int)strtol(token, NULL, 0);
                token = strtok(NULL, ",");
                itr++;
            }
#ifdef ADCS_ENBL_DBG_PRNT
            print_array(adcs_tc_data, itr);
#endif
            sleep(delay_val);
            mfcc_tcp_send(addr, port, adcs_tc_data, itr);
            itr = 0;
            delay_val = 0;
        }
        else
        {
            delay_val = (int)strtol(buffer, NULL, 0);
        }
        num++;
     }

    // close the file
    fclose(fp);
}

void parse_adcs_seq_cmd(void)
{

    FILE *fp = fopen("adcs_sequence.txt", "r");

    // reading line by line, max 1024 bytes
    const unsigned MAX_LENGTH = 1024;
    char buffer[MAX_LENGTH];
    char *token;
    uint itr = 0;
    int num = 1;
    int delay_val=0;

    if (fp == NULL)
    {
        printf("MFCC ADCS APP Error: could not open file %s", "adcs_sequence.txt");
        return;
    }

    while (fgets(buffer, MAX_LENGTH, fp))
    {
        if((num%2)!=0)
        {
            /* get the first token */
            token = strtok(buffer, ",");

            /* walk through other tokens */
            while( token != NULL )
            {
                adcs_tc_data[itr] = (int)strtol(token, NULL, 0);
                token = strtok(NULL, ",");
                itr++;
            }

            sleep(delay_val);
            printf("MFCC ADCS APP: TC ID is %d \n",adcs_tc_data[2]);
            mfcc_adcs_tc_send(adcs_tc_data[2], adcs_tc_data, itr);
            itr = 0;
            delay_val = 0;
        }
        else
        {
            delay_val = (int)strtol(buffer, NULL, 0);
        }
        num++;
    }

    // close the file
    fclose(fp);
}

void mfcc_adcs_tc_send(uint32_t tc_id, uint8_t *adcs_data, uint16_t tc_len)
{
    int32 port = atoi(tctm_soc_inf.port_num);
    int32 addr = inet_addr(tctm_soc_inf.ip_addr);
    s_tc_frame *tc_orig_app;
    uint8 *dst;
    int32 size = 0;
#ifdef ADCS_ENBL_DBG_PRNT
    uint32 loop = 0;
#endif

    /* Encryption related params */
    s_crypt_params param;
    uint8 key[] = {0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
                   0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                   0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};

    uint8 iv[16] = {0xff, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                    0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};

    /* Send buffer and frame memory allocations */
    tc_orig_app = (s_tc_frame *) malloc(sizeof(s_tc_frame));
    dst = (unsigned char *) malloc(MAX_TC_BUFF_SIZE);

    /* The above part need to be built in the Go Language */

    /* From here onwards TC TM library is used */
    /* Initialize telecommand structure instance */
    tc_orig_init(tc_orig_app);
    /* Reset the sequence number */
    //tc_orig_reset_seq_no();
    /* Configure Start of Frame */
    tc_orig_cfg_sof(tc_orig_app, TC_TM_LIB_SOF1, TC_TM_LIB_SOF2);
    /* Configure Satellite ID */
    tc_orig_cfg_sat_id(tc_orig_app, RECV_SAT_ID);
    /* Configure Ground station ID */
    tc_orig_cfg_gs_id(tc_orig_app, SOURCE_GROUND_STN_ID);
    /* Enable encryption */
    tc_orig_cfg_tc_ctrl(tc_orig_app, CRYPTO_ENCRYP_ENBL);
    /* Configure Quality of Service */
    tc_orig_cfg_qos(tc_orig_app, TC_QOS_CFG);
    /* Configure source address */
    tc_orig_cfg_sa_id(tc_orig_app, TC_SRC_GS);
    /* Configure Radio Module ID */
    tc_orig_cfg_rm_id(tc_orig_app, radio_mod_id);
    /* Configure destination address */

    if(TC_DEFERRED_STRG == tc_id)
    {
        tc_orig_cfg_da_id(tc_orig_app, TC_DST_OBC);
    }
    else
    {
        tc_orig_cfg_da_id(tc_orig_app, TC_DST_ADCS);
    }
    /* Configure Telecommand ID */
    tc_orig_cfg_tc_id(tc_orig_app, tc_id);
    /* Configure Telecommand Length */
    tc_orig_cfg_tc_len(tc_orig_app, tc_len);
    /* Configure Telecommand payload */
    tc_orig_cfg_tc_data (tc_orig_app, adcs_data);
    tc_orig_cfg_eof(tc_orig_app, TC_TM_LIB_EOF);

    /* Configure Encryption parameters */
    tc_orig_app->cmac_hmac = (uint8_t *) malloc(SHA256_HASH_SIZE);
    memcpy(param.iv, iv, 16);
    param.key_size = 256;
    param.key = (unsigned char *)malloc(param.key_size);
    memcpy(param.key, key, param.key_size);
    param.encrypt_mode = CRYPT_AES_CFB8;
    tc_orig_select_encryption(&param);

#ifdef ADCS_ENBL_DBG_PRNT
    printf("MFCC ADCS APP: TC payload : ");
    for(loop = 0; loop < tc_len; loop++)
        printf("%x ", adcs_data[loop]);
    printf("\n");
#endif

    /* Generate Telecommand as binary data stream */
    size = tc_pack(dst, tc_orig_app);

#ifdef ADCS_ENBL_DBG_PRNT
    printf("MFCC ADCS APP: TC byte stream to be transmitted: ");
    for(loop = 0; loop < size; loop++)
        printf("%x ", dst[loop]);
      printf("\n");
#endif

    /* Send generated Telecommand via socket interface */
    mfcc_tcp_send(addr, port, dst, size);
    free(tc_orig_app->cmac_hmac);
    free(tc_orig_app);
    free(param.key);
    free(dst);
}

void tm_adcs_dec(uint8* data, uint16 size, uint16 tm_id)
{
    int itr = 0;

    // creating file pointer to work with files
    FILE *fptr;

    // opening file in writing mode
    fptr = fopen("adcs_tlm_log.txt", "ab+");

    // exiting program 
    if (fptr == NULL) {
        printf("adcs_tlm_log.txt File Open Error!");
        exit(1);
    }
    if(NULL != data)
    {
#ifdef ADCS_ENBL_DBG_PRNT
        printf("MFCC ADCS APP: ADCS telemetry response: ");
        print_array(data, size);
#endif
        if((data[2] < 125) && (tm_id != 119) && (tm_id == 252))
        {
            if(tm_id == 252 || tm_id == 46)
            {
                printf("MFCC ADCS APP: Firmware Upload Status:- ");
                if(FW_UPLOAD_SUCCESS == (int8_t)data[3])
                {
                    printf("Success\n");
                }
                else if(FW_UPLOAD_FAILURE == (int8_t)data[3])
                {
                    printf("Failure\n");
                }
                else if(FW_NOT_AVAILABLE == (int8_t)data[3])
                {
                    printf("Not Available\n");
                }
                else if(FW_SDRAM_CRC_ERR == (int8_t)data[3])
                {
                    printf("SDRAM CRC Error\n");
                }
                else if(FW_FMC_NOR_CRC_ERR == (int8_t)data[3])
                {
                    printf("FMC NOR CRC Error\n");
                }
                else if(FW_ADCS_CRC_ERR == (int8_t)data[3])
                {
                    printf("CRC Error\n");
                }
                else if(FW_ADCS_BCK_FW_UPLD_SCS == (int8_t)data[3])
                {
                    printf("Backup Firmware upload Success\n");
                }
            }
            else
            {
                if(data[3] == 0)
                {
                    printf("MFCC ADCS APP: ADCS SUCCESS Id: %d \n",data[2]);
                }
                else
                {
                    printf("MFCC ADCS APP: ADCS FAILED Id: %d Error:%d\n",data[2],(int8_t)data[3]);
                }
            }
        }
        else
        {
            if(data[2] == 127)
            {
                if(data[3] == 0)
                {
                    printf("MFCC ADCS APP: ADCS SUCCESS Id: %d \n",data[4]);
                    cube_tlm_data_extract(&data[5], size, data[4]);
                }
                else
                {
                    printf("MFCC ADCS APP: ADCS FAILED Id: %d Error:%d\n",data[4],(int8_t)data[3]);
                }

            }
            if(tm_id == 254)
            {
                if(data[3] == 0)
                {
                    health_metric_print(data[4], data[5], &data[6]);
                }
                else
                {
                    printf("Health Metrics Error status:- %d\n", (int8_t)data[3]);
                }
            }
            else
            {
                if(0 == data[3] && data[2] != 127)
                {
                    printf("MFCC ADCS APP: ADCS SUCCESS Id: %d \n", tm_id);
                    unfied_tlm_data_extract(&data[4], size, tm_id);
                }
            }

            if(data[3] == 0 || tm_id == 119 || ((data[2] >= 125) && (data[2] <= 127)))
            {
               printf("MFCC ADCS APP: Telemetry response written to .txt file \n");
                for(itr = 0; itr < size; itr++)
                {
                    if(itr == (size-1))
                    {
                        fprintf(fptr,"0x%02x", data[itr]);
                    }
                    else
                    {
                        fprintf(fptr,"0x%02x,", data[itr]);
                    }
                }
                fprintf(fptr,"\n");
            }
            else
            {
                printf("MFCC ADCS APP: ADCS FAILED Id:- %d Error:%d\n",data[2],(int8_t)data[3]);
            }
        }
    }
    else
    {
        printf("MFCC ADCS APP: NULL Telemetry data from ADCS \n");
    }

    fclose(fptr);
}


void print_array(uint8_t* data, uint16_t size)
{
    int itx = 0;
    for(itx = 0; itx<size;itx++)
    {
        printf("0x%x ", data[itx]);
    }
}

static void cube_tlm_data_extract(uint8* data, uint16 size, uint16 tm_id)
{
    CUBEACP_CmdAttitude_t attitude;
    CUBEACP_AdcsMeasure_t measured_rate;
    CUBEACP_FineEstimRates_t fine_est_rate;
    CUBEACP_AdcsMeasure_t rwheel_speed;
    CUBEACP_ActuatorCmd_t magtorq_cmd;
    CUBEACP_AdcsPowerMeasure_t measured_pwr_temp;
    CUBEACP_AdcsState_t currnt_state;
    CUBEACP_AdcsState_t error_state;
    COMMON_ExtendedIdentification_t extnd_id;

    printf("ADCS: TM ID is %d \n", tm_id);
    switch(tm_id)
    {
        /*
         * Data format :
         * data[0] = 0x1f (SOF1)
         * data[1] = 0x7f (SOF2)
         * data[2] = TC/TM id
         * data[3] = Actual ADCS TC/TM data
         */
        case EstimAttitude:
             CUBEACP_ExtractCmdAttitudeTlm(data,
                                           &attitude);
#ifdef ADCS_TLM_PRINT
             printf(" roll : %lf \n",attitude.roll);
             printf(" pitch : %lf \n",attitude.pitch);
             printf(" yaw : %lf \n",attitude.yaw);
#endif
             break;

        case RateSensor:
             CUBEACP_ExtractRateSensorTlm(data,
                                          &measured_rate);
#ifdef ADCS_TLM_PRINT
             printf(" rateX : %lf \n",measured_rate.rateX);
             printf(" rateY : %lf \n",measured_rate.rateY);
             printf(" rateZ : %lf \n",measured_rate.rateZ);
#endif
             break;

        case FineEstimRates:
             CUBEACP_ExtractFineEstimRatesTlm(data,
                                              &fine_est_rate);
#ifdef ADCS_TLM_PRINT
             printf(" Fine est rateX : %f \n",fine_est_rate.rateX);
             printf(" Fine est rateY : %f \n",fine_est_rate.rateY);
             printf(" Fine est rateZ : %f \n",fine_est_rate.rateZ);
#endif
             break;

        case WheelSpeed:
             CUBEACP_ExtractWheelSpeedTlm(data,
                                          &rwheel_speed);
#ifdef ADCS_TLM_PRINT
             printf(" speedX : %d \n",rwheel_speed.speedX);
             printf(" speedY : %d \n",rwheel_speed.speedY);
             printf(" speedZ : %d \n",rwheel_speed.speedZ);
#endif
             break;

        case MagTorquerCmd:
             CUBEACP_ExtractMagTorquerCmdTlm(data,
                                             &magtorq_cmd);
#ifdef ADCS_TLM_PRINT
             printf(" torquerCmdX : %d \n",magtorq_cmd.torquerCmdX);
             printf(" torquerCmdY : %d \n",magtorq_cmd.torquerCmdY);
             printf(" torquerCmdZ : %d \n",magtorq_cmd.torquerCmdZ);
#endif
             break;

        case AdcsPowerMeasure:
             CUBEACP_ExtractAdcsPowerMeasureTlm(data,
                                                &measured_pwr_temp);
#ifdef ADCS_TLM_PRINT
             printf(" cubeSense1_3V3Current : %lf \n",measured_pwr_temp.cubeSense1_3V3Current);
             printf(" cubeSense1_CamCurrent : %lf \n",measured_pwr_temp.cubeSense1_CamCurrent);
             printf(" cubeSense2_3V3Current : %lf \n",measured_pwr_temp.cubeSense2_3V3Current);
             printf(" cubeControl3V3Current : %lf \n",measured_pwr_temp.cubeControl3V3Current);
             printf(" cubeControl5VCurrent : %lf \n",measured_pwr_temp.cubeControl5VCurrent);
             printf(" cubeControlBatCurrent : %lf \n",measured_pwr_temp.cubeControlBatCurrent);
             printf(" wheel1Current : %lf \n",measured_pwr_temp.wheel1Current);
             printf(" wheel2Current : %lf \n",measured_pwr_temp.wheel2Current);
             printf(" wheel3Current : %lf \n",measured_pwr_temp.wheel3Current);
             printf(" cubeStarCurrent : %lf \n",measured_pwr_temp.cubeStarCurrent);
             printf(" magtorqCurrent : %lf \n",measured_pwr_temp.magtorqCurrent);
             printf(" cubeStarTemp : %lf \n",measured_pwr_temp.cubeStarTemp);
             printf(" mCUTemp : %d \n",measured_pwr_temp.mCUTemp);
             printf(" magnetometerTemp : %lf \n",measured_pwr_temp.magnetometerTemp);
             printf(" redundentMagnetometerTemp : %lf \n",measured_pwr_temp.redundentMagnetometerTemp);
             printf(" xRateTemp : %d \n",measured_pwr_temp.xRateTemp);
             printf(" yRateTemp : %d \n",measured_pwr_temp.yRateTemp);
             printf(" zRateTemp : %d \n",measured_pwr_temp.zRateTemp);
#endif
             break;

        case CurrentAdcsState:
             CUBEACP_ExtractCurrentAdcsStateTlm(data,
                                                &currnt_state);
#ifdef ADCS_TLM_PRINT
             printf(" Estimation mode : %d \n",currnt_state.estimMode);
             printf(" Control mode : %d \n",currnt_state.controlMode);
#endif
             break;

        case AdcsState:
             CUBEACP_ExtractAdcsStateTlm(data,
                                         &error_state);
#ifdef ADCS_TLM_PRINT
             printf(" magRangeError : %d \n",error_state.magRangeError);
             printf(" rateSensRangeError : %d \n",error_state.rateSensRangeError);
             printf(" wheelSpeedRangeError : %d \n",error_state.wheelSpeedRangeError);
             printf(" orbitParamsInvalidError : %d \n",error_state.orbitParamsInvalidError);
             printf(" configInvalidError : %d \n",error_state.configInvalidError);
             printf(" nodeRecoveryError : %d \n",error_state.nodeRecoveryError);
             printf(" magnetometerError : %d \n",error_state.magnetometerError);
             printf(" rateSensorFailure : %d \n",error_state.rateSensorFailure);
#endif
             break;
        case ExtendedIdentification:
             COMMON_ExtractExtendedIdentificationTlm(data, &extnd_id);
#ifdef ADCS_TLM_PRINT
             printf(" MCU Reset Cause : %d \n", extnd_id.mcuResetCause);
             printf(" Boot Cause : %d \n", extnd_id.bootCause);
             printf(" Boot Counter : %d \n", extnd_id.bootCounter);
             printf(" Program Index : %d \n", extnd_id.runningProgramIndex);
             printf(" Firmware Major Version : %d \n", extnd_id.firmwareMajorVersion);
             printf(" Firmware Minor Version : %d \n", extnd_id.firmwareMinorVersion);
#endif
             break;
                                            
        default:
             //Do nothing
             break;
    }
}

void health_metric_print(uint8_t hm_id, uint8_t hm_buffer_cnt, uint8_t *hm_data)
{
    s_adcs_hm_sen_cur *hm_sen_cur_buffer;
    s_adcs_hm_cons_cur *hm_cons_cur_buffer;
    s_adcs_hm_rw_cur *hm_rw_cur_buffer;
    s_adcs_hm_css_vector *hm_css_vector_buffer;
    s_adcs_hm_est_att_angl *hm_est_att_angl_buffer;
    s_adcs_hm_raw_mag_measure *hm_raw_mag_measure_buffer;
    s_adcs_hm_raw_rates *hm_raw_rates_buffer;
    s_adcs_hm_meas_rates *hm_meas_rates;
    s_adcs_hm_fss_vec  *hm_fss_vec;
    s_adcs_hm_est_rates_sns *hm_est_rate_sns;
    s_adcs_hm_misc_cur *hm_misc_cur;
    s_adcs_hm_temp *hm_temp;
    s_adcs_hm_est_rate_sns_temp *hm_est_rate_sns_temp;
    s_adcs_hm_crnt_state *hm_crnt_state;
    int idx = 0;

    switch(hm_id)
    {
        case ADCS_HM_SESNOR_CURRENT:
            {
                hm_sen_cur_buffer = (s_adcs_hm_sen_cur*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_sen_cur_buffer[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_sen_cur_buffer[idx].time);
                        printf("Nadir 3v3 Current:- %lf\n", hm_sen_cur_buffer[idx].nadir_3v3_current);
                        printf("Nadir SRAM Current:- %lf\n", hm_sen_cur_buffer[idx].nadir_sram_current);
                        printf("FSS 3v3 Current:- %lf\n", hm_sen_cur_buffer[idx].fss_3v3_current);
                        printf("FSS SRAM Current:- %lf\n", hm_sen_cur_buffer[idx].fss_sram_current);
                    }
                    else
                    {
                        printf("Health Metric Sensor Current Error:- %d\n",hm_sen_cur_buffer[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_CONS_CURRENT:
            {
                hm_cons_cur_buffer = (s_adcs_hm_cons_cur*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_cons_cur_buffer[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_cons_cur_buffer[idx].time);
                        printf("3v3 Current:- %lf\n", hm_cons_cur_buffer[idx].current_3v3);
                        printf("5V Current:- %lf\n", hm_cons_cur_buffer[idx].current_5v);
                        printf("Vbat Current:- %lf\n", hm_cons_cur_buffer[idx].current_vbat);
                    }
                    else
                    {
                        printf("Health Metric Consumed Current Error:- %d\n",hm_cons_cur_buffer[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_RW_CURRENT:
            {
                hm_rw_cur_buffer = (s_adcs_hm_rw_cur*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_rw_cur_buffer[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_rw_cur_buffer[idx].time);
                        printf("Reaction wheel 1 current:- %lf\n", hm_rw_cur_buffer[idx].rw1_current);
                        printf("Reaction wheel 2 current:- %lf\n", hm_rw_cur_buffer[idx].rw2_current);
                        printf("Reaction wheel 3 current:- %lf\n", hm_rw_cur_buffer[idx].rw3_current);
                    }
                    else
                    {
                        printf("Health Metric RW Current Error:- %d\n",hm_rw_cur_buffer[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_CSS_VECTOR:
            {
                hm_css_vector_buffer = (s_adcs_hm_css_vector*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_css_vector_buffer[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_css_vector_buffer[idx].time);
                        printf("CSS X axis Sun Vector:- %d\n", hm_css_vector_buffer[idx].sun_x);
                        printf("CSS Y axis Sun Vector:- %d\n", hm_css_vector_buffer[idx].sun_y);
                        printf("CSS Z axis Sun Vector:- %d\n", hm_css_vector_buffer[idx].sun_z);
                    }
                    else
                    {
                        printf("Health Metric CSS Error:- %d\n",hm_css_vector_buffer[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_EST_ATTITUDE_ANGLE:
            {
                hm_est_att_angl_buffer = (s_adcs_hm_est_att_angl*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_est_att_angl_buffer[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_est_att_angl_buffer[idx].time);
                        printf("Estimated Roll Angle:- %lf\n", hm_est_att_angl_buffer[idx].roll);
                        printf("Estimated Pitch Angle:- %lf\n", hm_est_att_angl_buffer[idx].pitch);
                        printf("Estimated Yaw Angle:- %lf\n", hm_est_att_angl_buffer[idx].yaw);
                    }
                    else
                    {
                        printf("Health Metric Estimated Attitude Angle Error:- %d\n",hm_est_att_angl_buffer[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_RAW_MAG_MEASURE:
            {
                hm_raw_mag_measure_buffer = (s_adcs_hm_raw_mag_measure*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_raw_mag_measure_buffer[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_raw_mag_measure_buffer[idx].time);
                        printf("Raw magnetometer X measurement:- %d\n", hm_raw_mag_measure_buffer[idx].mag_raw_x);
                        printf("Raw magnetometer Y measurement:- %d\n", hm_raw_mag_measure_buffer[idx].mag_raw_y);
                        printf("Raw magnetometer Z measurement:- %d\n", hm_raw_mag_measure_buffer[idx].mag_raw_z);
                    }
                    else
                    {
                        printf("Health Metric Raw Magnetometer Error:- %d\n",hm_raw_mag_measure_buffer[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_RAW_RATE_SENSOR_MEASURE:
            {
                hm_raw_rates_buffer = (s_adcs_hm_raw_rates*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_raw_rates_buffer[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_raw_rates_buffer[idx].time);
                        printf("X axis Raw Rate sensor measurement:- %d\n", hm_raw_rates_buffer[idx].raw_rate_x);
                        printf("Y axis raw rate sensor measurement:- %d\n", hm_raw_rates_buffer[idx].raw_rate_y);
                        printf("Z axis raw rate sensor measurement:- %d\n", hm_raw_rates_buffer[idx].raw_rate_z);
                    }
                    else
                    {
                        printf("Health Metric Raw Rate Sensor Error:- %d\n",hm_raw_rates_buffer[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_RATE_SENSOR_MEASURE:
            {
                hm_meas_rates = (s_adcs_hm_meas_rates*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_meas_rates[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_meas_rates[idx].time);
                        printf("Measured X axis Rates:- %lf\n", hm_meas_rates[idx].rate_x);
                        printf("Measured Y axis Rates:- %lf\n", hm_meas_rates[idx].rate_y);
                        printf("Measured Z axis Rates:- %lf\n", hm_meas_rates[idx].rate_z);
                    }
                    else
                    {
                        printf("Health Metric Measured Rates Error:- %d\n",hm_meas_rates[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_FSS_VECTOR:
            {
                hm_fss_vec = (s_adcs_hm_fss_vec*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_fss_vec[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_fss_vec[idx].time);
                        printf("FSS X axis vector:- %d\n", hm_fss_vec[idx].sun_x);
                        printf("FSS Y axis vector:- %d\n", hm_fss_vec[idx].sun_y);
                        printf("FSS Z axis vector:- %d\n", hm_fss_vec[idx].sun_z);
                    }
                    else
                    {
                        printf("Health Metric FSS Vector Error:- %d\n",hm_fss_vec[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_EST_RATES:
            {
                hm_est_rate_sns = (s_adcs_hm_est_rates_sns*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_est_rate_sns[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_est_rate_sns[idx].time);
                        printf("Estimated X Angular Rate:- %d\n", hm_est_rate_sns[idx].est_rate_x);
                        printf("Estimated Y Angular Rate:- %d\n", hm_est_rate_sns[idx].est_rate_y);
                        printf("Estimated Z Angular Rate:- %d\n", hm_est_rate_sns[idx].est_rate_z);
                    }
                    else
                    {
                        printf("Health Metric Estimated Angular Rates Error:- %d\n",hm_est_rate_sns[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_MISC_CURRENT:
            {
                hm_misc_cur = (s_adcs_hm_misc_cur*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_misc_cur[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_misc_cur[idx].time);
                        printf("CubeStar Current Measurement:- %lf\n", hm_misc_cur[idx].cubestar_cur);
                        printf("Magnetorquer Current Measurement:- %lf\n", hm_misc_cur[idx].mgntqr_cur);
                        printf("CubeStar MCU Temperature:- %lf\n", hm_misc_cur[idx].cubestar_mcu_temp);
                    }
                    else
                    {
                        printf("Health Metric ADCS Misc current Error:- %d\n",hm_misc_cur[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_TEMPERATURE:
            {
                hm_temp = (s_adcs_hm_temp*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_temp[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_temp[idx].time);
                        printf("MCU Temperature:- %d\n", hm_temp[idx].mcu_temp);
                        printf("Magnetometer Temperature:- %lf\n", hm_temp[idx].mgntr_temp);
                        printf("Redundant Magnetometer Temperature:- %lf\n", hm_temp[idx].re_mgntr_temp);
                    }
                    else
                    {
                        printf("Health Metric ADCS Temperature Error:- %d\n",hm_temp[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_RATE_SENSOR_TEMPERATURE:
            {
                hm_est_rate_sns_temp = (s_adcs_hm_est_rate_sns_temp*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_est_rate_sns_temp[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_est_rate_sns_temp[idx].time);
                        printf("X Rate Sensor Temperature:- %d\n", hm_est_rate_sns_temp[idx].x_rate_sns);
                        printf("Y Rate Sensor Temperature:- %d\n", hm_est_rate_sns_temp[idx].y_rate_sns);
                        printf("Z Rate Sensor Temperature:- %d\n", hm_est_rate_sns_temp[idx].z_rate_sns);
                    }
                    else
                    {
                        printf("Health Metric Rate Sensor temperature Error:- %d\n",hm_est_rate_sns_temp[idx].status);
                    }
                }
            }
            break;
        case ADCS_HM_CURRENT_STATE:
            {
                hm_crnt_state = (s_adcs_hm_crnt_state*)hm_data;
                for(idx = 0; idx < hm_buffer_cnt; idx++)
                {
                    if(hm_crnt_state[idx].status == 0)
                    {
                        printf("Time:- %ld\n", hm_crnt_state[idx].time);
                        printf("Attitude Estimation Mode:- %d\n", hm_crnt_state[idx].att_est_mode);
                        printf("ADCS Control Mode:- %d\n", hm_crnt_state[idx].ctrl_mode);
                        printf("ADCS Run mode:- %d\n", hm_crnt_state[idx].run_mode);
                        printf("ASGP4 Mode:- %d\n", hm_crnt_state[idx].asgp4_mode);
                        printf("Cube Control Signal Enabled:- %d\n", hm_crnt_state[idx].ctrl_sig_en);
                        printf("Cube Control Motor Enabled:- %d\n", hm_crnt_state[idx].ctrl_motor_en);
                        printf("Cubesense1 Enabled:- %d\n", hm_crnt_state[idx].cubesense1_en);
                        printf("Cubesense1 Enabled:- %d\n", hm_crnt_state[idx].cubesense2_en);
                        printf("Cube Wheel 1 Enabled:- %d\n", hm_crnt_state[idx].rw1_en);
                        printf("Cube Wheel 2 Enabled:- %d\n", hm_crnt_state[idx].rw2_en);
                        printf("Cube Wheel 3 Enabled:- %d\n", hm_crnt_state[idx].rw3_en);
                        printf("Cube Star Enabled:- %d\n", hm_crnt_state[idx].star_en);
                        printf("GPS Receiver Enabled:- %d\n", hm_crnt_state[idx].gps_recv_en);
                        printf("GPS LNA Power Enabled:- %d\n", hm_crnt_state[idx].gps_lna_pwr_en);
                        printf("Motor Driver Enabled:- %d\n", hm_crnt_state[idx].motor_driver_en);
                        printf("Sun is above local horizon:- %d\n", hm_crnt_state[idx].sun_above_horizon);
                        printf("Cubesense1 Communications Error:- %d\n", hm_crnt_state[idx].cubesens1_comm_err);
                        printf("Cubesense2 Communications Error:- %d\n", hm_crnt_state[idx].cubesens2_comm_err);
                        printf("CubeControl Signal Communications error:- %d\n", hm_crnt_state[idx].ctrl_sig_comm_err);
                        printf("Cubecpntrol Motor communication error:- %d\n", hm_crnt_state[idx].ctrl_motor_comm_err);
                        printf("Cube Wheel1 Communications Error:- %d\n", hm_crnt_state[idx].rw1_comm_err);
                        printf("Cube Wheel2 Communications Error:- %d\n", hm_crnt_state[idx].rw2_comm_err);
                        printf("Cube Wheel3 Communications Error:- %d\n", hm_crnt_state[idx].rw3_comm_err);
                        printf("Cube Star Communications Error:- %d\n", hm_crnt_state[idx].star_comm_err);
                        printf("Magnetometer Range Error:- %d\n", hm_crnt_state[idx].mgntr_range_err);
                        printf("Cam1 SRAM Overcurrent Detected:- %d\n", hm_crnt_state[idx].cam1_sram_ocur);
                        printf("Cam1 3v3 Overcurrent Detected:- %d\n", hm_crnt_state[idx].cam1_3v3_ocur);
                        printf("Cam1 Sensor Busy Error:- %d\n", hm_crnt_state[idx].cam1_sns_busy_err);
                        printf("Cam1 Sensor Detection Error:- %d\n", hm_crnt_state[idx].cam1_sns_err);
                        printf("Sun Sensor range error:- %d\n", hm_crnt_state[idx].sun_sns_range_err);
                        printf("Cam2 SRAM Overcurrent Detected:- %d\n", hm_crnt_state[idx].cam2_sram_ocur);
                        printf("Cam2 3v3 Overcurrent Detected:- %d\n", hm_crnt_state[idx].cam2_3v3_ocur);
                        printf(" Cam2 3v3 Overcurrent Detected :%d\n", hm_crnt_state[idx].cam2_3v3_ocur);
                        printf(" Cam2 Sensor Busy Error :%d\n", hm_crnt_state[idx].cam2_sns_busy_err);
                        printf(" Cam2 Sensor Detection Error :%d\n", hm_crnt_state[idx].cam2_sns_err);
                        printf(" Nadir Sensor Range Error :%d\n", hm_crnt_state[idx].nadir_sns_range_err);
                        printf(" Rate Sensor Range Error :%d\n", hm_crnt_state[idx].rate_sns_range_err);
                        printf(" Wheel Speed Range Error :%d\n", hm_crnt_state[idx].rw_spd_range_err);
                        printf(" Coarse Sun Sensor Error :%d\n", hm_crnt_state[idx].css_err);
                        printf(" Star Tracker Match Error :%d\n", hm_crnt_state[idx].star_trckr_match_err);
                        printf(" Star Tracker Overcurrent Detected:%d\n", hm_crnt_state[idx].star_trckr_ocur);
                        printf(" Orbit Parameters are Invalid :%d\n", hm_crnt_state[idx].orbit_param_invalid);
                        printf(" Configuration is Invalid :%d\n", hm_crnt_state[idx].cfg_invalid);
                        printf(" Control Mode Change is not allowed :%d\n", hm_crnt_state[idx].ctrl_mode_chg_not_alwd);
                        printf(" Estimator Change is not allowed :%d\n", hm_crnt_state[idx].est_chg_not_alwd);
                        printf("Current Magnetometer Sampling Mode:%d\n", hm_crnt_state[idx].cur_mgntr_mode);
                        printf(" Modelled and measured magnetic field differs in size :%d\n", hm_crnt_state[idx].magfield_model_err);
                        printf(" Node Recovery Error :%d\n", hm_crnt_state[idx].node_reco_err);
                        printf(" CubeSense1 Runtime Error :%d\n", hm_crnt_state[idx].cubesense1_runt_err);
                        printf(" CubeSense2 Runtime Error :%d\n", hm_crnt_state[idx].cubesense2_runt_err);
                        printf(" CubeControl Signal Runtime Error :%d\n", hm_crnt_state[idx].cubectrl_sig_runt_err);
                        printf(" CubeControl Motor Runtime Error :%d\n", hm_crnt_state[idx].cubectrl_m_runt_err);
                        printf(" CubeWheel1 Runtime Error :%d\n", hm_crnt_state[idx].rw1_runt_err);
                        printf(" CubeWheel2 Runtime Error :%d\n", hm_crnt_state[idx].rw2_runt_err);
                        printf(" CubeWheel3 Runtime Error :%d\n", hm_crnt_state[idx].rw3_runt_err);
                        printf(" CubeStar Runtime Error :%d\n", hm_crnt_state[idx].star_runt_err);
                        printf(" Magnetometer Error :%d\n", hm_crnt_state[idx].mgntr_err);
                        printf(" Rate Sensor Failure :%d\n", hm_crnt_state[idx].rate_sns_fail);
                    }
                    else
                    {
                        printf("Health Metric Current ADCS state Error:- %d\n",hm_crnt_state[idx].status);
                    }
                }
            }
            break;
    }
}

