#include <stdio.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <pthread.h>
#include <string.h>
#include <sys/stat.h>
#include <ctype.h>
#include <malloc.h>
#include <sys/wait.h>
#include <unistd.h>
#include <time.h>
#include <sys/time.h>
#include <math.h>

#include "ChooseCmd.h"
#include "Fpga.h"
#include "AHB.h"
#include "GPIO.h"

typedef struct sockaddr Sockaddr;
typedef struct sockaddr_in Sockaddr_in;

#define MAX_STR_LENGTH 64
#define READ_LENGTH 4
#define READ_NUM 64 //因为这边可能有AHB_Memory_Write的参数，所以缓冲区要开大一些
#define DATA_NUM 65536

#define DEBUG 0                    //0--无打印信息；1--有打印信息
extern unsigned char mutex;        //ahb互斥锁
extern unsigned char server_mutex; //fpga互斥锁

#if THREADPRIORITY
int thread_first = 1;
#endif

#if TIMETEST
struct timeval start, end, end1;
double maxtime, maxtime1, maxtime2;
double mintime = 1;
double tmptime, tmptime1, tmptime2;
int exceed_time;
int maxfifolength = 0;
int minfifolength = 65535;
int fifo_buf_tail_previous;
int fifolength;
int maxtimelength;
double maxlengthtime;
int maxlength = 0;
int minlength = 65535;
struct timeval buf_start, buf_end;
double buf_maxtime = 0;
double buf_tmptime;
#endif

#define MAX_PARAS 10
typedef struct cmd_para
{
    int num_of_para;
    char *paras[MAX_PARAS];
} cmd_para_t;

int dedispersion_flag = 0;
int ensure_read = 0;
/******************************************************/
//功能： 获取文件大小
//参数： pFile--文件指针
/******************************************************/
off_t getfilesize(FILE *pFile)
{
    // check FILE*.
    if (pFile == NULL)
    {
        return -1;
    }

    // get file size.
    int fd = fileno((FILE *)pFile);
    if (fd == -1)
    {
        return -1;
    }
    struct stat fileStat;
    if (-1 == fstat(fd, &fileStat))
    {
        return -1;
    }

    // deal returns.
    return fileStat.st_size;
}
/******************************************************/
//功能： 往log文件中写入相关操作信息
//参数： intfaceinf--写入的信息
/******************************************************/
void writelog(char *intfaceinf)
{
    FILE *fp = NULL;
    fp = fopen("serverlog", "a");
    fprintf(fp, "%s\n", intfaceinf);
    if (getfilesize(fp) > 8000)
    {
        fclose(fp);
        fp = fopen("serverlog", "w");
    }
    fclose(fp);
}
/******************************************************/
//功能： 通过socket套接字往客户端发送信息
//参数： sock--套接字号
//      buffer--发送的消息内容
/******************************************************/
void send_message(int sock, char *buffer)
{
    int numbytes = 0;
    if ((numbytes = send(sock, buffer, strlen(buffer), 0)) == -1)
    {
        perror("send error");
    }
}
void send_data(int sock, const void *buffer, unsigned int len)
{
    int numbytes = 0;
    if ((numbytes = send(sock, buffer, len, 0)) == -1)
    {
        perror("send error");
    }
}
/******************************************************/
//功能： 字符串全部转换成大写
//参数： str--输入的字符串
//      buffer--转换为大写的字符串
/******************************************************/
char *strupr(char *str)
{
    char *orign = str;
    for (; *str != '\0'; str++)
        *str = toupper(*str);
    return orign;
}
/******************************************************/
//功能： 获取字符串中参数的个数，以空格为标识
//参数： cmdString--存放命令的字符串
//返回值：参数的个数
/******************************************************/
int get_para_num(char *cmdString)
{
    int i, r;
    r = 0;
    for (i = 1; i < strlen(cmdString); i++)
    {
        if ((cmdString[i] == ' ') && (cmdString[i - 1] != ' '))
            r++;
    }
    if (cmdString[i - 1] != ' ')
        r++;
    return r;
}
/******************************************************/
//功能： 获取字符串中指定的参数（一个中间函数，与get_para_str配合使用）
//参数： cmdString--存放命令的字符串
//      num--获取第num个参数
/******************************************************/
void get_para_str(char *cmdString, int num, char *retString)
{
    retString[0] = 0;

    if (get_para_num(cmdString) <= num)
        return;
    int i, r;
    //find the beginning of this parameter;
    for (i = 0, r = 0; (r < num) && (i < strlen(cmdString)); i++)
    {
        if ((cmdString[i] == ' ') && (cmdString[i - 1] != ' '))
            r++;
    }
    while (cmdString[i] == ' ')
        i++;
    r = 0;
    //i--;
    while ((cmdString[i] != ' ') && (cmdString[i] != 0x0d) && (cmdString[i] != 0x0a) && (cmdString[i] != 0))
    {
        retString[r] = cmdString[i];
        i++;
        r++;
    }
    retString[r] = 0;
    return;
}
/******************************************************/
//功能： 获取字符串中指定的参数
//参数： cmdString--存放命令的字符串
//      num--获取第num个参数
/******************************************************/
char *get_para(char *cmdString, int num, char *para)
{
    char tmp[MAX_STR_LENGTH];
    memset(para, 0, strlen(para));
    get_para_str(cmdString, num, tmp);
    if (strlen(tmp) > 0)
    {
        strcpy(para, tmp);
    }
    // if (num == 0)
    //     para = strupr(para);
    return para;
}

