#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/drivers/gpio.h>
#include <nrf_modem_gnss.h>
#include <modem/nrf_modem_lib.h>
#include <nrf_modem_at.h>
#include "AT_CMD/at_cmd.h"
#include <modem/lte_lc.h>
#include "modem/modem.h"

#include "gps.h"

#if IS_ENABLED(CONFIG_LTE_LINK_CONTROL)
#endif

/* Get gps_en node from devicetree */
#define GPS_EN_NODE DT_NODELABEL(gps_enable)

/* GPIO specification for GPS enable */
static const struct gpio_dt_spec gps_en = GPIO_DT_SPEC_GET(GPS_EN_NODE, gpios);

LOG_MODULE_REGISTER(gnss_app, LOG_LEVEL_INF);

static struct nrf_modem_gnss_pvt_data_frame pvt_data;

#define ENABLE_GNSS_RF_ROUTING 0

static int sv_in_view(const struct nrf_modem_gnss_pvt_data_frame *p)
{
    int count = 0;
    for (int i = 0; i < NRF_MODEM_GNSS_MAX_SATELLITES; i++)
    {
        if (p->sv[i].sv > 0)
        {
            count++;
        }
    }
    return count;
}

static int sv_used_in_fix(const struct nrf_modem_gnss_pvt_data_frame *p)
{
    int count = 0;
    for (int i = 0; i < NRF_MODEM_GNSS_MAX_SATELLITES; i++)
    {
        if (p->sv[i].flags & NRF_MODEM_GNSS_SV_FLAG_USED_IN_FIX)
        {
            count++;
        }
    }
    return count;
}

static void gnss_event_handler(int event)
{
    int err;

    static bool first_log = true;

    switch (event)
    {
    case NRF_MODEM_GNSS_EVT_PVT:
        err = nrf_modem_gnss_read(&pvt_data, sizeof(pvt_data), NRF_MODEM_GNSS_DATA_PVT);
        if (err)
        {
            LOG_ERR("Failed to read PVT data, err=%d", err);
            return;
        }

        if (first_log)
        {
            first_log = false;
            LOG_INF("GNSS PVT struct size=%u", (unsigned)sizeof(pvt_data));
        }
        LOG_INF("PVT flags=0x%08x", pvt_data.flags);

        for(int i = 0; i < NRF_MODEM_GNSS_MAX_SATELLITES; i++)
        {
            if(pvt_data.sv[i].sv >0)
            {

                 int used_in_fix = (pvt_data.sv[i].flags & NRF_MODEM_GNSS_SV_FLAG_USED_IN_FIX) ? 1 : 0;


                 
                 int      sv_id       = (int)pvt_data.sv[i].sv;
                 double   cn0         = (double)pvt_data.sv[i].cn0;
                 uint8_t  used_in_fix1 = (pvt_data.sv[i].flags & NRF_MODEM_GNSS_SV_FLAG_USED_IN_FIX) ? 1 : 0;
                 uint16_t flags       = (uint16_t)pvt_data.sv[i].flags;

                 LOG_INF("SV %d C/N0=%.1f used=%d flags=0x%04x",
                 (int)pvt_data.sv[i].sv,
                 (double)pvt_data.sv[i].cn0,
                 used_in_fix,
                 (unsigned)pvt_data.sv[i].flags);

            }
        
        }

        /* Valid fix if 'flags' contains FIX_VALID */
        if (pvt_data.flags & NRF_MODEM_GNSS_PVT_FLAG_FIX_VALID)
        {
            LOG_INF(" FIX: lat=%.6f lon=%.6f alt=%.1f(m) acc=%.1f(m) sv_used=%d sv_view=%d",
                    pvt_data.latitude,
                    pvt_data.longitude,
                    pvt_data.altitude,
                    pvt_data.accuracy,
                    sv_used_in_fix(&pvt_data),
                    sv_in_view(&pvt_data));

            LOG_INF("Speed=%.2f(m/s) Heading=%.2f(deg) HDOP=%.2f",
                    pvt_data.speed,
                    pvt_data.heading,
                    pvt_data.hdop);

            /* Stop after first valid fix (change this behavior as needed) */
            // gps_stop();
            // LOG_INF("GNSS stopped after first fix.");
        }
        else
        {
            LOG_INF("PVT: searching... sv_view=%d acc=%.1f(m) flags=0x%08x",
                    sv_in_view(&pvt_data), pvt_data.accuracy, pvt_data.flags);
        }
        break;

    case NRF_MODEM_GNSS_EVT_FIX:
        /* FIX event indicates a fix was achieved (PVT will also carry details) */
        LOG_INF("NRF_MODEM_GNSS_EVT_FIX");
        break;

    case NRF_MODEM_GNSS_EVT_NMEA:
        /* Optional: read NMEA sentences if enabled */
        {
            char nmea[256];
            err = nrf_modem_gnss_read(nmea, sizeof(nmea), NRF_MODEM_GNSS_DATA_NMEA);
            if (err > 0)
            {
                /* 'err' is number of bytes read */
                nmea[MIN(err, (int)sizeof(nmea) - 1)] = '\0';
                LOG_INF("NMEA: %s", nmea);
            }
        }
        break;

        /* A-GNSS request handling (only if headers provide these enums) */
#if defined(NRF_MODEM_GNSS_EVT_AGPS_REQ) && defined(NRF_MODEM_GNSS_DATA_AGPS_REQ)
    case NRF_MODEM_GNSS_EVT_AGPS_REQ:
    {
        struct nrf_modem_gnss_agps_data_request req = (struct nrf_modem_gnss_agps_data_request){0};
        err = nrf_modem_gnss_read(&req, sizeof(req), NRF_MODEM_GNSS_DATA_AGPS_REQ);
        if (err)
        {
            LOG_WRN("A-GNSS req read failed: %d", err);
        }
        else
        {
            LOG_INF("A-GNSS requested: utc=%u almanac=%u eph=%u klob=%u neq=%u time=%u pos=%u iono=%u",
                    req.utc, req.almanac, req.ephemeris, req.klobuchar,
                    req.nequick, req.system_time, req.position, req.iono);
            LOG_INF("Consider fetching assistance (nRF Cloud/SUPL) and injecting.");
        }
        break;
    }
#endif

    default:
        LOG_INF("GNSS event: %d", event);
        break;
    }
}

