/*
*
*
*/


/* Includes ------------------------------------------------------------------*/
#include "main.h"

#include "instance.h"

/* Private typedef -----------------------------------------------------------*/

#pragma pack(4)
typedef struct
{
    uint8_t channel ;
    uint8_t prf ;
    uint8_t datarate ;
    uint8_t preambleCode ;
    uint8_t preambleLength ;
    uint8_t pacSize ;
    uint8_t nsSFD ;
    uint8_t txPower;
    uint16_t sfdTO ;
} chConfig_t ;
#pragma pack()

#define REPORTBUFFER_LENGTH     4096
#define REPORTBUFFER_COUNT      4
#define REPORTBUFFER_MASK       (REPORTBUFFER_COUNT-1)

#define REPORT_HEAD_LENGTH      7
#define REPORT_TAGTOF_LENGTH    128
#define REPORT_THRESHOLD        (REPORT_HEAD_LENGTH+(REPORTBUFFER_LENGTH-REPORT_HEAD_LENGTH)/REPORT_TAGTOF_LENGTH*REPORT_TAGTOF_LENGTH)
#define REPORT_PEROID           480

typedef struct _report_buf_{
    uint8_t state;
    uint8_t count;
    uint16_t length;
    uint8_t buffer[REPORTBUFFER_LENGTH];
}stReportBuffer;

typedef struct _range_report_
{
    uint16_t index;
    uint16_t state;
    uint32_t time;
    stReportBuffer reportBuffers[REPORTBUFFER_COUNT];
}stRangeReport;


typedef struct _anc_command_
{
    uint8_t command1;
    uint8_t command2;
    uint8_t command3;
    uint8_t tagCount;
}stAnchorCommand;
/* Private define ------------------------------------------------------------*/
stRangeReport rangeReport = {0};
stAnchorCommand ancCommand = {0};

volatile stExtandCommand g_stExtandCommand = {0};

/* Private macro -------------------------------------------------------------*/
#define DEFAULT_DWM_ADDRESS         1

#define MASK_RANGEPEROID            0x0f

#define DEFAULT_RANGEPEROID         0
#define MIN_RANGEPEROID             2

/* Crystal frequency, in hertz. */
#define XTAL_FREQ_HZ                38400000

///command defination
#define COMMAND_ITME_SIZE           8

#define CMD_ANC_ADDRL               0
#define CMD_ANC_ADDRH               1
#define CMD_ANC_CMD1                2
#define CMD_ANC_CMD2                3
#define CMD_ANC_CMD3                4
#define CMD_ANC_TAGCOUNT            5

#define CMD_TAGx_ADDRL              0
#define CMD_TAGx_ADDRH              1
#define CMD_TAGx_CTRL               2
#define CMD_TAGx_CMD                3
#define CMD_TAGx_RESV1              4
#define CMD_TAGx_RESV2              5

/* Private variables ---------------------------------------------------------*/
//
// 0 1 2 ---- F
// peroid (ms)
const uint16_t PeroidValue[16] = {
    1000, 10, 15, 30, 60, 125, 250, 500, 1000, 2000, 3000, 5000, 10000, 30000,60000, 1000
};

const chConfig_t defaultChConfig = {
    .channel = 2,              // channel
    .prf = DWT_PRF_64M,    // prf
    .datarate = DWT_BR_6M8,    // datarate
    .preambleCode = 9,             // preambleCode
    .preambleLength = DWT_PLEN_128,   // preambleLength
    .pacSize = DWT_PAC8,       // pacSize
    .nsSFD = 0,       // non-standard SFD
    .txPower = 0x1f,
    .sfdTO = (129 + 8 - 4), //SFD timeout
};

const uint8_t prf16pCodes[4] = { 3, 4, 3, 4 };
const uint8_t prf64pCodes[4] = { 9, 10, 11, 12 };