//Get all command parameters and the number of them.
static cmd_para_t get_cmd_para(char *parameter)
{
    cmd_para_t para;
    para.num_of_para = 0;
    para.paras[0] = strtok(parameter, " ");
    if (para.paras[0] == NULL)
        return para;
    else
    {
        para.num_of_para++;
        int i = 0;
        while (para.paras[i] != NULL)
        {
            i++;
            para.paras[i] = strtok(NULL, " ");
            if (para.paras[i] != NULL)
                para.num_of_para++;
        }
    }
    return para;
}

/******************************************************/
//功能： 以下为对应的ChooseCmd函数中，针对不同的命令的处理函数
/******************************************************/
void CMD_SYSTEM_CHECK(int sock)
{
    send_message(sock, "ARM_SERVER");
}

void CMD_FPGA_Download(int sock, char *buf)
{
    char status = 0;
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 1)
    {
        send_message(sock, "ERROR");
        return;
    }
    char *download_file = para.paras[0];
#if DEBUG
    printf("Download Filename:%s\n", para);
#endif
    while (server_mutex == 1)
        ; //ysy add
    server_mutex = 1;
    status = FPGA_Download(sock, download_file);
    server_mutex = 0;
    if (status == 1)
        send_message(sock, "OK");
    else
        send_message(sock, "ERROR");
}

void CMD_AHB_Read(int sock, char *buf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 1)
    {
        send_message(sock, "ERROR");
        return;
    }
    char BackBuffer[READ_LENGTH];
    memset(BackBuffer, 0, sizeof(BackBuffer));
    int offset = atoi(para.paras[0]);
    // gettimeofday(&end, NULL);
    // printf("AHB_READ Decode time:%f s\n", (double)end.tv_sec - start.tv_sec + 1.0e-6 * (end.tv_usec - start.tv_usec));
    unsigned short read_context = AHB_Read(offset);
#if DEBUG
    printf("AHB_Read_Data:%d\n", read_context);
#endif
    strcpy(BackBuffer, "OK");
    //sprintf(BackBuffer+2,"%d",read_context);
    memcpy(BackBuffer + 2, (char *)(&read_context), sizeof(unsigned short));
    send_data(sock, BackBuffer, sizeof(BackBuffer));
}

void CMD_FPGA_Init(int sock, char *buf)
{
    while (server_mutex == 1)
        ; //ysy add
    server_mutex = 1;
    FPGA_Init();
    server_mutex = 0;
    send_message(sock, "OK");
}

void CMD_AHB_Write(int sock, char *buf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 2)
    {
        send_message(sock, "ERROR");
        return;
    }
    char BackBuffer[MAX_STR_LENGTH];
    memset(BackBuffer, 0, sizeof(BackBuffer));
    char *addr = para.paras[0];
    char *data = para.paras[1];
    AHB_Write(atoi(addr), atoi(data));
    strcpy(BackBuffer, "OK");
    send_message(sock, BackBuffer);
}

