#include <stdio.h>
#include <stdlib.h>

#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>

#include "ant_host_init.h"
#include "ant_interface.h"
#include "ant_parameters.h"
#include "ant_error.h"

#include "ant_channel_config.h"
#include "ant_search_config.h"
#include "ant_state_indicator.h"
#include <ant_key_manager.h>

#define WILDCARD_TRANSMISSION_TYPE 0
#define WILDCARD_DEVICE_NUMBER 0
#define ANT_NETWORK_NUM 0
#define ANT_BS_CHANNEL_NUMBER 0


#define logs_i(...) LOG_INF(__VA_ARGS__)
#define NRF_LOG_INFO(...) LOG_INF(__VA_ARGS__)
#define logs_e(...) LOG_ERR(__VA_ARGS__)
#define logs_w(...) LOG_WRN(__VA_ARGS__)
#define logs_d(...) LOG_DBG(__VA_ARGS__)

LOG_MODULE_REGISTER(ant_bs_scan, LOG_LEVEL_INF);

#define ANT_BEACON_PAGE ((uint8_t)1)
#define NRF_BLE_SCAN_ADDRESS_CNT 16

void ant_message_send(void);
static void background_scanner_process(ant_evt_t *p_ant_evt);

static uint8_t m_last_rssi = 0;
static uint16_t m_last_device_id = 0;
static uint8_t m_received = 0;
static bool scan_going = false;
static bool search_sharing_cycles = 0;
K_SEM_DEFINE(bs_scan_closed_sem, 0, 1);

static uint8_t connect_if_match;
static uint8_t scan_if_match;
static uint8_t filter_addr_count;
// static ant_gap_addr_t filter_addr_array[16];

/**@brief Process ANT message on ANT background scanning channel.
 *
 * @param[in] p_ant_evt   ANT message content.
 */

 uint16_t uint16_decode(const uint8_t *p_encoded_data)
{
    return ((((uint16_t)((uint8_t *)p_encoded_data)[0])) |
            (((uint16_t)((uint8_t *)p_encoded_data)[1]) << 8));
}


static void background_scanner_process(ant_evt_t *p_ant_evt)
{
    LOG_INF("channel=%d event=%d", p_ant_evt->channel, p_ant_evt->event);
    // char test[256]={0};

    // for(int i=0;i<256;i++)
    // {
    //     test[i]=p_ant_evt->channel+i;
    // }

    if (p_ant_evt->channel == 13)
    {
        // LOG_INF("disp_evt_handler=%d channel=%d test=%d\r\n", 0, p_ant_evt->channel,test[255]);
        LOG_INF("disp_evt_handler=%d channel=%d\r\n", 0, p_ant_evt->channel);

    }
    // ant_gap_addr_t gap_addr = {0};
    if (p_ant_evt->channel != ANT_BS_CHANNEL_NUMBER)
    {
        return;
    }

    if (p_ant_evt->event == EVENT_CHANNEL_CLOSED)
    {
        k_sem_give(&bs_scan_closed_sem);
    }

    switch (p_ant_evt->event)
    {
    case EVENT_RX:
    {
        if (p_ant_evt->message.ANT_MESSAGE_stExtMesgBF.bANTDeviceID)
        {
            // assumes little endian
            m_last_device_id = (p_ant_evt->message.ANT_MESSAGE_aucExtData[1]) << 8 | (p_ant_evt->message.ANT_MESSAGE_aucExtData[0]);
        }

        if (p_ant_evt->message.ANT_MESSAGE_stExtMesgBF.bANTRssi)
        {
            m_last_rssi = p_ant_evt->message.ANT_MESSAGE_aucExtData[5];
        }

        // LOG_INF("=============================================================");
        // LOG_INF("Message number %d", m_received);
        // LOG_INF("Device ID:     %d", m_last_device_id);
        // LOG_INF("RSSI:          %d", m_last_rssi);
        // gap_addr.device_number = uint16_decode(p_ant_evt->message.ANT_MESSAGE_aucExtData);
        // gap_addr.device_type = p_ant_evt->message.ANT_MESSAGE_aucExtData[2];
        // gap_addr.trans_type = p_ant_evt->message.ANT_MESSAGE_aucExtData[3];
        int8_t rssi = p_ant_evt->message.ANT_MESSAGE_aucExtData[5];

        // // logs_d("rssi=%d device_number=%d device_type=0x%x trans_type=%d\r\n",
        // //        rssi, gap_addr.device_number, gap_addr.device_type, gap_addr.trans_type);
        LOG_INF("rssi=%d device_number=%d device_type=0x%x trans_type=%d\r\n",
            rssi,
             uint16_decode(p_ant_evt->message.ANT_MESSAGE_aucExtData),
              p_ant_evt->message.ANT_MESSAGE_aucExtData[2],
              p_ant_evt->message.ANT_MESSAGE_aucExtData[3]);

        m_received++;

#if 0
        ant_event_scan_result(gap_addr.device_number, gap_addr.device_type, gap_addr.trans_type, rssi);

        if (connect_if_match)
        {
            for (uint8_t i = 0; i < filter_addr_count; i++)
            {
                if (0 == memcmp(&filter_addr_array[i], &gap_addr, sizeof(ant_gap_addr_t)))
                {
                    logs_i("connect_if_match\r\n");
                    if (!scan_if_match)
                    {
                        bs_scan_close();
                    }

                    extern uint32_t ant_cmd_connect(uint16_t number, uint8_t device_type, uint8_t tran_type);
                    ant_cmd_connect(gap_addr.device_number, gap_addr.device_type, gap_addr.trans_type);
                    break;
                }
            }
        }
#endif
        break;
    }
    default:
    {
        break;
    }
    }
}

