#include "system.h"
#include "plIntr.h"

bool badFrames = false;

// Init system
int InitSystem()
{
    LOG_INFO("Init System Start");

    // interupt system
    InitIntrSystem(&gic, SCUGIC_DEVICE_ID);
    LOG_INFO("Init Intr System Done");
    usleep(5000);

    // Init Uart ps
    InitUart(&uart, &gic, UART_DEVICE_ID, UART_INTR_ID);
    LOG_INFO("Init UartPs Done");
    usleep(5000);

    // flash init
    FlashInit(&qspi, QSPI_DEVICE_ID);
    LOG_INFO("Init Flash Done");
    usleep(5000);

    uint32_t parameters = FlashReadParameters(&qspi);
    tdmaId = PARAMETERS_GET_ID(parameters);
    tdmaSlotNum = PARAMETERS_GET_SLOT_NUM(parameters);

    if (tdmaSlotNum == 0xFF || tdmaId == 0xFF)
    {
        tdmaId = 1;
        tdmaSlotNum = 2;
        xil_printf(ERROR_MSG("Flash Parameters %d:%d"), tdmaId,tdmaSlotNum);
        LOG_ERROR("Flash Parameters Wrong, please set parameters");
    }
    xil_printf(INFO_MSG("TDMA ID: %d\tSlot Number %d"),tdmaId,tdmaSlotNum);

    // Init GPIO
    InitGpioPs(&gpio, GPIOPS_DEVICE_ID);
    LOG_INFO("Init GPIO Done");
    usleep(5000);

    // ad9361
    ad9361_init();

    // axi dma
    InitAxiDma(&dma, AXIDMA_DEVICE_ID);
    SetupDmaIntr(&gic, &dma, DmaTxHandler, DmaRxHandler);
    LOG_INFO("Init DMA Done");
    usleep(5000);

    // mac
    macSendQueue = CreateQueue();

    // eth
    EthInit();
    LOG_INFO("Init Ethernet Done");
    sleep(1);
    for (int i = 0; i < 10; i++)
    {
        EthSend((uint8_t *)&txMacFrame, 100);
        xemacif_input(netif);
    }
    sleep(1);
    for (int i = 0; i < 10; i++)
    {
        EthSend((uint8_t *)&txMacFrame, 100);
        xemacif_input(netif);
    }

    // manual Interupt
    InitPlIntr(&gic);
    PhyInit();
    PhyRstOff();

    LOG_INFO("Init System Done");
    LOG_BLANK_LINE;

    return XST_SUCCESS;
}

int InitGpioPs(XGpioPs *gpioPtr, u16 deviceID)
{
    int status;
    XGpioPs_Config *gpioCfgPtr = XGpioPs_LookupConfig(deviceID);
    Xil_AssertNonvoid(gpioCfgPtr != NULL);
    status = XGpioPs_CfgInitialize(gpioPtr, gpioCfgPtr, gpioCfgPtr->BaseAddr);
    CHECK_STATUS(status, ERROR_MSG("Init GPIO fail"));
    return XST_SUCCESS;
}

int InitUart(XUartPs *uartPtr, XScuGic *gicPtr, u16 deviceID, u32 intrID)
{
    int status;
    XUartPs_Config *uartCfgPtr = XUartPs_LookupConfig(deviceID);
    status = XUartPs_CfgInitialize(uartPtr, uartCfgPtr, uartCfgPtr->BaseAddress);
    CHECK_STATUS(status, ERROR_MSG("Init Uart PS fail"));

    // set parameters
    XUartPs_SetInterruptMask(uartPtr, XUARTPS_IXR_TOUT);
    XUartPs_SetOperMode(uartPtr, XUARTPS_OPER_MODE_NORMAL);
    XUartPs_SetRecvTimeout(uartPtr, 32);

    XScuGic_Connect(gicPtr, UART_INTR_ID,
                    (Xil_ExceptionHandler)UartHandler,
                    (void *)uartPtr);
    XScuGic_SetPriorityTriggerType(gicPtr, deviceID, 0xA0, 0x3);
    XUartPs_Recv(&uart, uartRecvBuf, 64);
    memset(uartRecvBuf, 0, 64);
    XScuGic_Enable(gicPtr, UART_INTR_ID);

    XUartPs_WriteReg(uartPtr->Config.BaseAddress, XUARTPS_ISR_OFFSET, XUARTPS_IXR_MASK);
    return XST_SUCCESS;
}

int InitAxiDma(XAxiDma *dmaPtr, u16 deviceId)
{
    int status;
    XAxiDma_Config *dmaCfgPtr = XAxiDma_LookupConfig(deviceId);
    status = XAxiDma_CfgInitialize(dmaPtr, dmaCfgPtr);
    CHECK_STATUS(status, ERROR_MSG("Init Axi Dma Fails"));
    return XST_SUCCESS;
}

