#include "l502api.h"
#include "e502api.h"
#include "dev_funcs.h"

#include "../../devs/e502/e502_fpga_regs.h"
#include "../../devs/e502/e502_fpga_regs.h"

#ifdef _WIN32
    #include <locale.h>
    #include <conio.h>
#else
    #include <signal.h>
    #include <unistd.h>
    #include <string.h>
    #include <sys/time.h>
    #include <unistd.h>
#endif

#include "timespec_funcs.h"

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <stdio.h>

#include "x502api_private.h"

#include "../../devs/e502/e502_cm4_defs.h"

#define STREAM_IN_WRD_TYPE(wrd) wrd & 0x80000000 ? STREAM_IN_WRD_ADC : \
      (wrd & 0xFF000000) == 0x0 ? STREAM_IN_WRD_DIN : \
    ((wrd & 0xFF000000)>>24) == 0x01 ? STREAM_IN_WRD_MSG : STREAM_IN_WRD_USR
    
    

    
/* признак необходимости завершить сбор данных */
static volatile int f_out = 0;

/* количество используемых логических каналов */
#define ADC_LCH_CNT  3

/* частота сбора АЦП в Гц*/
//#define ADC_FREQ          2000000
#define ADC_FREQ          400000
/* частота кадров (на логический канал). При ADC_FREQ/ADC_LCH_CNT - межкадровой задержки нет */
#define ADC_FRAME_FREQ    (ADC_FREQ/ADC_LCH_CNT)
/* частота синхронного ввода в Гц*/
#define DIGIN_FREQ              20000
#define DIGOUT_FREQ          400000

//#define ENABLE_DAC1
//#define ENABLE_DAC2

#define  E16_REG_TEST_ADC_CNTR_BIT        0
#define  E16_REG_TEST_WR_DISABLE_BIT      6
#define  E16_REG_TEST_RING_MODE_BIT       5

#define DIN_WRD_MASK        0xffff

/* сколько отсчетов считываем за блок */
#define READ_BLOCK_SIZE   4096*66*3

/* таймаут на прием блока (мс) */
#define READ_TIMEOUT     500


uint32_t tstCntr = 0x7788;
uint32_t tstCntr24 = 0x7788;

int dac2_once = 1;

typedef struct {
    t_x502_hnd hnd;
    // частота дискретизации АЦП
    double adc_freq;
    bool ring_mode;
    bool verbose;
    bool status_mode;
    bool is_e502;
    double digout_freq;
    double digin_freq;
    int streams_en;
} t_test_par;

#ifdef _WIN32
DWORD WINAPI ThreadFunc(void *arg)
#else
void * threadFunc(void *arg)
#endif
{
    t_test_par *par = (t_test_par*)arg;
    int32_t g_snd_cnt = 0;
    static uint32_t snd_buf[READ_BLOCK_SIZE];
    struct timespec start_time;
    struct timespec cur_time;
    struct timespec spent_time;

    clock_gettime(CLOCK_MONOTONIC, &start_time);

    while(!f_out) {
        int32_t snd_cnt = 0;
        uint32_t to_send = READ_BLOCK_SIZE;
        uint32_t *to_send_ptr = snd_buf;

        for (int i = 0; i < READ_BLOCK_SIZE; ) {
            snd_buf[i] = tstCntr24 & 0xffff;
            i++;
#ifdef ENABLE_DAC1
            snd_buf[i] = (tstCntr & 0xffff) | X502_STREAM_OUT_WORD_TYPE_DAC1;
            i++;
#endif
#ifdef ENABLE_DAC2
            snd_buf[i] = (tstCntr & 0xffff) | X502_STREAM_OUT_WORD_TYPE_DAC2;
            i++;
#endif
            tstCntr++;
            tstCntr24++;
        }
        
        while (to_send && !f_out) {
            snd_cnt = X502_Send(par->hnd, to_send_ptr, to_send, READ_TIMEOUT);
            if (snd_cnt < 0 || f_out) {
                printf("thread exiting X502_Send err=%d\n", snd_cnt);
                return snd_cnt;
            }
            to_send -= snd_cnt;
            to_send_ptr += snd_cnt;
        }

        g_snd_cnt += READ_BLOCK_SIZE;

        clock_gettime(CLOCK_MONOTONIC, &cur_time);
        if (cur_time.tv_sec  - start_time.tv_sec >= 5) 
        {
            timespec_diff(&cur_time, &start_time, &spent_time);

            printf("cnt=%d sec=%f snd speed=%f wrds/sec\n", g_snd_cnt, timespec_to_double(&spent_time), g_snd_cnt / timespec_to_double(&spent_time));
            start_time = cur_time;
            g_snd_cnt = 0;
        }
    }
    return 0;
}

