#include <math.h>
#include "gps_app.h"
#include "oem7_gps.h"
#include "exo_tctm_ipc.h"
#include "exo_osal.h"
#include "exo_common.h"
#include "nvm.h"
#include "obc_icd.h"
#include "obc_intf_ctrl_msg.h"
#include "sdr_app_main.h"
#include "exo_io_al_sos_timer.h"

static orbit_tle_t* orbital_elements;
static os_timer_handle_ptr loc_ntfy_tmr;
static os_timer_handle_ptr tle_ntfy_tmr;
#ifdef GPS_TIME_SYNC
static os_timer_handle_ptr gps_time_sync_tmr;
#endif
static os_sem_handle_ptr gps_tle_sem = NULL;

void gps_init(void)
{
    uint16_t ntfy_prdcty;
    os_thread_create(GPS_TASK,T_GPS_TASK_STACK_SIZE, P_GPS_TASK, "gps_thread",gps_hdlr_thread , NULL, NULL, NULL);
    os_timer_create(&loc_ntfy_tmr,"gps_loc_ntfy_tmr",PERIODIC,(ral_timer_cbfunc_t)gps_loc_ntfy_tmr_cb,NULL);
    os_timer_create(&tle_ntfy_tmr,"tle_loc_ntfy_tmr",PERIODIC,(ral_timer_cbfunc_t)gps_tle_ntfy_tmr_cb,NULL);

#ifdef GPS_TIME_SYNC
    uint32_t time_prdcty;
    os_timer_create(&gps_time_sync_tmr,"gps_time_sync_tmr",PERIODIC,(ral_timer_cbfunc_t)gps_time_sync_tmr_cb,NULL);
    nvm_parameter_read(NVM_DATA,NVM_GPS_SYNC_TIME_PRDCTY,(uint8_t*)&time_prdcty);
    if(time_prdcty>GPS_MIN_NTFY_PRDCTY)
    {
        os_timer_start(gps_time_sync_tmr,time_prdcty*1000);
    }
#endif
    nvm_parameter_read(NVM_DATA,NVM_GPS_LOC_NTFY_PRDCTY,(uint8_t*)&ntfy_prdcty);
    if(ntfy_prdcty>GPS_MIN_NTFY_PRDCTY)
    {
        os_timer_start(loc_ntfy_tmr,ntfy_prdcty*1000);
    }
    nvm_parameter_read(NVM_DATA,NVM_GPS_TLE_NTFY_PRDCTY,(uint8_t*)&ntfy_prdcty);
    if(ntfy_prdcty>GPS_MIN_NTFY_PRDCTY)
    {
        os_timer_start(tle_ntfy_tmr,ntfy_prdcty*1000);
    }
}


void gps_time_sync_tmr_cb(void* args)
{
    os_itc_msg_handle_t gps_ipc;
    gps_ipc.src_entity = OS_TIMER;
    gps_ipc.Msg_id = GPS_TIME_SYNC_TMR_EXPRY;
    gps_ipc.pld.pld_ptr = NULL;
    os_itc_msg_send(&gps_ipc,GPS_TASK,0);
}

void gps_tle_ntfy_tmr_cb(void* args)
{
    os_itc_msg_handle_t gps_ipc;
    gps_ipc.src_entity = OS_TIMER;
    gps_ipc.Msg_id = GPS_TLE_NTF_TMR_EXPRY;
    gps_ipc.pld.pld_ptr = NULL;
    os_itc_msg_send(&gps_ipc,GPS_TASK,0);
}

void gps_loc_ntfy_tmr_cb(void* args)
{
    os_itc_msg_handle_t gps_ipc;
    gps_ipc.src_entity = OS_TIMER;
    gps_ipc.Msg_id = GPS_LOC_NTF_TMR_EXPRY;
    gps_ipc.pld.pld_ptr = NULL;
    os_itc_msg_send(&gps_ipc,GPS_TASK,0);
}