//Slot and Superframe Configuration for DecaRangeRTLS TREK Modes (4 default use cases selected by the switch S1 [2,3] on EVB1000, indexed 0 to 3 )
const sfConfig_t sfConfig ={
    .slotPeriod = (TA_SLOT_TIME),   // slot period ms
    .numSlots = (TA_SLOT_COUNT),   // number of slots (only 10 are used) - thus 100 ms superframe means 10 Hz location rate
    .sfPeriod = (TA_SLOT_TIME * TA_SLOT_COUNT), // superframe period (100 ms - gives 10 Hz)
    .pollSleepDly = (TA_SLOT_TIME * TA_SLOT_COUNT), // poll sleep delay (tag sleep time, usually = superframe period)
    .replyDly = (TA_SLOT_TIME*TA_SLOT_FACTOR),
};

const stStationConfig defaultDwmConfig={
    .address = DEFAULT_DWM_ADDRESS,//
    .panID   = 0xdeca,//
    .signal={
        .channel = 0, 	    // channel2: 0; channel5: 1;
        .prfMode = 0, 	    // 0:64M; 1:16M;
        .pCode = 0,         // Preamble Code, ON PRF=64M: {0:9,1:10,2:11,3:12}, ON PRF=16M: {0:3,1:4,2:3,3:4}
        .encrypt = 1,       // 0:None; 1:TEA key=ScenAuto1234!@#$
        .reserved1 = 0,     // reserved1
        .reserved2 = 0,     // reserved2
        .reserved3 = 0,     // reserved3
        .temp = 0,
        .peroid = DEFAULT_RANGEPEROID,
    },
    .fixPower = 0x1f1f1f1f,
};

//
stStationConfig dwsConfig = {
    .panID   = 0xdeca,//
    .address = DEFAULT_DWM_ADDRESS,//
    .signal={
        .channel = 0, 	    // channel2: 0; channel5: 1;
        .prfMode = 0, 	    // 0:64M; 1:16M;
        .pCode = 0,         // Preamble Code, ON PRF=64M: {0:9,1:10,2:11,3:12}, ON PRF=16M: {0:3,1:4,2:3,3:4}
        .encrypt = 1,       // 0:None; 1:TEA key=ScenAuto1234!@#$
        .reserved1 = 0,     // reserved1
        .reserved2 = 0,     // reserved2
        .reserved3 = 0,     // reserved3
        .temp = 0,
        .peroid = DEFAULT_RANGEPEROID,
    },
    .fixPower = 0x1f1f1f1f,
};


//
uint32_t instance_anchaddr = 0;

/* Private function prototypes -----------------------------------------------*/
void addressconfigure(uint32_t addr);
int decarangingmode(stStationConfig cfg);


// ======================================================
void DWM_SetDefault(void)
{
    dwsConfig = defaultDwmConfig;
}

//
//  Configure instance tag/anchor/etc... addresses
//
void addressconfigure(uint32_t addr)
{
    uint16_t instAddress ;
    instance_anchaddr = addr;
    instAddress = GATEWAY_ANCHOR_ADDR | instance_anchaddr;
    instSetAddress(instAddress);
}

//returns the use case / operational mode
int decarangingmode(stStationConfig cfg)
{
    return cfg.signal.prfMode;
}