/* номера используемых физических каналов */
static uint32_t f_channels[ADC_LCH_CNT] = {0,4,6};
/* режимы измерения для каналов */
static uint32_t f_ch_modes[ADC_LCH_CNT] = {X502_LCH_MODE_ZERO, X502_LCH_MODE_DIFF, X502_LCH_MODE_DIFF};
/* диапазоны измерения для каналов */
static uint32_t f_ch_ranges[ADC_LCH_CNT] = {X502_ADC_RANGE_10, X502_ADC_RANGE_10, X502_ADC_RANGE_10};


#ifndef _WIN32
/* Обработчик сигнала завершения для Linux */
static void f_abort_handler(int sig) {
    f_out = 1;
}
#endif

/* настройка параметров модуля */
int32_t f_setup_params(t_test_par *par) {
    int32_t err = X502_ERR_OK, i;
    int ch_cnt = 1;//ADC_LCH_CNT;
    
    X502_SetSyncMode(par->hnd, X502_SYNC_INTERNAL);
    //X502_SetSyncMode(par->hnd, X502_SYNC_DI_SYN1_FALL);
    X502_StreamsStop(par->hnd);
    X502_StreamsDisable(par->hnd, X502_STREAM_ALL_IN | X502_STREAM_ALL_OUT);

    /* устанавливаем параметры логической таблицы АЦП */
    err = X502_SetLChannelCount(par->hnd, ch_cnt);
    for (i=0; (i < ch_cnt) && (err == X502_ERR_OK); i++)
        err = X502_SetLChannel(par->hnd, i, f_channels[i], f_ch_modes[i], f_ch_ranges[i], 0);

    /* устанавливаем частоты ввода для АЦП и цифровых входов */
    if (err == X502_ERR_OK) {
        err = X502_SetAdcFreq(par->hnd, &par->adc_freq, NULL);
        if (err == X502_ERR_OK)
            err = X502_SetDinFreq(par->hnd, &par->digin_freq);
        if (err == X502_ERR_OK) {
            /* выводим реально установленные значения - те что вернули функции */
            printf("Установлены частоты:\n    Частота сбора АЦП = %0.0f\n"
                "    Частота цифрового ввода = %0.0f\n",
                par->adc_freq, par->digin_freq);
        }
    }
    
    err = X502_SetOutFreq(par->hnd, &par->digout_freq);
    printf("X502_SetOutFreq err=%d dout_freq = %.1f\n", err, par->digout_freq);

    /* разрешаем синхронные потоки */
    if (err == X502_ERR_OK) {
        err = X502_StreamsEnable(par->hnd, par->streams_en);
    }
    /* записываем настройки в модуль */
    if (err == X502_ERR_OK)
        err = X502_Configure(par->hnd, 0);
    return err;
}