void gps_hdlr_thread(void* args)
{
    os_itc_msg_handle_t gps_ipc;
    gps_tle_data gps_tle;
    gps_loc_info loc_data;
    s_sdr_bcon_utc_gps* gps_sdr_bcn;
    os_itc_msg_handle_t gps_beacon_ipc;
#ifndef LINUX_TEMP_PORT
    oem7_gps_init();
#endif
    while(1)
    {
        os_itc_msg_rcv(GPS_TASK,&gps_ipc,os_wait_forever);
        switch(gps_ipc.Msg_id)
        {
        case GPS_LOC_NTF_TMR_EXPRY:
        {
            if(gps_get_location_info(&loc_data) == GPS_SCS)
            {
                obc_encode_icd_cmd(OBC_PS_GNSS_LOC_NTF,0,(uint8_t*)&loc_data);
            }
        }
        break;

        case GPS_TLE_NTF_TMR_EXPRY:
        {
            if(gps_get_cartesian_pos_velcty(&gps_tle) == GPS_SCS)
            {
                obc_encode_icd_cmd(OBC_PS_GNSS_TLE_DATA_NTF,0,(uint8_t*)&gps_tle);
            }
        }
        break;

        case SDR_APP_GPS_MH_REQ:
        {
            gps_sdr_bcn = os_malloc(sizeof(s_sdr_bcon_utc_gps));
            os_memset(gps_sdr_bcn,0,sizeof(s_sdr_bcon_utc_gps));
            os_memcpy(&gps_sdr_bcn->bcon_msg[0],"Hello, I am Janus2",sizeof("Hello, I am Janus2"));
            if(gps_get_location_info(&loc_data) == GPS_SCS)
            {
                gps_sdr_bcn->utc_gps_tim_sts = 1;
                gps_sdr_bcn->latitude = loc_data.latitude;
                gps_sdr_bcn->longitude = loc_data.longitude;
                gps_sdr_bcn->altitude = loc_data.altitude;
                gps_sdr_bcn->utc_tim = loc_data.time;
            }
            else
            {
                gps_sdr_bcn->utc_tim = (uint32_t)get_rtc_tm_epoch();
                gps_sdr_bcn->utc_gps_tim_sts = 0;
            }
            if(gps_get_cartesian_pos_velcty(&gps_tle)== GPS_SCS)
            {
                gps_sdr_bcn->x_pos = gps_tle.position_x;
                gps_sdr_bcn->y_pos = gps_tle.position_y;
                gps_sdr_bcn->z_pos = gps_tle.position_z;
                gps_sdr_bcn->x_vel = gps_tle.velocity_x;
                gps_sdr_bcn->y_vel = gps_tle.velocity_y;
                gps_sdr_bcn->z_vel = gps_tle.velocity_z;
            }

            gps_beacon_ipc.Msg_id = GPS_SDR_APP_MH_RSP;
            gps_beacon_ipc.Msg_len = sizeof(s_sdr_bcon_utc_gps);
            gps_beacon_ipc.pld.pld_ptr=gps_sdr_bcn;
            gps_beacon_ipc.src_entity=GPS_TASK;
            os_itc_msg_send(&gps_beacon_ipc,SDR_APP,1000);
        }
        break;
        case GPS_COMPUTE_TLE_RSP:
        {
            os_memcpy(&orbital_elements,gps_ipc.pld.pld_ptr,sizeof(orbit_tle_t));
            if(gps_tle_sem != NULL)
            {
                os_sem_give(gps_tle_sem);
            }
        }
        break;
        case GPS_TIME_SYNC_TMR_EXPRY:
        {
            uint8_t timer_flag = 1;
            gps_sync_time(NULL, timer_flag);
        }
        break;
        default:
        {
        }
        break;
        if(gps_ipc.pld.pld_ptr != NULL)
        {
            os_free(gps_ipc.pld.pld_ptr);
        }
        }
    }
}

/**
 * @brief This function NVM write the sync timer periodicity in
 * minutes
 */
void gps_set_sync_time_prdcty(uint32_t prdcty)
{
    if(prdcty>GPS_MIN_SYNC_TIME_PRDCTY)
    {
        nvm_parameter_write(NVM_DATA,NVM_GPS_SYNC_TIME_PRDCTY,\
                (uint8_t*)&prdcty);
#ifdef GPS_TIME_SYNC
        os_timer_start(gps_time_sync_tmr,prdcty*1000*60);
#endif
    }
}

/**
 * @brief This function read the sync timer periodicity in NVM
 */
uint32_t gps_get_sync_time_prdcty(void)
{
    uint32_t get_prdcty;
    nvm_parameter_read(NVM_DATA,NVM_GPS_SYNC_TIME_PRDCTY,\
            (uint8_t*)&get_prdcty);

    return get_prdcty;
}

