#include "thruster_simu.h"
#include "exo_osal.h"

typedef struct
{
    uint8_t thruster_id;
    uint16_t length;
    uint8_t rsp_flag;
    uint16_t register_addr;
}__attribute__((__packed__)) thr_intf_hdr;


thruster_write_fptr_t thrstr_write;
thruster_read_fptr_t  thrstr_read;

/*!
 * @brief This API function is used to set thruster operation mode
 */
uint8_t thruster_simu_set_operating_mode(uint8_t thruster_id, uint8_t set_mde)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 1;
    hdr.rsp_flag = 0;
    hdr.register_addr = THRUSTER_MODE_REG;
    uint16_t len = sizeof(thr_intf_hdr) + 1;
    uint8_t* ptr = os_malloc(len);
    os_memcpy((uint8_t*)ptr,&hdr,sizeof(thr_intf_hdr));
    os_memcpy((uint8_t*)ptr+sizeof(thr_intf_hdr),&set_mde,1);
    sts = thrstr_write(ptr,len);
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to get thruster operation mode
 */
uint8_t thruster_simu_get_operating_mode(uint8_t thruster_id, uint8_t* get_mde)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 0;
    hdr.rsp_flag = 1;
    hdr.register_addr = THRUSTER_MODE_REG;
    uint16_t len = sizeof(thr_intf_hdr)+1;
    uint8_t* ptr = os_malloc(len);
    os_memcpy(ptr,&hdr,sizeof(thr_intf_hdr));
    sts = thrstr_write(ptr,sizeof(thr_intf_hdr));
    os_memset(ptr,0,len);
    sts = thrstr_read(ptr,len);
    if(sts == 0)
    {
        os_memcpy((uint8_t*)&hdr,ptr,sizeof(thr_intf_hdr));
        os_memcpy((uint8_t*)get_mde,ptr+sizeof(thr_intf_hdr),1);
    }
    else
    {
        sts = 1;
    }
    os_free(ptr);
    return sts;

}

/*!
 * @brief This API function is used to set thruster firing duration
 */
uint8_t thruster_simu_set_firing_duration(uint8_t thruster_id,uint32_t thr_st_firng_dur)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;

    hdr.thruster_id = thruster_id;
    hdr.length = 4;
    hdr.rsp_flag = 0;
    hdr.register_addr = THRUSTER_MODE_FIRING_REG;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy((uint8_t*)ptr,&hdr,sizeof(thr_intf_hdr));
    os_memcpy((uint8_t*)ptr+sizeof(thr_intf_hdr),&thr_st_firng_dur,4);
    sts = thrstr_write(ptr,len);
    os_free(ptr);
    return sts;

}

/*!
 * @brief This API function is used to get thruster firing duration
 */
uint8_t thruster_simu_get_firing_duration(uint8_t thruster_id,uint32_t* thr_gt_firng_dur)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 0;
    hdr.rsp_flag = 1;
    hdr.register_addr = THRUSTER_MODE_FIRING_REG;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy(ptr,&hdr,sizeof(thr_intf_hdr));
    sts = thrstr_write(ptr,sizeof(thr_intf_hdr));
    os_memset(ptr,0,len);
    sts = thrstr_read(ptr,len);
    if(sts == 0)
    {
        os_memcpy((uint8_t*)&hdr,ptr,sizeof(thr_intf_hdr));
        os_memcpy((uint8_t*)thr_gt_firng_dur,ptr+sizeof(thr_intf_hdr),4);
    }
    else
    {
        sts = 1;
    }
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to get thruster power voltage consmption
 */
uint8_t thruster_simu_get_input_power_voltage_consmp(uint8_t thruster_id,float* get_voltage)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 0;
    hdr.rsp_flag = 1;
    hdr.register_addr = THRUSTER_PWR_VOLTAGE_REG;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy(ptr,&hdr,sizeof(thr_intf_hdr));
    sts = thrstr_write(ptr,sizeof(thr_intf_hdr));
    os_memset(ptr,0,len);
    sts = thrstr_read(ptr,len);
    if(sts == 0)
    {
        os_memcpy((uint8_t*)&hdr,ptr,sizeof(thr_intf_hdr));
        os_memcpy((uint8_t*)get_voltage,ptr+sizeof(thr_intf_hdr),4);
    }
    else
    {
        sts = 1;
    }
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to get thruster power current consmption
 */