void CMD_AHB_Memory_Read(int sock, char *buf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 4)
    {
        send_message(sock, "ERROR");
        return;
    }
    char *addr_addr = para.paras[0];
    char *data_addr = para.paras[1];
    char *start_addr = para.paras[2];
    char *len = para.paras[3];
    // gettimeofday(&end, NULL);
    // printf("AHB_MEMORY_READ Decode time:%f s\n", (double)end.tv_sec - start.tv_sec + 1.0e-6 * (end.tv_usec - start.tv_usec));
#if DEBUG
    printf("In AHB_Memory_Read:len=%d\n", atoi(len));
#endif
    char *buffer;
    buffer = malloc((atoi(len) * sizeof(unsigned short)) + 2); //实际分配的内存可能比申请的大，因为存在字节对齐的问题
    strcpy(buffer, "OK");
    AHB_Memory_Read(atoi(addr_addr), atoi(data_addr), atoi(start_addr), atoi(len), (unsigned short *)(buffer + 2));
    send_data(sock, buffer, (atoi(len) * sizeof(unsigned short)) + 2);
    free(buffer);
}

void CMD_AHB_Memory_Write(int sock, char *buf, char *databuf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 4)
    {
        send_message(sock, "ERROR");
        return;
    }
    char *addr_addr = para.paras[0];
    char *data_addr = para.paras[1];
    char *start_addr = para.paras[2];
    char *len = para.paras[3];
    int recv_len = 0;
    int target_len = 0;
    int tmp_len = 0;
    int timeout_count = 0;
    target_len = 2 * atoi(len);
    while (recv_len < target_len)
    {
        tmp_len = recv(sock, databuf + recv_len, sizeof(databuf), 0);
        if (tmp_len > 0)
        {
            recv_len += tmp_len;
            timeout_count = 0;
        }
        else
        {
            timeout_count++;
        }
        if (timeout_count > 100)
        {
            break;
        }
    }
    if (recv_len != target_len)
    {
#if DEBUG
        printf("Recv_len=%d\n", recv_len);
#endif
        send_message(sock, "ERROR");
    }
    else
    {
        AHB_Memory_Write(atoi(addr_addr), atoi(data_addr), atoi(start_addr), atoi(len), (unsigned short *)databuf);
        send_message(sock, "OK");
    }
}

void CMD_AHB_Memory_Read_Fast(int sock, char *buf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 4)
    {
        send_message(sock, "ERROR");
        return;
    }
    char *addr_addr = para.paras[0];
    char *data_addr = para.paras[1];
    char *start_addr = para.paras[2];
    char *len = para.paras[3];
#if DEBUG
    printf("In AHB_Memory_Read:len=%d\n", atoi(len));
#endif
    char *buffer;
    buffer = malloc((atoi(len) * sizeof(unsigned short)) + 2); //实际分配的内存可能比申请的大，因为存在字节对齐的问题
    strcpy(buffer, "OK");
    AHB_Memory_Read_Fast(atoi(addr_addr), atoi(data_addr), atoi(start_addr), atoi(len), (unsigned short *)(buffer + 2));
    send_data(sock, buffer, (atoi(len) * sizeof(unsigned short)) + 2);
    free(buffer);
}

void CMD_AHB_Memory_Write_Fast(int sock, char *buf, char *databuf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 4)
    {
        send_message(sock, "ERROR");
        return;
    }
    char *addr_addr = para.paras[0];
    char *data_addr = para.paras[1];
    char *start_addr = para.paras[2];
    char *len = para.paras[3];
    int recv_len = 0;
    int target_len = 0;
    int tmp_len = 0;
    int timeout_count = 0;
    target_len = 2 * atoi(len);
    while (recv_len < target_len)
    {
        tmp_len = recv(sock, databuf + recv_len, sizeof(databuf), 0);
        if (tmp_len > 0)
        {
            recv_len += tmp_len;
            timeout_count = 0;
        }
        else
        {
            timeout_count++;
        }
        if (timeout_count > 100)
        {
            break;
        }
    }
    if (recv_len != target_len)
    {
#if DEBUG
        printf("Recv_len=%d\n", recv_len);
#endif
        send_message(sock, "ERROR");
    }
    else
    {
        AHB_Memory_Write(atoi(addr_addr), atoi(data_addr), atoi(start_addr), atoi(len), (unsigned short *)databuf);
        send_message(sock, "OK");
    }
}