void gps_set_loc_ntfy_prdcty(uint16_t prdcty)
{
    if(prdcty>GPS_MIN_NTFY_PRDCTY)
    {
        nvm_parameter_write(NVM_DATA,NVM_GPS_LOC_NTFY_PRDCTY,\
                (uint8_t*)&prdcty);
        os_timer_start(loc_ntfy_tmr,prdcty*1000);
    }
    else
    {
        os_timer_stop(loc_ntfy_tmr);
    }
}

void gps_set_tle_data_ntfy_prdcty(uint16_t prdcty)
{
    if(prdcty>GPS_MIN_NTFY_PRDCTY)
    {
        nvm_parameter_write(NVM_DATA,NVM_GPS_TLE_NTFY_PRDCTY,\
                (uint8_t*)&prdcty);
        os_timer_start(tle_ntfy_tmr,prdcty*1000);
    }
    else
    {
        os_timer_stop(tle_ntfy_tmr);
    }
}

time_t gps_get_time_epoch()
{
    time_t time = 0;
    uint8_t ret = GPS_FAIL;
    s_oem7_time* time_info = NULL;
    time_info = os_malloc_eram(sizeof(s_oem7_time));
    ret = oem7_get_time_info(time_info);

    if(time_info->utc_status == OEM7_UTC_TIME_VALID && ret == GPS_SCS)
    {
        struct tm timeinfo;
        timeinfo.tm_year = time_info->utc_year - 1900; // Years since 1900
        timeinfo.tm_mon = time_info->utc_month - 1;    // Month (0-11)
        timeinfo.tm_mday = time_info->utc_day;         // Day of the month (1-31)
        timeinfo.tm_hour = time_info->utc_hour;        // Hour (0-23)
        timeinfo.tm_min = time_info->utc_min;          // Minutes (0-59)
        timeinfo.tm_sec = (int)round((double)(time_info->utc_ms/1000));
        time = mktime(&timeinfo);  
    }
    os_free(time_info);
    return time;
}

uint8_t gps_get_time_date(gps_time_data* loc_data)
{
    uint8_t sts = OEM7_FAIL;
    if(oem7_get_gpzda(loc_data) == OEM7_SCS)
    {
        sts = OEM7_SCS;
    }
    return sts;
}


uint8_t gps_get_location_info(gps_loc_info* loc_data)
{
    uint8_t sts = OEM7_FAIL;
    gps_location_data2 loc_data2;
    gps_location_data1 loc_data1;
    gps_time_data time_data;
    if(oem7_get_gprmc(&loc_data2) == OEM7_SCS)
    {
        if(oem7_get_gpgga(&loc_data1) == OEM7_SCS)
        {
            loc_data->altitude = loc_data1.altitude;
            loc_data->latitude = loc_data2.latitude;
            loc_data->longitude = loc_data2.longitude;
            loc_data->speed = loc_data2.speed;
            loc_data->satellites_tracked = loc_data1.satellites_tracked;
            loc_data->valid = loc_data2.valid;
            loc_data->height = loc_data1.height;
            time_data.date = loc_data2.date;
            time_data.time = loc_data2.time;
            loc_data->time = convert_to_epoch_time(&time_data);
            sts=OEM7_SCS;
        }
    }
    return sts;
}

uint8_t gps_get_cartesian_pos_velcty(gps_tle_data* gps_tle)
{
    uint8_t sts = OEM7_FAIL;
    s_oem7_best_xyz best_xyz;
    time_t tm;
    tm = gps_get_time_epoch();
    if(oem7_get_bestxyz(&best_xyz) == OEM7_SCS)
    {
        if(best_xyz.p_sts == 0 && best_xyz.v_sts ==0)
        {
            gps_tle->position_x = best_xyz.p_x;
            gps_tle->position_y = best_xyz.p_y;
            gps_tle->position_z = best_xyz.p_z;
            gps_tle->velocity_x = best_xyz.v_x;
            gps_tle->velocity_y = best_xyz.v_y;
            gps_tle->velocity_z = best_xyz.v_z;
            gps_tle->time = tm;
            sts = OEM7_SCS;
        }
    }
    return sts;
}