uint8_t thruster_simu_get_input_power_current_consmp(uint8_t thruster_id,float* get_current)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 0;
    hdr.rsp_flag = 1;
    hdr.register_addr = THRUSTER_PWR_CURRENT_REG;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy(ptr,&hdr,sizeof(thr_intf_hdr));
    sts = thrstr_write(ptr,sizeof(thr_intf_hdr));
    os_memset(ptr,0,len);
    sts = thrstr_read(ptr,len);
    if(sts == 0)
    {
        os_memcpy((uint8_t*)&hdr,ptr,sizeof(thr_intf_hdr));
        os_memcpy((uint8_t*)get_current,ptr+sizeof(thr_intf_hdr),4);
    }
    else
    {
        sts = 1;
    }
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to get thruster onboard temperature
 */
uint8_t thruster_simu_get_onbrd_temperature(uint8_t thruster_id,float* get_onbrd_temp)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 0;
    hdr.rsp_flag = 1;
    hdr.register_addr = THRUSTER_ONBRD_TEMP;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy(ptr,&hdr,sizeof(thr_intf_hdr));
    sts = thrstr_write(ptr,sizeof(thr_intf_hdr));
    os_memset(ptr,0,len);
    sts = thrstr_read(ptr,len);
    if(sts == 0)
    {
        os_memcpy((uint8_t*)&hdr,ptr,sizeof(thr_intf_hdr));
        os_memcpy((uint8_t*)get_onbrd_temp,ptr+sizeof(thr_intf_hdr),4);
    }
    else
    {
        sts = 1;
    }
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to set thruster radiator low temp threshold
 */
uint8_t thruster_simu_set_radiator_low_temp_threshold(uint8_t thruster_id,float thr_st_rad_lw_temp)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 4;
    hdr.rsp_flag = 0;
    hdr.register_addr = THRUSTER_RAD_LOW_TEMP;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy((uint8_t*)ptr,&hdr,sizeof(thr_intf_hdr));
    os_memcpy((uint8_t*)ptr+sizeof(thr_intf_hdr),&thr_st_rad_lw_temp,4);
    sts = thrstr_write(ptr,len);
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to set thruster radiator high temp threshold
 */
uint8_t thruster_simu_set_radiator_high_temp_threshold(uint8_t thruster_id,float thr_st_rad_hg_temp)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 4;
    hdr.rsp_flag = 0;
    hdr.register_addr = THRUSTER_RAD_HIGH_TEMP;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy((uint8_t*)ptr,&hdr,sizeof(thr_intf_hdr));
    os_memcpy((uint8_t*)ptr+sizeof(thr_intf_hdr),&thr_st_rad_hg_temp,4);
    sts = thrstr_write(ptr,len);
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to get thruster radiator low temp threshold
 */
uint8_t thruster_simu_get_radiator_low_temp_threshold(uint8_t thruster_id,float* thr_gt_rad_lw_temp)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 0;
    hdr.rsp_flag = 1;
    hdr.register_addr = THRUSTER_RAD_LOW_TEMP;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy(ptr,&hdr,sizeof(thr_intf_hdr));
    sts = thrstr_write(ptr,sizeof(thr_intf_hdr));
    os_memset(ptr,0,len);
    sts = thrstr_read(ptr,len);
    if(sts == 0)
    {
        os_memcpy((uint8_t*)&hdr,ptr,sizeof(thr_intf_hdr));
        os_memcpy((uint8_t*)thr_gt_rad_lw_temp,ptr+sizeof(thr_intf_hdr),4);
    }
    else
    {
        sts = 1;
    }
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to get thruster radiator high temp threshold
 */
uint8_t thruster_simu_get_radiator_high_temp_threshold(uint8_t thruster_id,float* thr_gt_rad_hg_temp)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 0;
    hdr.rsp_flag = 1;
    hdr.register_addr = THRUSTER_RAD_HIGH_TEMP;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy(ptr,&hdr,sizeof(thr_intf_hdr));
    sts = thrstr_write(ptr,sizeof(thr_intf_hdr));
    os_memset(ptr,0,len);
    sts = thrstr_read(ptr,len);
    if(sts == 0)
    {
        os_memcpy((uint8_t*)&hdr,ptr,sizeof(thr_intf_hdr));
        os_memcpy((uint8_t*)thr_gt_rad_hg_temp,ptr+sizeof(thr_intf_hdr),4);
    }
    else
    {
        sts = 1;
    }
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to get thruster remaining proplellant level
 */