/**@brief Function for handling a ANT stack event.
 *
 * @param[in] p_ant_evt  ANT stack event.
 */
static void antplus_evt_handler(ant_evt_t *p_ant_evt)
{
    background_scanner_process(p_ant_evt);
    LOG_INF("this is a test log0\r\n");

   // k_msleep(10);  
    LOG_INF("this is a test log1\r\n");
    

}

#define LIGHT_DEVICE_TYPE 0x28u
#define LIGHT_ANTPLUS_RF_FREQ 0x39u /* 2457 MHz */
#define LIGHT_MSG_PERIOD 4084u

#define ANT_LIGHT_HEAD_CHANNEL_NUM 13

void light_rx_init(void)
{
  ant_err_t err;
  ant_channel_config_t cfg = {
      .channel_type = CHANNEL_TYPE_SLAVE,
      .ext_assign = 0,
      .rf_freq = LIGHT_ANTPLUS_RF_FREQ,
      .transmission_type = WILDCARD_TRANSMISSION_TYPE,
      .device_type = LIGHT_DEVICE_TYPE,
      .device_number = WILDCARD_DEVICE_NUMBER,
      .channel_period = LIGHT_MSG_PERIOD,
      .network_number = ANT_NETWORK_NUM,
  };

  
  cfg.channel_number = ANT_LIGHT_HEAD_CHANNEL_NUM;
  err = ant_channel_init(&cfg);
  LOG_INF("channel=%d err=%d\r\n", ANT_LIGHT_HEAD_CHANNEL_NUM, err);
  if (err)
  {
    LOG_ERR("ant_channel_init LIGHT_HEAD failed: %d", err);
  }
}


static int bs_channel_init(void)
{
    /* Set library config to report RSSI and Device ID */

    ant_err_t err_code = ant_lib_config_set(ANT_LIB_CONFIG_MESG_OUT_INC_RSSI | ANT_LIB_CONFIG_MESG_OUT_INC_DEVICE_ID);


    ant_channel_config_t bs_channel_config = {
        .channel_number = ANT_BS_CHANNEL_NUMBER,
        .channel_type = CHANNEL_TYPE_SLAVE,
        .ext_assign = EXT_PARAM_ALWAYS_SEARCH,
        .rf_freq = CONFIG_RF_FREQ,
        .transmission_type = WILDCARD_TRANSMISSION_TYPE,
        .device_type = CONFIG_WILDCARD_DEVICE_TYPE, // CHAN_ID_DEV_TYPE,
        .device_number = WILDCARD_DEVICE_NUMBER,    // Wild card
        .channel_period = 0,                        // This is not taken into account.
        .network_number = ANT_NETWORK_NUM,
    };
    NRF_LOG_INFO("channel_number=%d\r\n", bs_channel_config.channel_number);
    NRF_LOG_INFO("channel_type=%d\r\n", bs_channel_config.channel_type);
    NRF_LOG_INFO("ext_assign=%d\r\n", bs_channel_config.ext_assign);
    NRF_LOG_INFO("rf_freq=%d\r\n", bs_channel_config.rf_freq);
    NRF_LOG_INFO("transmission_type=%d\r\n", bs_channel_config.transmission_type);
    NRF_LOG_INFO("device_type=%d\r\n", bs_channel_config.device_type);
    NRF_LOG_INFO("device_number=%d\r\n", bs_channel_config.device_number);
    NRF_LOG_INFO("channel_period=%d\r\n", bs_channel_config.channel_period);
    NRF_LOG_INFO("network_number=%d\r\n", bs_channel_config.network_number);
    err_code = ant_channel_init(&bs_channel_config);
    if(err_code)
    {
        LOG_ERR("ant_channel_init failed: %d", err_code);
    }
    return 0x0;
}

