#include "dw_mbx_tag.h"
#include "dps310.h"
#define DELAY_BETWEEN_TWO_FRAME_UUS 400
#define POLL_RX_TO_RESP_TX_DLY_UUS 470
#define UUS_TO_DWT_TIME 65536

static uint64_t get_rx_timestamp_u64(void)
{
    uint8_t ts_tab[5];
    uint64_t ts = 0;
    int i;
    dwt_readrxtimestamp(ts_tab);
    for (i = 4; i >= 0; i--)
    {
        ts <<= 8;
        ts |= ts_tab[i];
    }
    return ts;
}
static void resp_msg_set_ts(uint8_t *ts_field, const uint64_t ts)
{
    int i;
    for (i = 0; i < 4; i++)
    {
        ts_field[i] = (ts >> (i * 8)) & 0xFF;
    }
}
static uint16_t current_time,start_time,end_time;
static u8 frame_len,timeout,rec_nearbase_num,result;
static u8 rx_buffer[200],send_buffer[200];
static u16 anc_id_recv,tag_id_recv;
static uint64_t poll_rx_ts;
static uint32_t status_reg,resp_tx_time;
static uint64_t resp_tx_ts;

#define FZ_NEARBASENUM_INDEX    10
#define FZ_NEARBASEID_INDEX		 11
#define TAG_NUM_IN_SYS				25

#define TAG_KEEPTIMES           30
#define REPORT_TAG_KEEPTIMES     5
static uint16_t ancidlist_rec[TAG_NUM_IN_SYS],ancidlist_send[TAG_NUM_IN_SYS],rec_ancidlist[TAG_NUM_IN_SYS],rec_ancdistlist[TAG_NUM_IN_SYS];
static u8 frame_seq_nb2,battary,button,uwb_losttimer,taglist_pos;
uint8_t rec_num[3];
static u8 tagofflinetime[ANC_MAX_NUM];
u8 taglist_num;
uint16_t tagid_list[ANC_MAX_NUM],tagdist_list[ANC_MAX_NUM];
uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos) //0 mainbase  1 first near_base
{
    uint8_t motorstate;
    memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,2);