uint8_t gps_compute_orbital_elements(orbit_tle_t* orbital_info)
{
    uint8_t ret = GPS_FAIL;
    gps_tle_data gps_tle;
    if(os_sem_create_bin(&gps_tle_sem, "GPS_TLE_SEM")== ral_success && sat_dev_sts[HW_PL_SERVER].hw_sts == 1)
    {
        if(gps_get_cartesian_pos_velcty(&gps_tle) == GPS_SCS)
        {
            obc_encode_icd_cmd(OBC_PS_GNSS_COMPUTE_TLE,0,(uint8_t*)&gps_tle);
            if((os_sem_take(gps_tle_sem,2000) == ral_success))
            {
                os_memcpy(orbital_info,&orbital_elements,sizeof(orbital_info));
                os_sem_delete(gps_tle_sem);
                gps_tle_sem = NULL;
                ret = GPS_SCS;
            }
            else
            {
                os_sem_delete(gps_tle_sem);
                gps_tle_sem = NULL;
                ret = GPS_FAIL;
            }
        }
        else
        {
            ret = GPS_FAIL;
        }
    }
    else
    {
        ret = GPS_FAIL;
    }
    return ret;
}



// Function to convert gps_time_data to epoch time
time_t convert_to_epoch_time(gps_time_data *gps_data)
{
    struct tm timeinfo;

    // Initialize the struct tm with values from gps_time_data
    timeinfo.tm_sec = gps_data->time.seconds;
    timeinfo.tm_min = gps_data->time.minutes;
    timeinfo.tm_hour = gps_data->time.hours;
    timeinfo.tm_mday = gps_data->date.day;
    timeinfo.tm_mon = gps_data->date.month - 1; // Month is 0-based in struct tm

    if(gps_data->date.year<99)
    {
        timeinfo.tm_year = gps_data->date.year - 1900 + 2000; // Year is years since 1900 in struct tm
    }
    // Set microseconds (not standard tm field, so we ignore it for simplicity)

    // Use mktime to convert struct tm to epoch time
    return mktime(&timeinfo);
}

/**
 * @brief This function sync the gps time to RTC time
 */
uint8_t gps_sync_time(uint8_t* recv_epoch, uint8_t timer_flag)
{
    //time sync success, sync fail
    uint8_t ret = OEM7_FAIL;
    e_oem7_utc_time_sync status = SYNC_FAIL;
    int diff_time = 0;
    uint64_t utc2epoch = 0;
    uint64_t crnt_rtc_epoch = 0;
    uint64_t round_s = 0;
    s_oem7_time* time_info = NULL;
    time_info = os_malloc_eram(sizeof(s_oem7_time));

    ret = oem7_get_time_info(time_info);
    if(ret == OEM7_SCS)
    {
        //check if UTC Time status valid or not
        if(time_info->utc_status == OEM7_UTC_TIME_VALID)
        {
            struct tm timeinfo;
            timeinfo.tm_year = time_info->utc_year - 1900; // Years since 1900
            timeinfo.tm_mon = time_info->utc_month - 1;    // Month (0-11)
            timeinfo.tm_mday = time_info->utc_day;         // Day of the month (1-31)
            timeinfo.tm_hour = time_info->utc_hour;        // Hour (0-23)
            timeinfo.tm_min = time_info->utc_min;          // Minutes (0-59)

            round_s = round((double)(time_info->utc_ms/1000));
            timeinfo.tm_sec = (int)round_s; //Microseconds to seconds

            utc2epoch = mktime(&timeinfo);  
            //Get current rtc time in epoch
            crnt_rtc_epoch = get_crnt_time_date_in_epoch();
            //compare current epoch with received GPS UTC time
            diff_time = utc2epoch - crnt_rtc_epoch;
            diff_time = abs(diff_time);
            //check if periodical timer call(0) or force to update(1)
            if(timer_flag == 0)
            {
                if(diff_time < GPS_SYNC_TIME_DIFF)
                {
                    //RTC time and date is proper so,no need to update
                }
                else
                {
                    //Update and set the received GPS UTC time
                    set_crnt_time_date(utc2epoch);
                }
            }
            else
            {
                //Update and set the received GPS UTC time
                set_crnt_time_date(utc2epoch);
                os_memcpy(recv_epoch, &utc2epoch, sizeof(utc2epoch));
                //Time sync success
                status = SYNC_SUCCESS;
            }
        }
        else
        {
            //No action
        }
    }
    else
    {
        //No ACtion
    }
    os_free(time_info);
    return status;
}
