/*
 * tr_queue_handler.c
 *
 *  Created on: 2025年8月22日
 *      Author: 龙三郎
 */
#include "tr_queue.h"
#include <stdio.h>
#include "device/console.h"
#include "device/lwip.h"
#include "algorithm/emg_util.h"
#include "uartStdio.h"
#include "usbhspecific.h"
#include <stdlib.h>
#include "tr_protocol.h"

extern struct udp_pcb *upcb;
void handleSign(unsigned char *s, unsigned short sl);
void handleSign2(unsigned char *s, unsigned short sl);
void handleGUI(unsigned char *s, unsigned short sl);
/**
 * 数据转发业务处理，需要进行转发的数据在这个里面处理
 * params   da     目的地址
 */
void dataForwardedHandle(unsigned char *s, unsigned short sl, unsigned char da)
{
//    printf("进入到数据转发！\r\n");
    if (da == F4_ADDRESS)
    {
        // 执行发送到F4的逻辑
        USBHSPECIFICWrite(s, sl);
    }
    else if (da == GUI_ADDRESS)
    {
        // 执行发送到GUI的逻辑
        lwip_udp_send_gui(s, sl);
    }
    else if (da == UART_ADDRESS)
    {
        // 执行发送到串口的逻辑
        UARTwrite((const char*) s, sl);
    }

}

/**
 * 不需要转发的数据。即其他模块发送给本模块的数据。
 * params   sa      数据源地址
 * params   type    数据类型
 */
void dataNotForwardedHandle(unsigned char *s, unsigned short sl,
                            unsigned char sa, unsigned char type)
{
//    printf("进入到数据处理！\r\n");
    if (sa == F4_ADDRESS)
    {
        // 处理从F4发来的数据
        handleSign(s + 8, sl - 8);

    }
    else if (sa == GUI_ADDRESS)
    {
        // 处理从gui发来的数据
        handleGUI(s,sl);

    }
    else if (sa == UART_ADDRESS)
    {
        // 处理从uart发来的数据
//        ConsolePrintf("got it!!!\n");
    }

//    if(type == TYPE_DEFAULT)
//    {
//        // 执行自定义数据的逻辑
//    }
//    else if(type == TYPE_WAVEFORM)
//    {
//        // 执行波形数据的逻辑
//    }
//    else if(type == TYPE_COMMAND)
//    {
//        // 执行指令数据的逻辑
//    }

}

/**
 * 处理采集信号
 */

#define COUNT_SIZE 75
#define CHANNEL_COUNT 4 //采集通道总数
void handleSign(unsigned char *s, unsigned short sl)
{
    ConsolePuts("handle signal data from usb\n", -1);

    struct Frame
    {
        unsigned char sign;
        unsigned char d1[3];
        unsigned char d2[3];
        unsigned char d3[3];
        unsigned char d4[3];
    };
    short FRAME_LEN = sizeof(struct Frame);
    if (sl % FRAME_LEN > 0)
    {
        ConsolePuts("长度不匹配\n", -1);
        return;
    }

    int dataCount = sl / FRAME_LEN; //每个通道数据量, 每三个字节代表一个采集信号. 大端模式.

    float *const cd1 = malloc(sizeof(float) * dataCount);
    float *const cd2 = malloc(sizeof(float) * dataCount);
    float *const cd3 = malloc(sizeof(float) * dataCount);
    float *const cd4 = malloc(sizeof(float) * dataCount);

    static float fd1[COUNT_SIZE] = { 0.0 };
    static float fd2[COUNT_SIZE] = { 0.0 };
    static float fd3[COUNT_SIZE] = { 0.0 };
    static float fd4[COUNT_SIZE] = { 0.0 };
    static float fdgui[COUNT_SIZE * 4] = { 0.0 };

    const int FULL = 0x007fffff;
    const float FFULL = (float) FULL;
    int i = 0;
    static int scount = 0;
    for (i = 0; i < dataCount; i++)
    {
        struct Frame *fm = (struct Frame*) (s + FRAME_LEN * i);
        int origenData1 = fm->d1[0] << 24 | fm->d1[1] << 16 | fm->d1[2] << 8;
        *(cd1 + i) = ((float) (origenData1 >> 8) / FFULL) * 5000; //将采集数据转成mV

        int origenData2 = fm->d2[0] << 24 | fm->d2[1] << 16 | fm->d2[2] << 8;
        *(cd2 + i) = ((float) (origenData2 >> 8) / FFULL) * 5000; //将采集数据转成mV

        int origenData3 = fm->d3[0] << 24 | fm->d3[1] << 16 | fm->d3[2] << 8;
        *(cd3 + i) = ((float) (origenData3 >> 8) / FFULL) * 5000; //将采集数据转成mV

        int origenData4 = fm->d4[0] << 24 | fm->d4[1] << 16 | fm->d4[2] << 8;
        *(cd4 + i) = ((float) (origenData4 >> 8) / FFULL) * 5000; //将采集数据转成mV

        fd1[scount] = *(cd1 + i);
        fd2[scount] = *(cd2 + i);
        fd3[scount] = *(cd3 + i);
        fd4[scount] = *(cd4 + i);
        scount++;
        if (scount == COUNT_SIZE)
        {
            scount = 0;
            if (fm->sign & 0x01)
            {
                emg_denoised(cd1, COUNT_SIZE, 1000, fd1);
            }
            if (fm->sign & 0x02)
            {
                emg_denoised(cd2, COUNT_SIZE, 1000, fd2);
            }
            if (fm->sign & 0x04)
            {
                emg_denoised(cd3, COUNT_SIZE, 1000, fd3);
            }
            if (fm->sign & 0x08)
            {
                emg_denoised(cd4, COUNT_SIZE, 1000, fd4);
            }
            int m = 0;
            for(m = 0; m<COUNT_SIZE; m++)
            {
                fdgui[m*CHANNEL_COUNT] = fd1[m];
                fdgui[m*CHANNEL_COUNT+1] = fd2[m];
                fdgui[m*CHANNEL_COUNT+2] = fd3[m];
                fdgui[m*CHANNEL_COUNT+3] = fd4[m];
            }
            lwip_udp_send_gui_signal(fdgui,  COUNT_SIZE*CHANNEL_COUNT, fm->sign);
        }
    }

    free(cd1);
    free(cd2);
    free(cd3);
    free(cd4);

}