uint32_t inittestapplication(stStationConfig cfg)
{
    int chan;//, tagaddr, ancaddr;
    uint32_t devID ;
    instanceConfig_t instConfig;
    //uint16_t shortAddress;
    //uint16_t lp_osc_freq, sleep_cnt;

    BSP_DWM_SPI_FastRate(0);  //max SPI before PLLs configured is ~4M

    //this is called here to wake up the device (i.e. if it was in sleep mode before the restart)
    devID = instReadDeviceID() ;
    /*sprintf(debugBuffer, "ID: %d\r\n", devID);
    ReportMessage(debugBuffer);*/
    if(DWT_DEVICE_ID != devID) //if the read of device ID fails, the DW1000 could be asleep
    {
        port_SPIx_CS_clear();  //CS low

        Sleep(2);   //200 us to wake up then waits 5ms for DW1000 XTAL to stabilise
        port_SPIx_CS_set();  //CS high
        Sleep(7);
        devID = instReadDeviceID() ;
        // SPI not working or Unsupported Device ID
        if(DWT_DEVICE_ID != devID)
            return(-1) ;
        //clear the sleep bit - so that after the hard reset below the DW does not go into sleep
        dwt_softreset();
    }
    devID = dwt_read32bitoffsetreg(0x0F,0);//SYS_STATUS_ID
    devID++;

    //reset the DW1000 by driving the RSTn line low
    reset_DW1000();

    /*dwt_setaddress16(0xa5a5);
    dwt_getaddress16(&shortAddress);
    shortAddress++;*/
    devID = dwt_read32bitoffsetreg(0x0F,0);//SYS_STATUS_ID
    devID++;

    chan = instance_init();
    if (0 > chan)
        return(-1) ; // Some failure has occurred

    Sleep(1);
        /*port_SPIx_clear_chip_select();  //CS low

        Sleep(1);   //200 us to wake up then waits 5ms for DW1000 XTAL to stabilise
        port_SPIx_set_chip_select();  //CS high
        Sleep(7);*/
    devID = dwt_read32bitoffsetreg(0x0F,0);//SYS_STATUS_ID
    devID++;

#if (DEEP_SLEEP==2)
    /* Calibrate and configure sleep count. This has to be done with DW1000 clocks set to crystal speed. */
    lp_osc_freq = (XTAL_FREQ_HZ / 2) / dwt_calibratesleepcnt();
    sleep_cnt = ((SLEEP_TIME_MS * lp_osc_freq) / 1000) >> 12;
    dwt_configuresleepcnt(sleep_cnt);
#endif

    BSP_DWM_SPI_FastRate(1); //increase SPI to max

    devID = instReadDeviceID() ;
    if (DWT_DEVICE_ID != devID)   // Means it is NOT DW1000 device
    {
        // SPI not working or Unsupported Device ID
        return(-1) ;
    }

    instSetPanID(cfg.panID);
    addressconfigure(cfg.address) ;                            // set up initial payload configuration

    chConfig_t chCfg = defaultChConfig;
    chCfg.channel = cfg.signal.channel ? 5 : 2;
    chCfg.prf = cfg.signal.prfMode ? DWT_PRF_16M : DWT_PRF_64M;
    if (DWT_PRF_64M == chCfg.prf)
    {
        chCfg.preambleCode = prf64pCodes[cfg.signal.pCode];
    }
    else
    {
        chCfg.preambleCode = prf16pCodes[cfg.signal.pCode];
    }

    chan = instConfig.channelNumber = chCfg.channel ;
    instConfig.preambleCode = chCfg.preambleCode ;
    instConfig.pulseRepFreq = chCfg.prf ;
    instConfig.pacSize = chCfg.pacSize ;
    instConfig.nsSFD = chCfg.nsSFD ;
    instConfig.sfdTO = chCfg.sfdTO ;
    instConfig.dataRate = chCfg.datarate ;
    instConfig.preambleLen = chCfg.preambleLength ;

    instConfig.txPower = 0x1f;

    instance_config(&instConfig, &sfConfig) ;                  // Set operating channel etc

    chan = PeroidValue[MASK_RANGEPEROID & cfg.signal.peroid];
    instUpdateFramePeroid(chan);

    g_stExtandCommand.command = EXTCMD_ANCFRAMEPEROID;
    g_stExtandCommand.param = dwsConfig.signal.peroid;

    return devID;
}

void process_dwRSTn_irq(void)
{
}

void process_deca_irq(void)
{
    //do{
        //__HAL_GPIO_EXTI_CLEAR_IT(DWM_IRQ_PIN);
    instance_process_irq(0);
    //}while(port_CheckEXT_IRQ() == 1); //while IRS line active (ARM can only do edge sensitive interrupts)
    //}while(port_CheckEXT_IRQ() != RESET); //while IRS line active (ARM can only do edge sensitive interrupts)
}


// ======================================================
//
int UpdateTagCommand(uint8_t* buf, uint32_t len)
{
    if(len < COMMAND_ITME_SIZE)
    {
        return -1;
    }
    int index = 0, count = 0;
    index = buf[CMD_ANC_ADDRL] + (buf[CMD_ANC_ADDRH]<<8);
    if(index != gInst->setting.instanceAddress16)
    {
        return -2;
    }
    ancCommand.command1 = buf[CMD_ANC_CMD1];
    ancCommand.command2 = buf[CMD_ANC_CMD2];
    ancCommand.command3 = buf[CMD_ANC_CMD3];
    ancCommand.tagCount = buf[CMD_ANC_TAGCOUNT];

    index = 6;
    count = 0;
    for(; index < len && count < ancCommand.tagCount && count < MAX_TAG_LIST_SIZE; count++)
    {
        SaveTagCommand(buf+index, COMMAND_ITME_SIZE);
        index += COMMAND_ITME_SIZE;
    }
    return count;
}