void CMD_AHB_FIFO_Read(int sock, char *buf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 8)
    {
        send_message(sock, "ERROR");
        return;
    }
    char *clk_addr = para.paras[0];
    char *data_addr = para.paras[1];
    char *full_addr = para.paras[2];
    char *empty_addr = para.paras[3];
    char *almost_empty_addr = para.paras[4];
    char *valid_addr = para.paras[5];
    char *len = para.paras[6];
    char *mode = para.paras[7];
#if DEBUG
    printf("In AHB_FIFO_Read:len=%d\n", atoi(len));
#endif
    char *buffer, *length_str;
    int length, read_mode;
    length = atoi(len);
    read_mode = atoi(mode);
    if (read_mode != 1)
    {
        buffer = malloc((length * sizeof(unsigned short)) + 2); //实际分配的内存可能比申请的大，因为存在字节对齐的问题
        strcpy(buffer, "OK");
        AHB_FIFO_Read(atoi(clk_addr), atoi(data_addr), atoi(full_addr), atoi(empty_addr), atoi(almost_empty_addr), atoi(valid_addr), length, (unsigned short *)(buffer + 2));
        send_data(sock, buffer, (length * sizeof(unsigned short)) + 2);
        free(buffer);
    }
    else
    {
        buffer = malloc((length * sizeof(unsigned short)) + 2);
        strcpy(buffer, "OK");
        length = AHB_FIFO_Read(atoi(clk_addr), atoi(data_addr), atoi(full_addr), atoi(empty_addr), atoi(almost_empty_addr), atoi(valid_addr), length, (unsigned short *)(buffer + 2), 1);
        if (length != -1)
        {
            length_str = (char *)&length;
            send_data(sock, length_str, 4);
            send_data(sock, buffer, (length * sizeof(unsigned short)) + 2);
        }
        else
        {
            length = 0;
            length_str = (char *)&length;
            send_data(sock, length_str, 4);
            send_message(sock, "ERROR");
        }
        free(buffer);
    }
}

void CMD_AHB_FIFO_Write(int sock, char *buf, char *databuf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 3)
    {
        send_message(sock, "ERROR");
        return;
    }
    char *clk_addr = para.paras[0];
    char *data_addr = para.paras[1];
    char *len = para.paras[2];
    int recv_len = 0;
    int target_len = 0;
    int tmp_len = 0;
    int timeout_count = 0;
    target_len = 2 * atoi(len);
    while (recv_len < target_len)
    {
        tmp_len = recv(sock, databuf + recv_len, sizeof(databuf), 0);
        if (tmp_len > 0)
        {
            recv_len += tmp_len;
            timeout_count = 0;
        }
        else
        {
            timeout_count++;
        }
        if (timeout_count > 100)
        {
            break;
        }
    }
    if (recv_len != target_len)
    {
#if DEBUG
        printf("Recv_len=%d\n", recv_len);
#endif
        send_message(sock, "ERROR");
    }
    else
    {
        AHB_FIFO_Write(atoi(clk_addr), atoi(data_addr), atoi(len), (unsigned short *)databuf);
        send_message(sock, "OK");
    }
}

void CMD_AHB_FIFO_BUF_SET(int sock, char *buf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 4)
    {
        send_message(sock, "ERROR");
        return;
    }
    fifo_buf_clk_addr = atoi(para.paras[0]);
    fifo_buf_data_addr = atoi(para.paras[1]);
    fifo_buf_valid_addr = atoi(para.paras[2]);
    fifo_buf_len = atoi(para.paras[3]);
    fifo_buf_head = 0;
    fifo_buf_tail = 0;
    fifo_buf = malloc(fifo_buf_len * 2);
    send_message(sock, "OK");
#if TIMETEST
    maxfiforead = 0;
    minfiforead = 1;