void handleSign2(unsigned char *s, unsigned short sl)
{
    ConsolePuts("===handle signal 2 data from usb\n", -1);
    struct Frame
    {
        unsigned char sign;
        unsigned char d1[3];
        unsigned char d2[3];
        unsigned char d3[3];
        unsigned char d4[3];
    };
    short FRAME_LEN = sizeof(struct Frame);
    if (sl % FRAME_LEN > 0)
    {
        ConsolePuts("长度不匹配\n", -1);
        return;
    }

    int dataCount = sl / FRAME_LEN; //每个通道数据量, 每三个字节代表一个采集信号. 大端模式.

    float *const cd1 = malloc(sizeof(float) * dataCount);
    float *const cd2 = malloc(sizeof(float) * dataCount);
    float *const cd3 = malloc(sizeof(float) * dataCount);
    float *const cd4 = malloc(sizeof(float) * dataCount);
    static float fd1[COUNT_SIZE] = { 0.0 };
    static float fd2[COUNT_SIZE] = { 0.0 };
    static float fd3[COUNT_SIZE] = { 0.0 };
    static float fd4[COUNT_SIZE] = { 0.0 };

    const int FULL = 0x007fffff;
    const float FFULL = (float) FULL;
    int i = 0;
    static int scount = 0;

    for (i = 0; i < dataCount; i++)
    {
        struct Frame *fm = (struct Frame*) (s + FRAME_LEN * i);
        int origenData1 = fm->d1[0] << 24 | fm->d1[1] << 16 | fm->d1[2] << 8;
        *(cd1 + i) = ((float) (origenData1 >> 8) / FFULL) * 5000; //将采集数据转成mV

        int origenData2 = fm->d2[0] << 24 | fm->d2[1] << 16 | fm->d2[2] << 8;
        *(cd2 + i) = ((float) (origenData2 >> 8) / FFULL) * 5000; //将采集数据转成mV

        int origenData3 = fm->d3[0] << 24 | fm->d3[1] << 16 | fm->d3[2] << 8;
        *(cd3 + i) = ((float) (origenData3 >> 8) / FFULL) * 5000; //将采集数据转成mV

        int origenData4 = fm->d4[0] << 24 | fm->d4[1] << 16 | fm->d4[2] << 8;
        *(cd4 + i) = ((float) (origenData4 >> 8) / FFULL) * 5000; //将采集数据转成mV
        fd1[scount] = *(cd1 + i);
        fd2[scount] = *(cd2 + i);
        fd3[scount] = *(cd3 + i);
        fd4[scount] = *(cd4 + i);
        scount++;
        if (scount == COUNT_SIZE)
        {
            scount = 0;
            if (fm->sign & 0x01)
            {
                emg_denoised(cd1, COUNT_SIZE, 1000, fd1);
            }
            if (fm->sign & 0x02)
            {
                emg_denoised(cd2, COUNT_SIZE, 1000, fd2);
            }
            if (fm->sign & 0x04)
            {
                emg_denoised(cd3, COUNT_SIZE, 1000, fd3);
            }
            if (fm->sign & 0x08)
            {
                emg_denoised(cd4, COUNT_SIZE, 1000, fd4);
            }
            const int STEP = 100;
            int pos = 0;
            int left = 0;
            do
            {
                int left = COUNT_SIZE - pos;
                if (left <= STEP)
                {
                    lwip_udp_send_vofa4(cd1 + pos, cd2 + pos, cd3 + pos,
                                        cd4 + pos, left);
                }
                else
                {
                    lwip_udp_send_vofa4(cd1 + pos, cd2 + pos, cd3 + pos,
                                        cd4 + pos, STEP);
                    pos += STEP;
                }
            }
            while (left > STEP);
        }
    }

    free(cd1);
    free(cd2);
    free(cd3);
    free(cd4);

}

void handleGUI(unsigned char *s, unsigned short sl)
{
    ConsolePuts("\n===handleGUI from gui\n", -1);
    ConsolePutsHexStr(s, sl);
    unsigned char check = 0;
    int a = ProtocolGetOption(s,sl,99,&check);
    if(a)
    {
        unsigned char udpAdd[8] = {0};
        trProtocol verPro;
        ProtocolInit(&verPro);
        ProtocolSetDestAddress(&verPro,3);
        ProtocolSetSourceAddress(&verPro,1);
        ProtocolSetType(&verPro,2);
        //ProtocolSetOption(&verPro,98,sizeof(udpAdd),udpAdd);
        ProtocolSetOptionChar(&verPro, 100, 1);
        ProtocolPackage(&verPro);
        ConsolePuts("\n===handleGUI to gui version\n", -1);
        ConsolePutsHexStr(verPro.message, verPro.length);
        lwip_tcp_send_gui(verPro.message,verPro.length);
    }
}