void StoreTagStatus()
{
    //tagStatus
}

void ExcuteTimeSettingCommand(void)
{
    stDateTime.year     = rtcTime.tm_year;
    stDateTime.month    = rtcTime.tm_mon;
    stDateTime.day      = rtcTime.tm_mday;
    stDateTime.hour     = rtcTime.tm_hour;
    stDateTime.minute   = rtcTime.tm_min;
    stDateTime.second   = rtcTime.tm_sec;
    RtcSetDateTime(&stDateTime);

    secondsFromCentry(&rtcTime, (uint32_t*)&g_stTimeValue.timeValue);
    
    g_stTimeValue.tickReceived = 2;
}

void ClearTimeSettingCommand(void)
{
    if(0 < g_stTimeValue.timeValue)
    {
        if (g_stTimeValue.tickReceived > 0)
        {
            g_stTimeValue.tickReceived--;
        }
        else
        {
            g_stTimeValue.timeValue = 0;//clear it after sent
            g_stTimeValue.tickReceived = 0;
        }
    }
}

void SendRangeReport(void)
{
    stReportBuffer* pReport = rangeReport.reportBuffers + rangeReport.index;

    //ReportMessage(pReport->buffer, pReport->length);
    SendMessage(pReport->buffer, pReport->length);
    SendMessageDbg(pReport->buffer, pReport->length);
    SendMessageWifi(pReport->buffer, pReport->length);
}

void CreateRangeReportHead()
{
    if(rangeReport.state) return;

    stReportBuffer* pReport;
    rangeReport.state = 1;

    pReport = rangeReport.reportBuffers + rangeReport.index;
    pReport->state = 1;
    pReport->count = 0;
    pReport->buffer[PROTOCAL_START_INDEX] = PROTOCAL_HEAD_FLAG;
    pReport->buffer[PROTOCAL_LENLOW_INDEX] = 0;
    pReport->buffer[PROTOCAL_LENHIGH_INDEX] = 0;
    pReport->buffer[PROTOCAL_CMD_INDEX] = CFG_COMMAND_REPORTRANGE;
    
    int index = 0;
    pReport->buffer[PROTOCAL_DATASTART_INDEX + index++] = gInst->setting.instanceAddress16 & 0xff;
    pReport->buffer[PROTOCAL_DATASTART_INDEX + index++] = (gInst->setting.instanceAddress16>>8) & 0xff;
    pReport->buffer[PROTOCAL_DATASTART_INDEX + index++] = anchorStatus.value;
    uint8_t pwDCFlag = powerVoltage.dcToggleFlag >= 5 ? 4 : 0;
    uint8_t pwStatus = (powerVoltage.status & 0x0f) | pwDCFlag;
    pReport->buffer[PROTOCAL_DATASTART_INDEX + index++] = (pwStatus << 4) + (powerVoltage.power & 0x0f);
    pReport->buffer[PROTOCAL_DATASTART_INDEX + index++] = 0;
    pReport->buffer[PROTOCAL_DATASTART_INDEX + index++] = 0;

    pReport->length = PROTOCAL_DATASTART_INDEX+index;
}

void UpdateAnchorStatus(void)
{
    stReportBuffer* pReport;

    pReport = rangeReport.reportBuffers + rangeReport.index;

    anchorStatus.status.range = pReport->count  > 0 ? 1 : 0;
    pReport->buffer[PROTOCAL_DATASTART_INDEX + 2] = anchorStatus.value;
    uint8_t pwDCFlag = powerVoltage.dcToggleFlag >= 5 ? 4 : 0;
    uint8_t pwStatus = (powerVoltage.status & 0x0f) | pwDCFlag;
    pReport->buffer[PROTOCAL_DATASTART_INDEX + 3] = (pwStatus << 4) + (powerVoltage.power & 0x0f);
    uint8_t pwDCVoltage = powerVoltage.batteryVoltage > 400 ? powerVoltage.batteryVoltage - 400 : 0;
    pReport->buffer[PROTOCAL_DATASTART_INDEX + 4] = pwDCVoltage;
    pReport->buffer[PROTOCAL_DATASTART_INDEX + 5] = pReport->count;
}