int SetupDmaIntr(XScuGic *gicPtr, XAxiDma *dmaPtr, Xil_InterruptHandler mm2s_handler, Xil_InterruptHandler s2mm_handler)
{
    // enable all the mask , direction is MM2S
    XAxiDma_IntrDisable(dmaPtr, XAXIDMA_IRQ_ALL_MASK, XAXIDMA_DMA_TO_DEVICE);
    XAxiDma_IntrEnable(dmaPtr, XAXIDMA_IRQ_ALL_MASK, XAXIDMA_DMA_TO_DEVICE);
    XScuGic_SetPriorityTriggerType(gicPtr, DMA_MM2S_INTR_ID, 8, 0x3);

    // enable all the mask , direction is S2MM
    XAxiDma_IntrDisable(dmaPtr, XAXIDMA_IRQ_ALL_MASK, XAXIDMA_DEVICE_TO_DMA);
    //    XAxiDma_IntrEnable(dmaPtr, XAXIDMA_IRQ_ALL_MASK, XAXIDMA_DEVICE_TO_DMA);
    XAxiDma_IntrEnable(dmaPtr, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DEVICE_TO_DMA);
    XScuGic_SetPriorityTriggerType(gicPtr, DMA_S2MM_INTR_ID, 0, 0x3);

    int status = XScuGic_Connect(gicPtr, DMA_MM2S_INTR_ID, mm2s_handler, dmaPtr);
    CHECK_STATUS(status, ERROR_MSG("Setup Dma MM2S Intr Fail"));

    status = XScuGic_Connect(gicPtr, DMA_S2MM_INTR_ID, s2mm_handler, dmaPtr);
    CHECK_STATUS(status, ERROR_MSG("Setup Dma S2MM Intr Fail"));

    XScuGic_Enable(gicPtr, DMA_MM2S_INTR_ID);
    XScuGic_Enable(gicPtr, DMA_S2MM_INTR_ID);

    return XST_SUCCESS;
}

int InitIntrSystem(XScuGic *gicPtr, u16 deviceId)
{
    int status;

    // Init gic
    XScuGic_Config *gicCfgPtr = XScuGic_LookupConfig(deviceId);
    status = XScuGic_CfgInitialize(gicPtr, gicCfgPtr, gicCfgPtr->CpuBaseAddress);
    CHECK_STATUS(status, ERROR_MSG("Init ScuGic Fails"));

    // Init Exception
    Xil_ExceptionInit();
    Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
                                 (Xil_ExceptionHandler)XScuGic_InterruptHandler,
                                 (void *)gicPtr);
    Xil_ExceptionEnable();

    return XST_SUCCESS;
}

int InitManualTrigger(XScuGic *gicPtr, u32 intrId, Xil_InterruptHandler handler)
{
    XScuGic_Connect(gicPtr, intrId, handler, NULL);
    XScuGic_SetPriorityTriggerType(gicPtr, intrId, 32, 0b11);
    XScuGic_Enable(gicPtr, intrId);
    return XST_SUCCESS;
}