void recv_proc(t_test_par *par) {
        struct timespec start_time;
        int32_t err = X502_ERR_OK;
        int32_t g_rcv_cnt = 0;
        int cntr = 0;
        int rcv_cntr = 1;//0x02000000;
        int last_good_rcv_cntr = 1;//0x02000000;
        int din_rcv_cntr = 1;//0x02000000;
        int din_error_cntr = 0;
        uint32_t ADC_WRD_MASK = 0xFFFFFF;

        if (par->ring_mode) {
            // если включен режим E16_REG_TEST_RING_MODE_BIT то проверяем только 16 бит
            // 32бит 31бит - из потока ЦАП переходят  18 и 17 бит потока АЦП при включенном ring mode
            ADC_WRD_MASK = 0xFFFF;
        }

        clock_gettime(CLOCK_MONOTONIC, &start_time);
        
        if ((par->streams_en & (X502_STREAM_ADC | X502_STREAM_DIN)) == 0) {
            while(!f_out) {}
        } else

        while (err == X502_ERR_OK && !f_out) {
            int32_t rcv_size;
            uint32_t adc_size, din_size;
            /* массив для приема необработанных данных */
            static uint32_t rcv_buf[READ_BLOCK_SIZE];
            int i;
            struct timespec cur_time;
            
            /* принимаем данные (по таймауту) */
            rcv_size = X502_Recv(par->hnd, rcv_buf, READ_BLOCK_SIZE, READ_TIMEOUT);
            if (rcv_size < 0) {
                fprintf(stderr, "Recv(%d) = %d\n", READ_BLOCK_SIZE, rcv_size);
                break;
            }
            g_rcv_cnt += rcv_size;
            int error = 0;
            
            clock_gettime(CLOCK_MONOTONIC, &cur_time);
            if (cur_time.tv_sec  - start_time.tv_sec >= 5 && par->verbose) {
                printf("rcv speed=%ld wrds/sec adc_cntr=%x din_cntr=%x din_error_cntr=%d\n", (g_rcv_cnt) / (cur_time.tv_sec  - start_time.tv_sec), rcv_cntr, din_rcv_cntr, din_error_cntr);
                start_time.tv_sec = cur_time.tv_sec;
                g_rcv_cnt = 0;
            }

            for (i = 0; i < rcv_size; i++) {
                
                // тест с заглушкой DOUT -> DIN
                t_stream_in_wrd_type type = STREAM_IN_WRD_TYPE(rcv_buf[i]);
                /* проверяем - это данные от АЦП или цифровых входов */
#define INC_VAL 1
                if (rcv_buf[i] == X502_STREAM_IN_MSG_OVERFLOW) {
                    if (rcv_buf[i] == rcv_cntr) {
                        // в E502 и E16 есть различие в режиме E16_REG_TEST_ADC_CNTR_BIT
                        // в E502 сырой счетчик 32 бит
                        // в E16 счетчик 24 бит, и чередование признака каналов какой был задан
                        rcv_cntr += INC_VAL;
                    } else {
                        printf("overflow msg received!\n");
                    }
                }
                else
                if (par->is_e502 || type == STREAM_IN_WRD_ADC) {
                    if ((rcv_buf[i] & ADC_WRD_MASK) != (rcv_cntr & ADC_WRD_MASK )) 
                    {
                        printf("ADC %x[%i] %x expected\n", rcv_buf[i], i, rcv_cntr);
                        printf("diff %d\n", (rcv_buf[i] & ADC_WRD_MASK) - (rcv_cntr & ADC_WRD_MASK));
                        rcv_cntr = rcv_buf[i];
                        error = 1;
                    }                    
                    rcv_cntr += INC_VAL;
                } else
                if (type == STREAM_IN_WRD_DIN) {
                    if ((rcv_buf[i]  & DIN_WRD_MASK)!= (din_rcv_cntr & DIN_WRD_MASK)) 
                    {
                        if (!din_error_cntr) {
                            printf("DIN %x[%i] %x expected\n", rcv_buf[i], i, din_rcv_cntr);
                            printf("diff %d\n", (rcv_buf[i] & DIN_WRD_MASK) - (din_rcv_cntr & DIN_WRD_MASK));
                        }
                        din_rcv_cntr = rcv_buf[i];
                        din_error_cntr++;
                    } else {
                        if (din_error_cntr) {
                            printf("DIN error cntr: %d, rcv_val = %x\n", din_error_cntr, rcv_buf[i]);
                            printf("din_cntr - last_good = %d\n", (rcv_buf[i] & DIN_WRD_MASK) - (last_good_rcv_cntr & DIN_WRD_MASK));
                        }
                        last_good_rcv_cntr = rcv_buf[i];
                        din_error_cntr = 0;
                    }
                    din_rcv_cntr += INC_VAL;
                } else {
                    if ((rcv_buf[i] & ADC_WRD_MASK) != (rcv_cntr & ADC_WRD_MASK )) 
                    {
                        printf("unknown wrd %x[%i] %x expected\n", rcv_buf[i], i, rcv_cntr);
                        rcv_cntr = rcv_buf[i];
                    }
                    rcv_cntr += INC_VAL;
                }
            }

#ifdef _WIN32
            /* проверка нажатия клавиши для выхода */
            if (err == X502_ERR_OK) {
                if (_kbhit())
                    f_out = 1;
            }
#endif
        }
}

#define ADC_FREQ_KEY              "adc_freq"
#define RING_MODE_KEY            "ring"
#define DIGOUT_FREQ_KEY        "digout_freq"
#define DIGIN_FREQ_KEY            "digin_freq"
#define VERBOSE_KEY               "verbose"

void print_usage(char** argv) {
    fprintf(stderr, "Опции:\n");
    fprintf(stderr, "  " ADC_FREQ_KEY ":частота АЦП в Гц, если не задано - выключен\n");
    fprintf(stderr, "  " DIGOUT_FREQ_KEY ":частота цифрового вывода Гц, если не задано - выключен\n");
    fprintf(stderr, "  " DIGIN_FREQ_KEY ":частота цифрового ввода Гц, если не задано - выключен\n");
    fprintf(stderr, "  " RING_MODE_KEY "\tкольцевой тест: ЦАП -> АЦП, иначе тестовый счетчик по каналу АЦП\n");
    fprintf(stderr, "  " VERBOSE_KEY "\tвыводить скорость\n");
    fprintf(stderr, "\nПример: использовать E16 по адресу 192.168.12.152 и установить частоту сбора 1000000 Гц:\n");
    fprintf(stderr, "  %s E16:192.168.12.152 " ADC_FREQ_KEY ":1000000\n", argv[0]);
    exit(0);
}