extern uint8_t bs_scan_init(void);

int bsp_ant_init(void)
{
  int err;
  err = bs_scan_init();
  if (err)
  {
    LOG_ERR("ant_stack_setup failed: %d", err);
		return 1;
  }
  LOG_INF("ANT stack setup success\r\n");

//   ant_profiles_init();
  return 0;
}

/**@brief Function for ANT stack initialization.
 */
static ant_err_t ant_stack_setup(void)
{
    ant_err_t err_code;

    err_code = ant_init();
    if (err_code == 0)
    {
        LOG_INF("ANT Version %s", ANT_VERSION_STRING);
    }
    else
    {
        LOG_ERR("ant_init failed: 0x%X", err_code);
        return 0x1;
    }

    err_code = ant_cb_register(&antplus_evt_handler);
    if (err_code)
    {
        LOG_ERR("ant_cb_register() failed: 0x%X", err_code);
    }

    err_code = ant_plus_key_set(ANT_NETWORK_NUM);
    if (err_code)
    {
        LOG_ERR("ant_plus_key_set failed: %d", err_code);
    }


    
    err_code = bs_channel_init();
    if (err_code)
    {
        LOG_ERR("channel init fail\r\n");
    }

    light_rx_init();
    return 0x0;
}

static int utils_setup(void)
{
    int err = ant_state_indicator_init(ANT_BS_CHANNEL_NUMBER, CHANNEL_TYPE_SLAVE);
    if (err)
    {
        LOG_ERR("ant_state_indicator_init failed: %d", err);
        return err;
    }

    return err;
}

uint8_t bs_scan_init(void)
{
    ant_err_t err_code;

    err_code = ant_stack_setup();
    if (err_code)
    {
        LOG_ERR("ant_stack_setup() failed: 0x%X", err_code);
        return 0x1;
    }

    // err_code = utils_setup();
    // if (err_code)
    // {
    //     return 0x2;
    // }

    return 0;
}

const ant_search_config_t bs_search_config_default =
    {
        .channel_number = ANT_BS_CHANNEL_NUMBER,
        .low_priority_timeout = ANT_LOW_PRIORITY_TIMEOUT_DISABLE,
        .high_priority_timeout = ANT_HIGH_PRIORITY_SEARCH_DISABLE,
        .search_sharing_cycles = ANT_SEARCH_SHARING_CYCLES_DISABLE,
        .search_priority = ANT_SEARCH_PRIORITY_DEFAULT,
        .waveform = ANT_WAVEFORM_DEFAULT,
};

uint32_t light_rx_open_head(uint16_t device_number, uint8_t device_type, uint8_t tran_type)
{
  uint8_t channel_number = ANT_LIGHT_HEAD_CHANNEL_NUM;
  ant_err_t err;

  logs_i("light_rx_open ch=%u dev=%u type=%u tran=%u", channel_number, device_number, device_type, tran_type);

  err = ant_channel_id_set(channel_number, device_number, device_type, tran_type);
  if (err)
  {
    LOG_ERR("ant_channel_id_set LIGHT failed: %d", err);
    return (uint32_t)err;
  }
  err = ant_channel_open(channel_number);
  if (err)
  {
    LOG_ERR("ant_channel_open LIGHT failed: %d", err);
    return (uint32_t)err;
  }
  return 0;
}