void UartHandler(void *callBack)
{
    XUartPs *uartPtr = (XUartPs *)callBack;
    XUartPs_WriteReg(uartPtr->Config.BaseAddress, XUARTPS_ISR_OFFSET, XUARTPS_IXR_MASK);
    u32 num = XUartPs_Recv(uartPtr, uartRecvBuf, 64);
    if (num == 0)
    {
        return;
    }
    //    xil_printf("Recv len is %d\r\nRecv Data is\t", num);
    //    xil_printf(uartRecvBuf);
    //    LOG_BLANK_LINE;
    if (strncmp(uartRecvBuf, "ATTEN=", 6) == 0)
    {
        u32 atten = atoi(uartRecvBuf + 6);
        xil_printf(INFO_MSG("Set Atten = %d"), atten);
        ad9361_set_tx_attenuation(ad9361_phy, 0, atten * 1000);
    }
    //    else if (strncmp(uartRecvBuf, "GAIN=", 5) == 0)
    //    {
    //        u32 gain = atoi(uartRecvBuf + 5);
    //        xil_printf(INFO_MSG("Set Gain = %d"), gain);
    //        ad9361_set_rx_rf_gain(ad9361_phy, 0, gain);
    //        PhyRstOn();
    //        usleep(10);
    //        PhyRstOff();
    //    }
    else if (strncmp(uartRecvBuf, "GAIN?", 5) == 0)
    {
        int32_t gain = 0;
        ad9361_get_rx_rf_gain(ad9361_phy, 0, &gain);
        xil_printf(INFO_MSG("Gain = %d dB"), gain);
    }
    //    else if (strncmp(uartRecvBuf, "PHYRST", 6) == 0)
    //    {
    //        PhyRstOn();
    //        usleep(10);
    //        PhyRstOff();
    //    }
    else if (strncmp(uartRecvBuf, "ID=", 3) == 0)
    {
        u32 id = atoi(uartRecvBuf + 3);
        tdmaId = id;
        xil_printf(INFO_MSG("ID = %d"), tdmaId);
        FlashWriteParameters(&qspi, PARAMETERS_FORMAT(tdmaId, tdmaSlotNum));
        PhyConfigTdma(tdmaId, tdmaSlotNum, SLOT_LEN, TIC_2_PS);
    }
    else if (strncmp(uartRecvBuf, "SN=", 3) == 0)
    {
        u32 slotNum = atoi(uartRecvBuf + 3);
        tdmaSlotNum = slotNum;
        xil_printf(INFO_MSG("Slot Num = %d"), tdmaSlotNum);
        FlashWriteParameters(&qspi, PARAMETERS_FORMAT(tdmaId, tdmaSlotNum));
        PhyConfigTdma(tdmaId, tdmaSlotNum, SLOT_LEN, TIC_2_PS);
    }
    else if(strncmp(uartRecvBuf, "T=", 2) == 0)
    {
    	testFrameSyncTimes = atoi(uartRecvBuf + 2);

    }
    else
    {
        LOG_WARNING("INVALID COMMAND");
    }
    memset(uartRecvBuf, 0, num + 3);
}

// MM2S
void DmaTxHandler(void *Callback)
{
    XAxiDma *axiDmaPtr = (XAxiDma *)Callback;
    u32 IrqStatus = XAxiDma_IntrGetIrq(axiDmaPtr, XAXIDMA_DMA_TO_DEVICE);

    if ((IrqStatus & XAXIDMA_IRQ_ERROR_MASK))
    {
        // LOG_ERROR("AXI DMA Error");
        xil_printf(ERROR_MSG("MM2S DMA Error Code :0x%x"), IrqStatus);
        u32 v = 0;
        v = XAxiDma_ReadReg((axiDmaPtr)->RegBase + (XAXIDMA_TX_OFFSET), XAXIDMA_SRCADDR_OFFSET);
        xil_printf("v: 0x%x\r\n", v);
        v = XAxiDma_ReadReg((axiDmaPtr)->RegBase + (XAXIDMA_TX_OFFSET), XAXIDMA_SRCADDR_MSB_OFFSET);
        xil_printf("v: 0x%x\r\n", v);
        v = XAxiDma_ReadReg((axiDmaPtr)->RegBase + (XAXIDMA_TX_OFFSET), XAXIDMA_BUFFLEN_OFFSET);
        xil_printf("v: 0x%x\r\n", v);
        // XAxiDma_Reset(AxiDmaInst);
        return;
    }
    XAxiDma_IntrAckIrq(axiDmaPtr, IrqStatus, XAXIDMA_DMA_TO_DEVICE);
    transDone = true;
    //	if ((IrqStatus & XAXIDMA_IRQ_IOC_MASK))
    //	{
    //		LOG_INFO("DMA TX Done");
    //	}
}

// S2MM
void DmaRxHandler(void *Callback)
{
    XAxiDma *axiDmaPtr = (XAxiDma *)Callback;

    u32 IrqStatus;
    /* Read pending interrupts */
    IrqStatus = XAxiDma_IntrGetIrq(axiDmaPtr, XAXIDMA_DEVICE_TO_DMA);
    XAxiDma_IntrAckIrq(axiDmaPtr, IrqStatus, XAXIDMA_DEVICE_TO_DMA);
    recvDone = true;

    if ((IrqStatus & XAXIDMA_IRQ_ERROR_MASK))
    {
        XAxiDma_Reset(axiDmaPtr);
        u32 v = XAxiDma_ReadReg((axiDmaPtr)->RegBase + (XAXIDMA_RX_OFFSET), XAXIDMA_SR_OFFSET);
        xil_printf(ERROR_MSG("S2MM DMA Error,val: 0x%x\r\n"), v);
        //        v = XAxiDma_ReadReg((axiDmaPtr)->RegBase + (XAXIDMA_RX_OFFSET), XAXIDMA_SRCADDR_MSB_OFFSET);
        //        xil_printf("v: 0x%x\r\n",v);
        //        v = XAxiDma_ReadReg((axiDmaPtr)->RegBase + (XAXIDMA_RX_OFFSET), XAXIDMA_BUFFLEN_OFFSET);
        //        xil_printf("v: 0x%x\r\n",v);
        return;
    }
}