#endif
#if THREADPRIORITY
    thread_first = 1;
#endif
}

void CMD_AHB_FIFO_BUF_START(int sock)
{
    dedispersion_flag = 1;
    send_message(sock, "OK");
}

void CMD_AHB_FIFO_BUF_FLUSH(int sock)
{
    while (mutex == 1)
        ;
    mutex = 1;
    fifo_buf_tail = AHB_FIFO_Buf_Read(fifo_buf_clk_addr, fifo_buf_data_addr, fifo_buf_valid_addr, fifo_buf_len, fifo_buf, fifo_buf_tail);
    fifo_buf_head = 0;
    fifo_buf_tail = 0;
    memset(fifo_buf, 0, fifo_buf_len * sizeof(unsigned short));
    send_message(sock, "OK");
    mutex = 0;
#if TIMETEST
    exceed_time = 0;
    maxtime = 0;
    maxtime1 = 0;
    maxtime2 = 0;
    buf_maxtime = 0;
#endif
}

void CMD_AHB_FIFO_BUF_STOP(int sock)
{
    dedispersion_flag = 0;
    send_message(sock, "OK");
}

void CMD_AHB_FIFO_BUF_CLEAR(int sock)
{
    while (mutex == 1)
        ;
    free(fifo_buf);
    fifo_buf_head = 0;
    fifo_buf_tail = 0;
    send_message(sock, "OK");
#if TIMETEST
    printf("max timeout time: %f\n", maxtime);
    printf("max fiforead time: %f length: %d\n", maxtime2, maxtimelength);
    printf("min fiforead time: %f\n", mintime);
    printf("max loop time: %f\n", maxtime1);
    printf("max fifo length: %d time: %f\n", maxfifolength, maxlengthtime);
    printf("min fifo length: %d\n", minfifolength);
    printf("max length: %d\n", maxlength);
    printf("min length: %d\n", minlength);
    printf("max bufread time: %f\n", buf_maxtime);
    printf("max fiforead once time: %f\n", maxfiforead);
    printf("min fiforead once tiem: %f\n", minfiforead);
#endif
}

void CMD_AHB_FIFO_BUF_READ(int sock, char *buf)
{
#if TIMETEST
    gettimeofday(&buf_start, NULL);
#endif
    int tail_current, actual_head, actual_tail, length;
    tail_current = fifo_buf_tail;
    length = tail_current - fifo_buf_head;
    actual_head = fifo_buf_head % fifo_buf_len;
    actual_tail = tail_current % fifo_buf_len;
    send_data(sock, (char *)&length, 4);
    if (length != 0)
    {
#if TIMETEST
        maxlength = length > maxlength ? length : maxlength;
        minlength = length < minlength ? length : minlength;
#endif
        if (actual_tail > actual_head)
        {
            send_data(sock, fifo_buf + actual_head, length * sizeof(unsigned short));
        }
        else
        {
            send_data(sock, fifo_buf + actual_head, (length - actual_tail) * sizeof(unsigned short));
            if (actual_tail != 0)
            {
                send_data(sock, fifo_buf, actual_tail * sizeof(unsigned short));
            }
        }
        fifo_buf_head = tail_current;
    }
#if TIMETEST
    gettimeofday(&buf_end, NULL);
    buf_tmptime = (double)buf_end.tv_sec - buf_start.tv_sec + 1.0e-6 * (buf_end.tv_usec - buf_start.tv_usec);
    if (buf_tmptime > buf_maxtime)
    {
        buf_maxtime = buf_tmptime;
    }
#endif
}

void CMD_GPIO_Set_Config(int sock, char *buf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 3)
    {
        send_message(sock, "ERROR");
        return;
    }
    char *port = para.paras[0];
    char *bit = para.paras[1];
    char *dir = para.paras[2];
    GPIO_Set_Config(atoi(port), atoi(bit), atoi(dir));
    send_message(sock, "OK");
}