uint32_t bs_scan_open(uint8_t *pdata, uint8_t len)
{
    if (scan_going)
    {
        logs_i("scan already going\r\n");
        return 1;
    }
    uint32_t err_code = 0;
    uint8_t channel_status = 0;
    err_code = ant_channel_status_get(ANT_BS_CHANNEL_NUMBER, &channel_status);
    if (err_code)
    {
        logs_e("ant_channel_status_get() failed: 0x%X,bs", err_code);
        return err_code;
    }

    if ((channel_status == STATUS_SEARCHING_CHANNEL) || (channel_status == STATUS_TRACKING_CHANNEL))
    {
        logs_e("in wrong state, channel_status=0x%x\r\n", channel_status);
        return NRF_ANT_ERROR_CHANNEL_IN_WRONG_STATE;
    }
    logs_i("%s len=%d channel_status=0x%x\r\n", __func__, len, channel_status);

#if 1
    ant_search_config_t bs_search_config = {
        .low_priority_timeout = ANT_LOW_PRIORITY_TIMEOUT_DISABLE,
        .high_priority_timeout = ANT_HIGH_PRIORITY_TIMEOUT_DISABLE,
        .search_priority = ANT_SEARCH_PRIORITY_HIGHEST,
        .waveform = ANT_WAVEFORM_DEFAULT,
    };

    search_sharing_cycles = bs_search_config.search_sharing_cycles;
    bs_search_config.channel_number = ANT_BS_CHANNEL_NUMBER;
    err_code = ant_search_init(&bs_search_config);
    if (err_code)
    {
        LOG_ERR("ant_search_init() failed: 0x%X", err_code);
        return err_code;
    }


    

    err_code = ant_channel_open(ANT_BS_CHANNEL_NUMBER);
    if (err_code)
    {
        LOG_ERR("ant_channel_open() failed: 0x%X,bs", err_code);
        return err_code;
    }

    scan_going = true;
    ant_state_indicator_channel_opened();

    return err_code;
#endif
}

uint8_t s_bs_scan_close_flag = 0;

uint32_t bs_scan_close(void)
{
    uint32_t err_code = 0;
    int sem_ret;

    if (!scan_going)
    {
        logs_w("scan not going\r\n");
        return 1;
    }

    uint8_t channel_status = 0;
    err_code = ant_channel_status_get(ANT_BS_CHANNEL_NUMBER, &channel_status);
    if (err_code)
    {
        logs_e("ant_channel_status_get() failed: 0x%X,bs", err_code);
        return err_code;
    }
    logs_i("channel_status=%d\r\n", channel_status);

    if ((channel_status == STATUS_ASSIGNED_CHANNEL) || (channel_status == STATUS_UNASSIGNED_CHANNEL))
    {
        scan_going = false;
        return 0;
    }

    k_sem_reset(&bs_scan_closed_sem);
    scan_going = false;
    s_bs_scan_close_flag = 1;
    err_code = ant_channel_close(ANT_BS_CHANNEL_NUMBER);
    if (err_code)
    {
        logs_e("ant_channel_close() failed: 0x%X,bs", err_code);
        return err_code;
    }

    sem_ret = k_sem_take(&bs_scan_closed_sem, K_SECONDS(5));
    if (sem_ret)
    {
        err_code = ant_channel_status_get(ANT_BS_CHANNEL_NUMBER, &channel_status);
        if (err_code)
        {
            logs_e("ant_channel_status_get() failed: 0x%X,bs", err_code);
            s_bs_scan_close_flag = 0;
            return err_code;
        }

        if (channel_status != STATUS_ASSIGNED_CHANNEL)
        {
            LOG_ERR("channel_close timeout, channel_status=0x%x\r\n", channel_status);
            s_bs_scan_close_flag = 0;
            return NRF_ANT_ERROR_CHANNEL_IN_WRONG_STATE;
        }
    }

    s_bs_scan_close_flag = 0;

    
    // if (search_sharing_cycles)
    // {
    //     err_code = ant_channel_close(ANT_SHIMANO_BS_CHANNEL_NUMBER);
    //     if (err_code)
    //     {
    //         logs_e("ant_channel_close() failed: 0x%X,", err_code);
    //     }
    // }
    return err_code;
}