uint8_t thruster_simu_get_remaining_propellant_level(uint8_t thruster_id,float* thr_gt_rmg_prplnt_lvl)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 0;
    hdr.rsp_flag = 1;
    hdr.register_addr = THRUSTER_REMNG_PROPLNT_LVL;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy(ptr,&hdr,sizeof(thr_intf_hdr));
    sts = thrstr_write(ptr,sizeof(thr_intf_hdr));
    os_memset(ptr,0,len);
    sts = thrstr_read(ptr,len);
    if(sts == 0)
    {
        os_memcpy((uint8_t*)&hdr,ptr,sizeof(thr_intf_hdr));
        os_memcpy((uint8_t*)thr_gt_rmg_prplnt_lvl,ptr+sizeof(thr_intf_hdr),4);
    }
    else
    {
        sts = 1;
    }
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to set thruster min tank pressure limit
 */
uint8_t thruster_simu_set_min_tank_pressure_limit(uint8_t thruster_id,float set_min_tnk_prs)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 4;
    hdr.rsp_flag = 0;
    hdr.register_addr = THRUSTER_MIN_TNK_PRS;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy((uint8_t*)ptr,&hdr,sizeof(thr_intf_hdr));
    os_memcpy((uint8_t*)ptr+sizeof(thr_intf_hdr),&set_min_tnk_prs,4);
    sts = thrstr_write(ptr,len);
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to set thruster max tank pressure limit
 */
uint8_t thruster_simu_set_max_tank_pressure_limit(uint8_t thruster_id,float set_max_tnk_prs)
{
 uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 4;
    hdr.rsp_flag = 0;
    hdr.register_addr = THRUSTER_MAX_TNK_PRS;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy((uint8_t*)ptr,&hdr,sizeof(thr_intf_hdr));
    os_memcpy((uint8_t*)ptr+sizeof(thr_intf_hdr),&set_max_tnk_prs,4);
    sts = thrstr_write(ptr,len);
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to get thruster min tank pressure limit
 */
uint8_t thruster_simu_get_min_tank_pressure_limit(uint8_t thruster_id,float* get_min_tnk_prs)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 0;
    hdr.rsp_flag = 1;
    hdr.register_addr = THRUSTER_MIN_TNK_PRS;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy(ptr,&hdr,sizeof(thr_intf_hdr));
    sts = thrstr_write(ptr,sizeof(thr_intf_hdr));
    os_memset(ptr,0,len);
    sts = thrstr_read(ptr,len);
    if(sts == 0)
    {
        os_memcpy((uint8_t*)&hdr,ptr,sizeof(get_min_tnk_prs));
        os_memcpy((uint8_t*)get_min_tnk_prs,ptr+sizeof(thr_intf_hdr),4);
    }
    else
    {
        sts = 1;
    }
    os_free(ptr);
    return sts;
}

/*!
 * @brief This API function is used to get thruster max tank pressure limit
 */
uint8_t thruster_simu_get_max_tank_pressure_limit(uint8_t thruster_id,float* get_max_tnk_prs)
{
    uint8_t sts = 1;
    thr_intf_hdr hdr;
    hdr.thruster_id = thruster_id;
    hdr.length = 0;
    hdr.rsp_flag = 1;
    hdr.register_addr = THRUSTER_MAX_TNK_PRS;
    uint16_t len = sizeof(thr_intf_hdr) + 4;
    uint8_t* ptr = os_malloc(len);
    os_memcpy(ptr,&hdr,sizeof(thr_intf_hdr));
    sts = thrstr_write(ptr,sizeof(thr_intf_hdr));
    os_memset(ptr,0,len);
    sts = thrstr_read(ptr,len);
    if(sts == 0)
    {
        os_memcpy((uint8_t*)&hdr,ptr,sizeof(get_max_tnk_prs));
        os_memcpy((uint8_t*)get_max_tnk_prs,ptr+sizeof(thr_intf_hdr),4);
    }
    else
    {
        sts = 1;
    }
    os_free(ptr);
    return sts;
}