void CMD_GPIO_Get_Config(int sock, char *buf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 2)
    {
        send_message(sock, "ERROR");
        return;
    }
    char BackBuffer[READ_LENGTH];
    unsigned int status;
    char *port = para.paras[0];
    char *bit = para.paras[1];
    status = GPIO_Get_Config(atoi(port), atoi(bit));
    strcpy(BackBuffer, "OK");
    memcpy(BackBuffer + 2, (char *)(&status), sizeof(unsigned short));
    send_message(sock, BackBuffer);
}

void CMD_GPIO_Read(int sock, char *buf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 2)
    {
        send_message(sock, "ERROR");
        return;
    }
    char BackBuffer[READ_LENGTH];
    unsigned int status;
    char *port = para.paras[0];
    char *bit = para.paras[1];
    status = GPIO_Read(atoi(port), atoi(bit));
    strcpy(BackBuffer, "OK");
    memcpy(BackBuffer + 2, (char *)(&status), sizeof(unsigned short));
    send_message(sock, BackBuffer);
}

void CMD_GPIO_Write(int sock, char *buf)
{
    cmd_para_t para;
    para = get_cmd_para(buf);
    if (para.num_of_para != 3)
    {
        send_message(sock, "ERROR");
        return;
    }
    char *port = para.paras[0];
    char *bit = para.paras[1];
    char *value = para.paras[2];
    GPIO_Write(atoi(port), atoi(bit), atoi(value));
    send_message(sock, "OK");
}