void AddRange2Report()
{
    if(!rangeReport.state) return;

    stReportBuffer* pReport = rangeReport.reportBuffers + rangeReport.index;
    if(!pReport->state)return;

    uint32_t valid, rxa, tof;
    stTagStatus tagStatus;
    uint32_t addr = instNewRangeTagAdd();
    pReport->buffer[pReport->length++] = (uint8_t)addr;
    pReport->buffer[pReport->length++] = (uint8_t)(addr>>8);

    pReport->buffer[pReport->length++] = instGetTagRangeNum(addr);
    GetTagStatus(addr, &tagStatus);
    
    pReport->buffer[pReport->length++] = tagStatus.reserved1;
    pReport->buffer[pReport->length++] = tagStatus.status;
    pReport->buffer[pReport->length++] = tagStatus.power;
    pReport->buffer[pReport->length++] = tagStatus.heartRate;
    pReport->buffer[pReport->length++] = tagStatus.reserved2;

    for(valid = 0,rxa = 0; valid < MAX_ANCHOR_LIST_SIZE; valid++)
    {
        addr = instGetIdistAddress(valid);
        if(BROADCAST_ADDR == addr)continue;

        tof = instGetIdist_mm(valid);
        pReport->buffer[pReport->length++] = (uint8_t)addr;
        pReport->buffer[pReport->length++] = (uint8_t)(addr>>8);
        pReport->buffer[pReport->length++] = (uint8_t)(tof);
        pReport->buffer[pReport->length++] = (uint8_t)(tof>>8);
        pReport->buffer[pReport->length++] = (uint8_t)(tof>>16);
        pReport->buffer[pReport->length++] = (uint8_t)(tof>>24);

        rxa++;
        if(rxa >= NUM_EXPECTED_RESPONSES)
        {
            break;
        }
    }
    if(rxa < NUM_EXPECTED_RESPONSES)
    {
        for(; rxa < NUM_EXPECTED_RESPONSES; rxa++)
        {
            pReport->buffer[pReport->length++] = 0;
            pReport->buffer[pReport->length++] = 0;
            pReport->buffer[pReport->length++] = 0;
            pReport->buffer[pReport->length++] = 0;
            pReport->buffer[pReport->length++] = 0;
            pReport->buffer[pReport->length++] = 0;
        }
    }
    pReport->count++;//tag count
}

void FinishRangeReport(void)
{
    if(!rangeReport.state) return;

    stReportBuffer* pReport = rangeReport.reportBuffers + rangeReport.index;
    if(!pReport->state)return;

    UpdateAnchorStatus();

    uint32_t len = pReport->length - PROTOCAL_HEAD_LENGTH + 1 + PROTOCAL_CHECKSUM_LENGTH;
    pReport->buffer[PROTOCAL_LENLOW_INDEX] = (uint8_t)len;
    pReport->buffer[PROTOCAL_LENHIGH_INDEX] = (uint8_t)(len>>8);
    
    uint32_t dataLen = pReport->length - PROTOCAL_HEAD_LENGTH;
    uint32_t remainder = dataLen % 8;
    if (remainder)
    {
        remainder = 8 - remainder;
    }
    while (remainder)
    {
        pReport->buffer[pReport->length++] = 0;
        dataLen++;
        len++;
        remainder--;
    }
    if (dwsConfig.signal.encrypt)
    {
        pReport->buffer[PROTOCAL_START_INDEX] = PROTOCAL_HEAD_ENCRYPT_FLAG;
        EncryptTEA(pReport->buffer, PROTOCAL_DATASTART_INDEX, dataLen);
    }

    pReport->buffer[PROTOCAL_LENLOW_INDEX] = (uint8_t)len;
    pReport->buffer[PROTOCAL_LENHIGH_INDEX] = (uint8_t)(len>>8);

    len = CalculateCRC16(pReport->buffer, pReport->length);
    pReport->buffer[pReport->length++] = len;
    pReport->buffer[pReport->length++] = len>>8;

    SendRangeReport();

    pReport->state = 0;
    rangeReport.state = 0;
}