void parse_test_params(int argc, char** argv, t_test_par *par) {
    int i;

    par->adc_freq = ADC_FREQ;
    par->ring_mode = false;
    par->digout_freq = DIGOUT_FREQ;
    par->digin_freq = DIGIN_FREQ;
    par->verbose = false;
    par->streams_en = 0;
    par->status_mode = false;

    for (i=1; (int)i < argc; i++) {
        if (sscanf(argv[i], ADC_FREQ_KEY":%lf", &par->adc_freq) == 1) {
            par->streams_en |= X502_STREAM_ADC;
        } else
        if (sscanf(argv[i], DIGOUT_FREQ_KEY":%lf", &par->digout_freq) == 1) {
            par->streams_en |= X502_STREAM_ALL_OUT;
        } else
        if (sscanf(argv[i], DIGIN_FREQ_KEY":%lf", &par->digin_freq) == 1) {
            par->streams_en |= X502_STREAM_DIN;
        } else
        if (strcmp(argv[i], RING_MODE_KEY) == 0) {
            par->ring_mode = true;
            continue;
        } else
        if (strcmp(argv[i], VERBOSE_KEY) == 0) {
            par->verbose = true;
        } else
        if (strcmp("/?", argv[i]) == 0) {
            print_usage(argv);
        } else
        if (strcmp("--help", argv[i]) == 0) {
            print_usage(argv);
        } else
        if (strcmp("status", argv[i]) == 0) {
            par->status_mode = true;
        }
    }

    if (par->ring_mode) {
        // если кольцевой тест, то включаем поток на вывод
        par->streams_en |= X502_STREAM_ALL_OUT | X502_STREAM_ADC;
    }
}

int e502_lpc_tst_start(t_x502_hnd hnd, uint16_t test, uint16_t flags) {
    int err  = E502_CortexExecCmd(hnd, E502_CM4_CMD_TEST_START,
                                  test | ((uint32_t)flags << 16),
                                  NULL, 0, NULL, 0, 0, NULL);
    return err ;
}

int e502_lpc_tst_stop(t_x502_hnd hnd) {
    int err  = E502_CortexExecCmd(hnd, E502_CM4_CMD_TEST_STOP,
                                  0, NULL, 0, NULL, 0, 0, NULL);

    return err;
}

#define E502_REGS_ARM_DCI_TEST_MODE     0x10C

t_x502_info dev_info;