//			memcpy(&send_buffer[ANCTIMEUS],&tmp_time,2);
    send_buffer[TAGFREQ_IDX] = tag_frequency;
    memcpy(&send_buffer[TAG_ID_IDX],&tag_id_recv,2);
    poll_rx_ts = get_rx_timestamp_u64();//»ñµÃPoll°ü½ÓÊÕʱ¼äT2
    resp_tx_time = (poll_rx_ts + ((rec_nearbase_num*20+POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//¼ÆËãResponse·¢ËÍʱ¼äT3¡£

    dwt_setdelayedtrxtime(resp_tx_time);//ÉèÖÃResponse·¢ËÍʱ¼äT3
    //dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS+(rec_nearbase_num+1-ancrec_nearbasepos)*DELAY_BETWEEN_TWO_FRAME_UUS);//ÉèÖ÷¢ËÍÍê³Éºó¿ªÆô½ÓÊÕÑÓ³Ùʱ¼ä
    //dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//½ÓÊÕ³¬Ê±Ê±¼ä
    resp_tx_ts = (((uint64_t)(resp_tx_time & 0xFFFFFFFEUL)) << 8) ;

    /* Write all timestamps in the final message. See NOTE 8 below. */
    resp_msg_set_ts(&send_buffer[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts);
    resp_msg_set_ts(&send_buffer[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts);
    memcpy(&send_buffer[RESP_MSG_ANC_DISTOFFSET],&g_com_map[DIST_OFFSET],2);


    send_buffer[GROUP_ID_IDX] = group_id;
//			send_buffer[MAINBASE_INDEX]=flag_syncbase;
    send_buffer[MESSAGE_TYPE_IDX]=MBX_RESPONSE;
    send_buffer[MOTORSTATE_INDEX]=motorstate;//(remotesend_state<<4)|motorstate;
    send_buffer[MOTORSTATE_INDEX]&=0x0f;



    dwt_writetxdata(38, send_buffer, 0);//дÈë·¢ËÍÊý¾Ý
    dwt_writetxfctrl(38, 0);//É趨·¢Ëͳ¤¶È
    result = dwt_starttx(DWT_START_TX_DELAYED);
    if(result==0)
    {
        rec_num[2]++;
        start_time=HAL_LPTIM_ReadCounter(&hlptim1);
        timeout=100; //µ¥Î»0.1ms
        end_time=start_time+(timeout<<2);
        if(end_time>=32768)
        {
            end_time-=32768;
        }
        current_time=HAL_LPTIM_ReadCounter(&hlptim1);
        while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & SYS_STATUS_TXFRS ))///²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
        {
            current_time=HAL_LPTIM_ReadCounter(&hlptim1);
            if(current_time>=end_time&&current_time<end_time+15000)
                break;
        };
    }

    tagofflinetime[taglist_pos] = 0;
//            printf("²â¾à³É¹¦£¬ÐòºÅ%d£¬¾àÀë%d\r\n",frame_seq_nb2,tagdist_list[taglist_pos]);
//            delay_ms(10);
}
uint8_t report_ancnum;
uint16_t report_ancdist[ANC_MAX_NUM],report_ancid[ANC_MAX_NUM];
void TagListUpdate(void)
{
    uint16_t i,j=0,k=0,temp[TAG_NUM_IN_SYS];
    for(i=0; i<taglist_num; i++)
    {
        if(tagofflinetime[i]++<REPORT_TAG_KEEPTIMES)
        {
            report_ancid[k]=tagid_list[i];
            report_ancdist[k++]=tagdist_list[i];
        }
        if(tagofflinetime[i]++<TAG_KEEPTIMES)
        {
            tagid_list[j]=tagid_list[i];
            tagdist_list[j] = tagdist_list[i];
            tagofflinetime[j++]=tagofflinetime[i];
        }
    }
    report_ancnum = k;
    taglist_num=j;
}
uint16_t CmpTagInList(uint16_t tagid)
{   uint16_t i;
    for(i=0; i<taglist_num; i++)
    {
        if(memcmp(&tagid,&tagid_list[i],2)==0)
            break;
    }
    return i;
}
uint32_t temp231;

void MbxTagUwbRec(void)
{

    u8 send_len,i;
    u16 tempid;
    uint32_t rec_syncid,status_reg;
    uint16_t checksum;
    uint8_t kk;

    dwt_setrxtimeout(3000);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
    dwt_rxenable(0);//´ò¿ª½ÓÊÕ
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_RESET);
    temp231 =  dwt_read32bitreg(CHAN_CTRL_ID) ;
    start_time=HAL_LPTIM_ReadCounter(&hlptim1);
    timeout=100; //µ¥Î»0.1ms
    end_time=start_time+(timeout<<2);
    if(end_time>=32768)
    {
        end_time-=32768;
    }
    rec_num[0]++;
    current_time=HAL_LPTIM_ReadCounter(&hlptim1);

    while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
    {
        current_time=HAL_LPTIM_ReadCounter(&hlptim1);
        if(current_time>=end_time&&current_time<end_time+15000)
            break;
    };

    if (status_reg & SYS_STATUS_RXFCG && status_reg!=0xffffffff)//³É¹¦½ÓÊÕ
    {
        uint16_t tag_recv_interval;
        float temp_tagpos;

        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
        frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È
        rec_num[1]++;
        dwt_readrxdata(rx_buffer, frame_len, 0);//¶ÁÈ¡½ÓÊÕÊý¾Ý
        memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
        //½«ÊÕµ½µÄtag_id·Ö±ðдÈë¸÷´ÎͨѶµÄ°üÖУ¬Îª¶à±êǩͨѶ·þÎñ£¬·ÀÖ¹Ò»´ÎͨѶÖнÓÊÕµ½²»Í¬ID±êÇ©µÄÊý¾Ý
        memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
        switch(rx_buffer[MESSAGE_TYPE_IDX])
        {   uint8_t target_tagi;
        case MBX_REG:
            break;
        case MBX_POLL:
            if(group_id==rx_buffer[GROUP_ID_IDX])
            {
//                dwt_readdiagnostics(&d1);
//                LOS(&d1);
//                if(firstpath_power<-100)
//                {
//                    break;
//                }
                frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
                battary = rx_buffer[BATTARY_IDX];
                button = rx_buffer[BUTTON_IDX];
                rec_nearbase_num=rx_buffer[FZ_NEARBASENUM_INDEX];
                memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
                memcpy(&rec_ancidlist,&rx_buffer[FZ_NEARBASEID_INDEX],2*rec_nearbase_num);
                memcpy(&rec_ancdistlist,&rx_buffer[FZ_NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num);
                // memcpy(&rec_antdelay,&rx_buffer[FZ_NEARBASEID_INDEX+rec_nearbase_num*4],2);
                uwb_losttimer = 0;

                taglist_pos=CmpTagInList(tag_id_recv);
                if(taglist_num>=ANC_MAX_NUM)
                    break;

                for(i=0; i<rec_nearbase_num; i++)
                {
                    memcpy(&tempid,&rx_buffer[FZ_NEARBASEID_INDEX+i*2],2);
                    if(tempid==dev_id)
                    {
                        Anchor_RecNearPoll(i);
                        tagdist_list[taglist_pos]=rec_ancdistlist[i];
                        if(taglist_pos==taglist_num)// taglist_pos==taglist_num ˵Ã÷Õâ¸ö»ùÕ¾²»ÔÚµ±Ç°ÁбíÖÐ
                        {   //tempid==dev_id ˵Ã÷»ùվϷ¢²â¾à±¨ÎÄÓÐÕâ¸ö±êÇ©µÄÐÅÏ¢
                            taglist_num++;          //Âú×ãÉÏÊöÁ½ÖÖÇé¿ö²Å»áÌí¼Ó»ùÕ¾ID½øÈëÁбíÖУ¬·ñÔò»á³öÏÖ±êÇ©²»ÔÚ»ùÕ¾ÁбíÖУ¬±êÇ©Ò²²»ÏìÓ¦µÄÇé¿ö
                            tagid_list[taglist_pos] = tag_id_recv;
                        }
                        break;
                    }
                }
                if(i==rec_nearbase_num&&taglist_pos==taglist_num)//µ±»ùÕ¾²»ÔÚ±êÇ©ÁбíÖУ¬±êÇ©Ò²²»ÔÚ»ùÕ¾ÁбíÖÐʱ£¬Ëæ»úÕÒ¸öʱ¼äƬ»Ø¸´»ùÕ¾¡£
                {
                    Anchor_RecNearPoll(rec_nearbase_num+start_time%5);
                    //   Anchor_RecNearPoll(rec_nearbase_num);
                }
            }

            break;
        }

    } else {
//        printf("debug4 timec:%d timee:%d\r\n",current_time,end_time);
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
        //  dwt_rxenable(0);
    }

    dwt_forcetrxoff();
    dwt_entersleep();

}
wg_state_enum wg_state = WG_Lost;
uint8_t lora_sendbuffer[200];
u8 seq_num;
extern u8 wg_lost_count;
uint16_t wg_report_freq,wg_report_id;
#define WG_LOST_SWITCH_THRES   5
#define WG_LOST_NOUWB_COUNT    60
#define DEFAULT_WG_ID       0xFFFF
u8 lora_jianting_flag = 1;
void LoraReportFreqPoll(void)
{
    if(wg_lost_count++>WG_LOST_NOUWB_COUNT) //Íø¹Ø60ÃëûÓÐͨѶ£¬¾Í²»¿ªÆôUWB²â¾àlora¼àÌý
    {
        lora_jianting_flag = 0;
    } else {
        lora_jianting_flag = 1;
    }
    if(wg_state==WG_Lost)
    {
        wg_report_id = DEFAULT_WG_ID;
        wg_report_freq = REPORT_MANGE_CHANNEL_FRQ; //Èç¹û¶ªÊ§Á´½Ó¾Í½øÈëWG¹ÜÀíÐŵÀ¡£
    } else {
        if(wg_lost_count>WG_LOST_SWITCH_THRES)
        {
            wg_state = WG_Lost;
        }
    }
}
static u16 checksum;
u8 gps_state,gps_chafenlingqi,gps_satel_num,gps_signalpower;
double gps_jingdu,gps_weidu;
float gps_height;
extern u8 bat_percent,stationary_flag;
int16_t sendcount = 0,tp1,bar_height;
u8 flag_getwgresp;
void LoraHeartBeartPoll(void)
{
    lora_sendbuffer[MSG_TYPE_IDX] = LORA_MSGTYPE_TAGHEARTBEAT;
    lora_sendbuffer[MSG_LENGTH] = 14;
    memcpy(&lora_sendbuffer[SOURCE_ID_IDX],&g_com_map[DEV_ID],2);
    memcpy(&lora_sendbuffer[DEST_ID_IDX],&wg_report_id,2);
    memcpy(&lora_sendbuffer[HB_VERSION_IDX],&g_com_map[VERSION],2);
    lora_sendbuffer[HB_UWBPOWER_IDX] = g_com_map[POWER];
    lora_sendbuffer[HB_LORAPOWER_IDX] = g_com_map[LORA_POWER];
    checksum = Checksum_u16(lora_sendbuffer,14);
    memcpy(&lora_sendbuffer[14],&checksum,2);
    Radio.Send(lora_sendbuffer,16);
}
uint16_t heatbeat_count = 3600;
int16_t intheight;
#define HEATBEAT_UPDATE_TIME   3600
#define LORA_REPORT_MAXANC_NUM 20
extern float Height;
u8 tarray[20]= {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20};
void LoraReportPoll(void)
{
    // delay_ms(100);
#ifdef _USE_BAR
    GetPressAndHeight();
    intheight = Height*100;
#endif
#ifdef _SMT_TEST
    printf("Æøѹֵ£º%d",intheight);
#endif

    TagListUpdate();

    LoraReportFreqPoll();
    flag_getwgresp = 0;
    //  wg_report_freq = REPORT_MANGE_CHANNEL_FRQ;
    SwitchLoraSettings(wg_report_freq,REPORT_CHANNEL_SF,g_com_map[LORA_POWER]);
    if(heatbeat_count++>HEATBEAT_UPDATE_TIME && WG_Connected == wg_state) //Èç¹ûÐÄÌø°üµ½´ïÉÏ´«Ê±¼ä£¬²¢ÇÒÍø¹Ø´¦ÓÚÁ´½Ó״̬£¬¾ÍÉÏ´«ÐÄÌø°ü
    {
        heatbeat_count = 0;
        LoraHeartBeartPoll();
        return;
    }
    for(u16 i=0; i<report_ancnum-1; i++)
    {
        for(u16 j=0; j<report_ancnum-1-i; j++)
        {
            if(report_ancdist[j]>report_ancdist[j+1])
            {
                u16 id,dist;
                u8 bat;
                id = report_ancid[j];
                dist = report_ancdist[j];
                report_ancid[j] = report_ancid[j+1];
                report_ancdist[j] = report_ancdist[j+1];
                report_ancid[j+1] = id;
                report_ancdist[j+1] = dist;
            }
        }
    }
    lora_sendbuffer[BAT_IDX] = report_ancnum;//²â¾à»ùÕ¾ÊýÁ¿
    if(report_ancnum>LORA_REPORT_MAXANC_NUM)    //¿¼ÂÇlora´«Êäʱ¼ä£¬×î¶à·¢ËÍ10¸ö»ùÕ¾Êý¾Ý¡£
        report_ancnum = LORA_REPORT_MAXANC_NUM;
#ifdef USE_GPS
    lora_sendbuffer[MSG_TYPE_IDX] = LORA_MSGTYPE_TAGMSGTOWG_GPS;
    lora_sendbuffer[MSG_LENGTH] = 4*report_ancnum+30;
#else
    lora_sendbuffer[MSG_TYPE_IDX] = LORA_MSGTYPE_TAGMSGTOWG;
    lora_sendbuffer[MSG_LENGTH] = 4*report_ancnum+ANCID_IDX;
#endif
    memcpy(&lora_sendbuffer[SOURCE_ID_IDX],&g_com_map[DEV_ID],2);
    memcpy(&lora_sendbuffer[DEST_ID_IDX],&wg_report_id,2);
    lora_sendbuffer[SEQNUM_IDX] = seq_num++;
    //lora_sendbuffer[BAT_IDX] = bat_percent;
    lora_sendbuffer[STATE_IDX] = !GET_USERKEY|stationary_flag<<1;
#ifdef USE_GPS
    memcpy(&lora_sendbuffer[GPS_JINGDU_IDX],&gps_jingdu,8);
    memcpy(&lora_sendbuffer[GPS_WEIDU_IDX],&gps_jingdu,8);
    memcpy(&lora_sendbuffer[GPS_HEIGHT_IDX],&gps_height,8);
    lora_sendbuffer[GPS_STATE_IDX] = gps_state;
    lora_sendbuffer[GPS_SATEL_NUM_IDX] = gps_satel_num;
    lora_sendbuffer[GPS_SPOWER_IDX] = gps_signalpower;
    lora_sendbuffer[GPS_CHAFENLINGQI] = gps_chafenlingqi;
#endif
  //  memcpy(&lora_sendbuffer[BAR_HEIGHT_IDX],&bar_height,2);
    lora_sendbuffer[ANCNUM_IDX] = report_ancnum;
    memcpy(&lora_sendbuffer[ANCID_IDX],report_ancid,report_ancnum*2);
    memcpy(&lora_sendbuffer[ANCID_IDX+report_ancnum*2],report_ancdist,report_ancnum*2);
    checksum = Checksum_u16(lora_sendbuffer,4*report_ancnum+ANCID_IDX);
    memcpy(&lora_sendbuffer[ANCID_IDX+report_ancnum*4],&checksum,2);
    Radio.Send(lora_sendbuffer,ANCID_IDX+report_ancnum*4+2);
    LED_TB_ON;
    bat_percent=Get_Battary();

}
uint8_t Lora_send_flag;
void LedTask(void)
{
    if(report_ancnum>0)
    {
        LED2_TB_ON;
#ifdef _SMT_TEST
//        tflag1 = 1;
        printf("²â¾àÖµ%d\r\n",tagdist_list[0]);
#endif
    }
    if(wg_lost_count<=1)
    {
#ifdef _SMT_TEST
        //      tflag2 = 1;
        printf("Íø¹ØͨѶÕý³£\r\n");
#endif
        LED_TB_ON;
    }
    if(!HAL_GPIO_ReadPin(INPUT_5V_GPIO_Port,INPUT_5V_Pin))
    {
        if(bat_percent<15)
        {
            LED_TR_ON;
        } else {
            LED2_TB_ON;
        }
    }

}
void LedOff_task(void)
{
    LED_TB_OFF;
    LED_TR_OFF;
    LED2_TB_OFF;
    LED2_TR_OFF;
}
extern uint8_t rec_secdelay;
u16 lora_send_count;
uint8_t send_count;

void Lora_Send_Poll(void)
{
    if(Lora_send_flag)
    {
        if(lora_send_count++%10==0||lora_jianting_flag)
        {
            Lora_send_flag=0;
           
            if(rec_secdelay>0)
            {
              //  loraled=YELLOW;
                rec_secdelay--;
                return;
            }  
            LedTask();
            LoraReportPoll();
            LedOff_task();
        
        }
    }
}