uint32_t GetCurrentBufferLength()
{
    stReportBuffer* pReport = rangeReport.reportBuffers + rangeReport.index;
    return pReport->length;
}

void ReportResetSignPeriodically()
{
    uint8_t buf[MAX_MESSAGE_LEN];
    uint32_t bufLen = 0;
    
    BuildResponse(CFG_COMMAND_RESETSIGN, buf, &bufLen);
    SendMessage(buf, bufLen);
    SendMessageDbg(buf, bufLen);
    SendMessageWifi(buf, bufLen);
}

void ReportRangePeriodically(uint32_t flag)
{
    uint32_t tick;
    do
    {
        tick = HAL_GetTick();
        if(0 == rangeReport.state)
        {
            CreateRangeReportHead();
            rangeReport.time = tick;
        }
        else
        {
            UpdateAnchorStatus();
        }
        if(rangeReport.time > tick)
        {
            rangeReport.time = tick;
        }
        if(flag)
        {
            AddRange2Report();
        }

        if(REPORT_THRESHOLD <= GetCurrentBufferLength()||
           REPORT_PEROID <= (tick - rangeReport.time))
        {
            FinishRangeReport();

            rangeReport.index++;
            rangeReport.index &= REPORTBUFFER_MASK;

        }
        flag = 0;
    }
    while(0 == rangeReport.state);
}

void ReportRangeImmediately(uint32_t flag)
{
    CreateRangeReportHead();
    AddRange2Report();
    FinishRangeReport();

    rangeReport.index++;
    rangeReport.index &= REPORTBUFFER_MASK;
}

void  ReportRangeResult(int rx)
{
    int l = 0, r= 0, aaddr=0, taddr=0, n =0;
    uint32_t rangeTime=0;
    uint16_t txa, rxa, valid=1;

    //send the new range information to LCD and/or USB
    aaddr = instNewRangeAncAdd() & 0xff;
    taddr = instNewRangeTagAdd();
    rangeTime = instNewRangeTim() & 0xffffffff;

    l = instGetLCount() & 0xFFFF;

    r = instGetTagRangeNum(taddr);
    taddr &= 0xff;

    txa =  instTXAntennaDly();
    rxa =  instRXAntennaDly();
    valid = instValidRanges();

    //n = sprintf((char*)debugBuffer, "T:%d\r\n",portGetTickCnt());
    //ReportMessage(debugBuffer,n);

    n = 0;
    if(rx == TOF_REPORT_T2A)
    {
        n += sprintf((char*)(&debugBuffer[n]), "mc ");
        valid = 0;
        for(valid = 0,rxa = 0; valid < MAX_ANCHOR_LIST_SIZE; valid++)
        {
            if(instGetIdistAddress(valid) != BROADCAST_ADDR)//address > 0
            {
                n += sprintf((char*)(&debugBuffer[n]), "%04x %08x ",
                     instGetIdistAddress(valid), instGetIdist_mm(valid));

                rxa++;
                if(rxa >= NUM_EXPECTED_RESPONSES)
                {
                    break;
                }
            }
        }
        if(rxa < NUM_EXPECTED_RESPONSES)
        {
            for(; rxa < NUM_EXPECTED_RESPONSES; rxa++)
            {
                n += sprintf((char*)(&debugBuffer[n]), "0000 00000000 ");
            }
        }
        n += sprintf((char*)(&debugBuffer[n]), "%04x %02x %08x %c%d:%d T %d\r\n",
                     l, r, rangeTime,
                     'a', taddr,aaddr,
                     portGetTickCnt());
        ReportMessage(debugBuffer,n);
        SendMessage(debugBuffer,n);
        SendMessageDbg(debugBuffer, n);
        SendMessageWifi(debugBuffer,n);
    }

    instClearDistTableAll();
}
/********************************* END OF FILE *********************************/