int main(int argc, char** argv) {
    int32_t err = X502_ERR_OK;
#ifndef _WIN32
    struct sigaction sa;
    memset(&sa, 0, sizeof(sa));
    /* В ОС Linux устанавливаем свой обработчик на сигнал закрытия,
       чтобы завершить сбор корректно */
    sa.sa_handler = f_abort_handler;
    sigaction(SIGTERM, &sa, NULL);
    sigaction(SIGINT, &sa, NULL);
    sigaction(SIGABRT, &sa, NULL);
#endif
#ifdef _WIN32
    /* для вывода русских букв в консоль для ОС Windows в CP1251 без перевода в OEM */
    setlocale(LC_CTYPE, "");
#endif

#ifdef _WIN32
    HANDLE thread;
#else
    pthread_t thread;
#endif
    
    t_test_par par;
    
    parse_test_params(argc, argv, &par);
    
    // HACK:
    // сбрасываем флаг X502_DEVFLAGS_FPGA_LOADED,
    // таким образом никаких манипуляций с регистрами не происходит
    // и мы не нарушаем работу с уже запущенным потоком в другом экземпляре процесса
    par.hnd = select_dev_from_list(argc, argv, par.status_mode ? X502_DEVFLAGS_FPGA_LOADED : 0);
    /********************************** Работа с модулем **************************/
    /* если успешно выбрали модуль и установили с ним связь - продолжаем работу */
    if (par.hnd == NULL) {
        return -1;
    }

    err = X502_GetDevInfo(par.hnd, &dev_info);
    par.is_e502 = strcmp(dev_info.name, "E502") ? false: true;
    printf("is_e502=%d\n", par.is_e502);

    if (!err && par.status_mode) {
        // Специальный режим:
        // по usb проверяем исчерпание буфера на вывод и переполнение буфера на ввода
        // при этом поток идет по ethernet, через другой экземпляр данной утилиты (надо запустить отдельно и указать IP адрес!)
        uint32_t status;
        struct timespec start_time;
        struct timespec cur_time;
        struct timespec spent_time;

        clock_gettime(CLOCK_MONOTONIC, &start_time);

        while (!f_out) {
            err = X502_OutGetStatusFlags(par.hnd, &status);
            if (err) {
                fprintf(stderr, "X502_OutGetStatusFlags error: %s\n", X502_GetErrorString(err));
                break;
            }

            clock_gettime(CLOCK_MONOTONIC, &cur_time);
            timespec_diff(&cur_time, &start_time, &spent_time);

            if (status & X502_OUT_STATUS_FLAG_BUF_WAS_EMPTY) {
                fprintf(stderr, "%f sec: BUF_WAS_EMPTY\n", timespec_to_double(&spent_time));
            }
#ifdef _WIN32
            Sleep(10);
#else
            sleep(10);
#endif
        }
        goto close_module;
    }
    
    if (!par.streams_en) {
        fprintf(stderr, "Потоки на ввод или вывод не включены!\n");
        print_usage(argv);
        return -1;
    }

    /* настраиваем параметры модуля */
    if (err == X502_ERR_OK) {
        err = f_setup_params(&par);
    }
    if (err != X502_ERR_OK) {
        fprintf(stderr, "Ошибка настройки модуля: %s!", X502_GetErrorString(err));
        return -1;
    }

    if (par.ring_mode) {
        // такого режима для E502 нет!
        // вместо данных АЦП получаем данные которые выслали на DOUT
        err = X502_FpgaRegWrite(par.hnd, E502_REGS_ARM_DCI_TEST_MODE, (1 << E16_REG_TEST_RING_MODE_BIT));
    } else {
        // вместо данных АЦП получаем счетчик
        err = X502_FpgaRegWrite(par.hnd, E502_REGS_ARM_DCI_TEST_MODE, (1 << E16_REG_TEST_ADC_CNTR_BIT));
    }
    
    //err = e502_lpc_tst_start(par.hnd, E502_CM4_TEST_DCI_CNTR, 0);
    if (err) {
        return err;
    }

    X502_FpgaRegWrite(par.hnd, E502_REGS_IOHARD_GO_SYNC_IO, 0);
    X502_FpgaRegWrite(par.hnd, E502_REGS_BF_CTL, X502_REGBIT_BF_CTL_BF_RESET_Msk);

    if (par.streams_en & X502_STREAM_ALL_OUT) {
#ifdef _WIN32
            thread = CreateThread(NULL, 0, ThreadFunc, &par, 0, NULL);
#else
            // Create a thread that will function threadFunc()
            err = pthread_create(&thread, NULL, threadFunc, &par);
#endif
        {
            uint32_t status = 0;
            do {
                err = X502_OutGetStatusFlags(par.hnd, &status);
            } while (!f_out && err == X502_ERR_OK && (status & X502_OUT_STATUS_FLAG_BUF_IS_EMPTY));
        }
    }

    /* запуск синхронного ввода-вывода */
    if (err == X502_ERR_OK) {
        err = X502_StreamsStart(par.hnd);
        if (err != X502_ERR_OK) {
            fprintf(stderr, "Ошибка запуска сбора данных: %s!\n", X502_GetErrorString(err));
        }
    }

    if (err == X502_ERR_OK) {
        int block;
        int32_t stop_err;

        printf("Сбор данных запущен. Для останова нажмите %s\n",
#ifdef _WIN32
                "любую клавишу"
#else
                "CTRL+C"
#endif
                );
        fflush(stdout);

        recv_proc(&par);

        if (par.streams_en & X502_STREAM_ALL_OUT) {
#ifdef _WIN32
            WaitForSingleObject(thread, INFINITE);
#else
            pthread_join(thread, NULL);
#endif
        }

        /* останавливаем поток сбора данных (независимо от того, была ли ошибка) */
        stop_err = X502_StreamsStop(par.hnd);
        if (stop_err != X502_ERR_OK) {
            fprintf(stderr, "Ошибка останова сбора данных: %s\n", X502_GetErrorString(err));
            if (err == X502_ERR_OK)
                err = stop_err;
        } else {
            printf("Сбор данных остановлен успешно\n");
        }
    }
close_module:

    // выключаем режим когда по DCI должен передаваться счетчик начиная с 1
    X502_FpgaRegWrite(par.hnd, E502_REGS_ARM_DCI_TEST_MODE, 0);

    /* закрываем связь с модулем */
    X502_Close(par.hnd);
    /* освобождаем описатель */
    X502_Free(par.hnd);
    return err;
}