void CMD_System(int sock, char *buf)
{
    char *system_buf = buf;
#if DEBUG
    printf("system=%s\n", system_buf);
#endif
    //Close_FPGA();
    while (server_mutex == 1)
        ;
    server_mutex = 1;
    int status = system(system_buf);
    server_mutex = 0;
    printf("status=%d\n", status);
    printf("Real_status=%d\n", WEXITSTATUS(status));
#if DEBUG
    printf("SYSTEM Status=%d\n", status);
#endif
    //Open_FPGA();
    if (-1 == status)
    {
        send_message(sock, "ERROR");
#if DEBUG
        printf("system error!");
#endif
    }
    else
    {
        if (WIFEXITED(status))
        {
            if (0 == WEXITSTATUS(status))
            {
                send_message(sock, "OK");
#if DEBUG
                printf("run shell script successfully.\n");
#endif
            }
            else
            {
                send_message(sock, "ERROR");
#if DEBUG
                printf("run shell script fail, script exit code: %d\n", WEXITSTATUS(status));
#endif
            }
        }
        else
        {
#if DEBUG
            printf("exit status = [%d]\n", WEXITSTATUS(status));
#endif
        }
    }
}
/******************************************************/
//功能： 命令处理函数，在一个线程中建立
//参数： sock--对应的socket连接
/******************************************************/
void *ChooseCmd(void *sock)
{
    char buf[READ_NUM];
    // char Cmd[READ_NUM];
    char databuf[DATA_NUM];
    int consock = *((int *)sock);
#if DEBUG
    printf("ChooseCmd consock=%d\n", consock);
#endif
    int numbytes;
    int bypass = 0;
#if TIMETEST
    int time_test = 0;
    struct timeval start_local;
#endif
    while (1)
    {
#if TIMETEST
        gettimeofday(&start_local, NULL);
#endif
        memset(buf, 0, sizeof(buf));
        numbytes = recv(consock, buf, sizeof(buf), 0);
        if (numbytes <= 0)
        {
            if (numbytes == 0)
            {
                shutdown(consock, 2);
                return 0;
            }
            else
            {
                if (bypass == 1)
                {
                    continue;
                }
                if (dedispersion_flag == 0)
                {
                    continue;
                }
                else
                {
#if THREADPRIORITY
                    if (thread_first == 1)
                    {
                        int ret = 0;
                        pid_t pid = getpid();
                        int current_scheduler = sched_getscheduler(pid);
                        if (current_scheduler < 0)
                        {
                            printf("getscheduler error\n");
                        }
                        printf("scheduler before %d\n", current_scheduler);
                        struct sched_param s_param;
                        s_param.sched_priority = sched_get_priority_max(SCHED_RR);
                        printf("scheduler max %d min %d\n", sched_get_priority_max(SCHED_RR), sched_get_priority_min(SCHED_RR));
                        ret = sched_setscheduler(pid, SCHED_RR, &s_param);
                        if (ret < 0)
                        {
                            printf("setscheduler error\n");
                        }
                        current_scheduler = sched_getscheduler(pid);
                        printf("scheduler after %d\n", current_scheduler);
                        thread_first = 0;
                    }
#endif
                    while (mutex == 1)
                    {
#if TIMETEST
                        time_test = 1;
#endif
                    }
                    mutex = 1;
#if TIMETEST
                    gettimeofday(&end, NULL);
                    if (time_test == 1)
                    {
                        tmptime = 0;
                    }
                    else
                    {
                        tmptime = (double)end.tv_sec - start_local.tv_sec + 1.0e-6 * (end.tv_usec - start_local.tv_usec);
                    }
                    if (tmptime > maxtime)
                    {
                        maxtime = tmptime;
                    }
                    fifo_buf_tail_previous = fifo_buf_tail;
#endif
                    fifo_buf_tail = AHB_FIFO_Buf_Read(fifo_buf_clk_addr, fifo_buf_data_addr, fifo_buf_valid_addr, fifo_buf_len, fifo_buf, fifo_buf_tail);
#if TIMETEST
                    fifolength = fifo_buf_tail - fifo_buf_tail_previous;
#endif
                    ensure_read = 1;
                    mutex = 0;
#if TIMETEST
                    gettimeofday(&end1, NULL);
                    if (time_test == 1)
                    {
                        tmptime1 = 0;
                        time_test = 0;
                    }
                    else
                    {
                        tmptime1 = (double)end1.tv_sec - start_local.tv_sec + 1.0e-6 * (end1.tv_usec - start_local.tv_usec);
                    }
                    if (tmptime1 > maxtime1)
                    {
                        maxtime1 = tmptime1;
                    }
                    tmptime2 = tmptime1 - tmptime;
                    if (tmptime2 > maxtime2)
                    {
                        maxtime2 = tmptime2;
                        maxtimelength = fifolength;
                    }
                    if (tmptime2 < mintime)
                    {
                        if (tmptime2 != 0)
                        {
                            mintime = tmptime2;
                        }
                    }
                    if (fifolength > maxfifolength)
                    {
                        maxfifolength = fifolength;
                        maxlengthtime = tmptime2;
                    }
                    if (fifolength < minfifolength)
                    {
                        minfifolength = fifolength;
                    }
#endif
                    continue;
                }
            }
        }
        else
        {
            if ((bypass == 0) && (ensure_read == 0) && (dedispersion_flag == 1))
            {
                while (mutex == 1)
                    ;
                mutex = 1;
                fifo_buf_tail = AHB_FIFO_Buf_Read(fifo_buf_clk_addr, fifo_buf_data_addr, fifo_buf_valid_addr, fifo_buf_len, fifo_buf, fifo_buf_tail);
                mutex = 0;
            }
            if (bypass == 0)
            {
                ensure_read = 0;
            }
#if DEBUG
            printf("Received CMD:%s\n", buf);
            printf("CMD Number:%d\n", get_para_num(buf));
#endif
            buf[numbytes] = '\0';
            char *Cmd = strtok(buf, " ");
            if (Cmd == NULL)
            {
                send_message(consock, "ERROR");
                continue;
            }
            char *argument;
            if (numbytes > strlen(Cmd) + 1)
            {
                argument = buf + strlen(Cmd) + 1;
            }
            else
            {
                argument = NULL;
            }
#if DEBUG
            printf("CMD:%s\n", Cmd);
#endif
            if (strcasecmp(Cmd, "AHB_FIFO_BUF_READ") == 0)
            {
                writelog("ahb fifo buf read");
                CMD_AHB_FIFO_BUF_READ(consock, argument);
            }
            else if (strcasecmp(Cmd, "AHB_FIFO_READ") == 0)
            {
                writelog("ahb fifo read");
                CMD_AHB_FIFO_Read(consock, argument);
            }
            else if (strcasecmp(Cmd, "AHB_READ") == 0)
            {
                CMD_AHB_Read(consock, argument);
            }
            else if (strcasecmp(Cmd, "AHB_MEMORY_READ") == 0)
            {
                writelog("ahb memory read");
                CMD_AHB_Memory_Read(consock, argument);
            }
            else if (strcasecmp(Cmd, "AHB_WRITE") == 0)
            {
                CMD_AHB_Write(consock, argument);
            }
            else if (strcasecmp(Cmd, "AHB_MEMORY_WRITE") == 0)
            {
                writelog("ahb memory write");
                CMD_AHB_Memory_Write(consock, argument, databuf);
            }
            else if (strcasecmp(Cmd, "AHB_FIFO_BUF_SET") == 0)
            {
                writelog("ahb fifo buf set");
                bypass = 1;
                CMD_AHB_FIFO_BUF_SET(consock, argument);
            }
            else if (strcasecmp(Cmd, "AHB_FIFO_BUF_START") == 0)
            {
                writelog("ahb fifo buf start");
                CMD_AHB_FIFO_BUF_START(consock);
            }
            else if (strcasecmp(Cmd, "AHB_FIFO_BUF_FLUSH") == 0)
            {
                writelog("ahb fifo buf flush");
                CMD_AHB_FIFO_BUF_FLUSH(consock);
            }
            else if (strcasecmp(Cmd, "AHB_FIFO_BUF_STOP") == 0)
            {
                writelog("ahb fifo buf stop");
                CMD_AHB_FIFO_BUF_STOP(consock);
            }
            else if (strcasecmp(Cmd, "AHB_FIFO_BUF_CLEAR") == 0)
            {
                writelog("ahb fifo buf clear");
                CMD_AHB_FIFO_BUF_CLEAR(consock);
            }
            else if (strcasecmp(Cmd, "AHB_FIFO_WRITE") == 0)
            {
                writelog("ahb fifo write");
                CMD_AHB_FIFO_Write(consock, argument, databuf);
            }
            else if (strcasecmp(Cmd, "AHB_MEMORY_READ_FAST") == 0)
            {
                writelog("ahb memory read fast");
                CMD_AHB_Memory_Read_Fast(consock, argument);
            }
            else if (strcasecmp(Cmd, "AHB_MEMORY_WRITE_FAST") == 0)
            {
                writelog("ahb memory write fast");
                CMD_AHB_Memory_Write_Fast(consock, argument, databuf);
            }
            else if (strcasecmp(Cmd, "SYSTEM_CHECK") == 0)
            {
                writelog("system check");
                CMD_SYSTEM_CHECK(consock);
            }
            else if (strcasecmp(Cmd, "FPGA_DOWNLOAD") == 0)
            {
                writelog("fpga download");
                CMD_FPGA_Download(consock, argument);
            }
            else if (strcasecmp(Cmd, "FPGA_INIT") == 0)
            {
                writelog("fpga init\n");
                CMD_FPGA_Init(consock, argument);
            }
            else if (strcasecmp(Cmd, "GPIO_SET_CONFIG") == 0)
            {
                writelog("gpio set");
                CMD_GPIO_Set_Config(consock, argument);
            }
            else if (strcasecmp(Cmd, "GPIO_GET_CONFIG") == 0)
            {
                writelog("gpio get");
                CMD_GPIO_Get_Config(consock, argument);
            }
            else if (strcasecmp(Cmd, "GPIO_READ") == 0)
            {
                writelog("gpio read");
                CMD_GPIO_Read(consock, argument);
            }
            else if (strcasecmp(Cmd, "GPIO_WRITE") == 0)
            {
                writelog("gpio write");
                CMD_GPIO_Write(consock, argument);
            }
            else if (strcasecmp(Cmd, "SYSTEM") == 0)
            {
                writelog("system");
                CMD_System(consock, argument);
            }
            else if (strcasecmp(Cmd, "HELLO") == 0)
            {
                writelog("hello");
                send_message(consock, "ARM");
            }
            else if (strcasecmp(Cmd, "CLOSESOCK") == 0)
            {
                writelog("close socket");
#if DEBUG
                printf("Close Socket!\n");
#endif
                shutdown(consock, 2);
                break;
            }
            else
            {
#if DEBUG
                printf("No Such Cmd!\n");
#endif
                writelog("No Such Cmd!");
                send_message(consock, "ERROR");
            }
        }
    }
    close(consock);
    return 0;
}