int gnss_setup_and_start(void)
{
    int err;

    LOG_INF("Entered  gnss_setup_and_start starting function");
    /* Ensure GPS_EN GPIO device is ready */
    #if 0
    if (!device_is_ready(gps_en.port))
    {
        LOG_ERR("GPS_EN GPIO not ready");
        return -ENODEV;
    }

    /* Configure GPS_EN pin as output and turn GPS ON */
    err = gpio_pin_configure_dt(&gps_en, GPIO_OUTPUT_ACTIVE);
    if (err)
    {
        LOG_ERR("Failed to configure GPS_EN");
        return err;
    }

    err = gpio_pin_set_dt(&gps_en, 1);
    if (err)
    {
        LOG_ERR("Failed to set GPS_EN");
        return err;
    }

    #endif

    LOG_INF("GPS powered ON");

    /* LNA/antenna warm-up */
    k_msleep(50);

    /* Ensure modem is switched to GNSS-only mode before GNSS APIs */
    {
        char rsp[256] = {0};
        int at_err;

        /* Query current system mode to avoid unnecessary writes */
        at_err = nrf_modem_at_cmd(rsp, sizeof(rsp), "%s", "AT%XSYSTEMMODE?\r\n");
        if (at_err)
        {
            int type = nrf_modem_at_err_type(at_err);
            int code = nrf_modem_at_err(at_err);
            LOG_WRN("XSYSTEMMODE? failed (type=%d code=%d), proceeding to set", type, code);
        }


            memset(rsp, 0, sizeof(rsp));
            at_err = nrf_modem_at_cmd(rsp, sizeof(rsp), "%s", "AT%XCOEX0=1,1,1565,1586");
            if (at_err)
            {
                LOG_WRN("XCOEX0 set failed: type=%d code=%d ret=%d",
                        nrf_modem_at_err_type(at_err), nrf_modem_at_err(at_err), at_err);
            }
            else
            {
                LOG_INF("XCOEX0 configured for RF coexistence");
            }


        /* Go to minimum or offline functionality before changing system mode */
        at_err = nrf_modem_at_cmd(rsp, sizeof(rsp), "%s", AT_CFUN_OFF_RADIO_CMD);
        if (at_err)
        {
            int type = nrf_modem_at_err_type(at_err);
            int code = nrf_modem_at_err(at_err);
            LOG_WRN("CFUN=0 failed: type=%d code=%d ret=%d, trying CFUN=4", type, code, at_err);
            memset(rsp, 0, sizeof(rsp));
            /* Try offline mode */
            at_err = nrf_modem_at_cmd(rsp, sizeof(rsp), "%s", AT_CFUN_OFFLINE_CMD);
            if (at_err)
            {
                type = nrf_modem_at_err_type(at_err);
                code = nrf_modem_at_err(at_err);
                LOG_ERR("CFUN=4 failed: type=%d code=%d ret=%d", type, code, at_err);
                return at_err;
            }
        }

        /* Small delay to let modem settle */
        k_msleep(200);

        /* Set GNSS-only system mode */
        memset(rsp, 0, sizeof(rsp));
        at_err = nrf_modem_at_cmd(rsp, sizeof(rsp), "%s", AT_COMMAND_SYS_GNSS_ONLY_MODE);
        if (at_err)
        {
            int type = nrf_modem_at_err_type(at_err);
            int code = nrf_modem_at_err(at_err);
            LOG_ERR("XSYSTEMMODE GNSS-only failed: type=%d code=%d ret=%d", type, code, at_err);
            return at_err;
        }

        /* Enter GNSS-only functional mode */
        memset(rsp, 0, sizeof(rsp));
        at_err = nrf_modem_at_cmd(rsp, sizeof(rsp), "%s", AT_CFUN_GNSS_ONLY_CMD);
        if (at_err)
        {
            int type = nrf_modem_at_err_type(at_err);
            int code = nrf_modem_at_err(at_err);
            LOG_ERR("CFUN GNSS-only failed: type=%d code=%d ret=%d", type, code, at_err);
            return at_err;
        }
        LOG_INF("Modem set to GNSS-only mode");

        /* GNSS RF routing and coexistence */
        if (ENABLE_GNSS_RF_ROUTING)
        {
            memset(rsp, 0, sizeof(rsp));
            at_err = nrf_modem_at_cmd(rsp, sizeof(rsp), "%s", "AT%XMAGPIO=1,1,1,7,0,0");
            if (at_err)
            {
                LOG_WRN("XMAGPIO set failed: type=%d code=%d ret=%d",
                        nrf_modem_at_err_type(at_err), nrf_modem_at_err(at_err), at_err);
            }
            else
            {
                LOG_INF("XMAGPIO configured for GNSS antenna/LNA");
            }
            memset(rsp, 0, sizeof(rsp));
            at_err = nrf_modem_at_cmd(rsp, sizeof(rsp), "%s", "AT%XCOEX0=1,1,0");
            if (at_err)
            {
                LOG_WRN("XCOEX0 set failed: type=%d code=%d ret=%d",
                        nrf_modem_at_err_type(at_err), nrf_modem_at_err(at_err), at_err);
            }
            else
            {
                LOG_INF("XCOEX0 configured for RF coexistence");
            }
        }
        else
        {
            LOG_INF("Skipping XMAGPIO/XCOEX0");
        }
    }
    // uint32_t mask = NRF_MODEM_GNSS_DELETE_EPHEMERIDES |
    //                 NRF_MODEM_GNSS_DELETE_ALMANACS |
    //                 NRF_MODEM_GNSS_DELETE_IONO_CORRECTION_DATA |
    //                 NRF_MODEM_GNSS_DELETE_UTC_DATA |
    //                 NRF_MODEM_GNSS_DELETE_LAST_GOOD_FIX |
    //                 NRF_MODEM_GNSS_DELETE_GPS_TOW;

    // //delets the non-volatile data in the modem to ensure a cold start (clear ephemeris/almanac/assist data for testing)                
    // err = nrf_modem_gnss_nv_data_delete(mask);
    // if (err)
    // {
    //     LOG_WRN("GNSS NV data delete failed: %d", err);
    //     /* Not fatal—continue */
    // }
    // else
    // {
    //     LOG_INF("GNSS NV data cleared (cold start).");
    // }

    /* Register GNSS event handler */
    err = nrf_modem_gnss_event_handler_set(gnss_event_handler);
    if (err)
    {
        LOG_ERR("Failed to set GNSS event handler, err=%d", err);
        return err;
    }

    /* Configure fix interval / retry:
     * - fix_interval: time between starts of consecutive fix attempts (seconds)
     *   0 = continuous tracking
     * - fix_retry: how long GNSS tries to get a fix before sleeping (seconds)
     *   0 = try forever
     */
    err = nrf_modem_gnss_fix_interval_set(0); /* 0 = continuous tracking */
    if (err)
    {
        LOG_ERR("fix_interval_set failed: %d", err);
        return err;
    }

    err = nrf_modem_gnss_fix_retry_set(0); /* 0 = try forever for cold start */
    if (err)
    {
        LOG_ERR("fix_retry_set failed: %d", err);
        return err;
    }

    /* Enable NMEA sentences for visibility (GGA/RMC + satellites GSV/GSA) */
    {
        uint16_t nmea_mask = NRF_MODEM_GNSS_NMEA_GGA_MASK |
                             NRF_MODEM_GNSS_NMEA_RMC_MASK |
                             NRF_MODEM_GNSS_NMEA_GSV_MASK |
                             NRF_MODEM_GNSS_NMEA_GSA_MASK;
        err = nrf_modem_gnss_nmea_mask_set(nmea_mask);
        if (err)
        {
            LOG_WRN("nmea_mask_set failed: %d", err);
        }

        /* Optional GNSS performance tuning */
        (void)nrf_modem_gnss_use_case_set(NRF_MODEM_GNSS_USE_CASE_MULTIPLE_HOT_START |
                            NRF_MODEM_GNSS_USE_CASE_LOW_ACCURACY);

        // Increase warm‑up time if using an external active antenna or LNA, as they may require more time to stabilize and provide optimal performance.
        k_msleep(50);

        /* Start GNSS */
        err = nrf_modem_gnss_start();
        if (err)
        {
            LOG_ERR("Failed to start GNSS, err=%d", err);
            return err;
        }

        LOG_INF("GNSS started.");

        return 0;
    }
}

/* Stop GPS */
void gps_stop(void)
{
    LOG_INF("Stopping GPS");

    nrf_modem_gnss_stop();
}