/*
    Данный пример демонстрирует работу с модулем LTR24.
    По умолчанию идет работа с первым слотом первого крейта, но это можно изменить
        через параметры командной строки:
    ltr25_recv  slot crate_serial srvip
    где:
        slot         - номер слота (от 1 до 16)
        crate_serial - серийный номер крейта (если крейтов больше одного)
        srvip        - ip-адрес программы ltrd или LtrServer (если он запущен на другой машине)
    Параметры опциональны: можно указать только слот, слот и серийный номер крейта или все

    Пример принимает данные от первых трех каналов на максимальной частоте сбора.
    На экране отображается значение пик-пик (максимум - минимум) по принятому блоку
    по каждому каналу.

    Пользователь может изменить настройки на свои, поменяв заполнение полей
    структуры перед запуском сбора.

    Сбор идет до нажатия любой клавиши на Windows или  CTRL+C на Linux

    Сборка в VisualStudio:
    Для того чтобы собрать проект в Visual Studio, измените путь к заголовочным файлам
    (Проект (Project) -> Свойства (Properties) -> Свойства конфигурации (Configuration Properties)
    -> С/С++ -> Общие (General) -> Дополнительные каталоги включения (Additional Include Directories))
    на нужный в зависимаости от пути установки библиотек (ltr25api.h  и остальные заголовочные
    файлы должны находится в поддиректории ltr/include относительно указанного пути)
    и измените путь к .lib файлам на <путь установки библиотек>/lib/msvc
    (Проект (Project) -> Свойства (Properties) -> Свойства конфигурации (Configuration Properties) ->
    Компоновщик (Linker) -> Общие (General) -> Дополнительные каталоги библиотек (Additional Library Directories)).

    Внимание!: Если Вы собираете проект под Visual Studio и у Вас некорректно
    отображается вывод русских букв, то нужно изменить кодировку:
    выберите Файл (File) -> Дополнительные параметры сохранения (Advanced Save Options)...
    и в поле Кодировка (Encoding) выберите Юникод (UTF8, с сигнатурой)/Unicode (UTF-8 with signature)
    и сохраните изменения в файле.
*/

#include "ltr/include/ltr24api.h"
#include "ltr/include/lpw25api.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "lmath.h"
#include "math.h"
#ifdef _WIN32
#include <locale.h>
#include <conio.h>
#else
#include <signal.h>
#include <unistd.h>
#endif

#include "phasefilter.h"

#undef M_PI

#ifndef M_PI
    #define M_PI 3.1415926535897932384626433832795
#endif


/* количество отсчетов на канал, принмаемых за раз */
#define RECV_TIME_MS  3000
/* таймаут на ожидание данных при приеме */
#define RECV_TOUT  4000


#define EN_CHANNELS_CNT 1
#define HARM_CNT  150
#define HARM_NUM(idx) (2*idx + 1)

#define EN_LTR_PHASE_COR
//#define LTR_AFC_COR_FLAG  (LTR24_PROC_FLAG_AFC_COR)
#define LTR_AFC_COR_FLAG  (LTR24_PROC_FLAG_AFC_COR_EX)
//#define LTR_AFC_COR_FLAG 0
//#define LPW25_EN_SENS_ROUT

#define LTR24_FREQ_CODE  LTR24_FREQ_117K //LTR24_FREQ_610 //LTR24_FREQ_117K // LTR24_FREQ_610 //LTR24_FREQ_39K


static const int en_channels_nums[EN_CHANNELS_CNT] = {0};//{0,1,2,3};
static const int ref_channels_idx[EN_CHANNELS_CNT] = {0,0,0,0};
static const double lpw_sens[EN_CHANNELS_CNT] = {LPW25_DEFAULT_SENS_U_2_230,
                                                 LPW25_DEFAULT_SENS_U_2_230,
                                                 LPW25_DEFAULT_SENS_I_1_5,
                                                LPW25_DEFAULT_SENS_I_1_5};
static const BOOLEAN ch_lpw_en[EN_CHANNELS_CNT] = {TRUE, TRUE, TRUE, TRUE};


typedef struct {
    double amp;
    double pha;
} t_harm_params;

typedef struct {
    double freq;
    int harm_cnt;
    t_harm_params harms[HARM_CNT];
} t_sig_params;


static double calc_pha_delay(const t_sig_params *params, int harm_idx) {
    return lmath_phase_delay(params->freq, params->harms[0].pha,
            params->freq * HARM_NUM(harm_idx),  params->harms[harm_idx].pha);
}

static void f_goertzel_amp_pha(const double *sig, int sig_size, double dt, double f, double *amp, double *pha) {
    t_lmath_goertzel_ctx ctx = lmath_goertzel_ctx_create(sig_size, dt, f);
    if (ctx) {
        lmath_goertzel_process(ctx, sig, sig_size);
        lmath_goertzel_result_amp_pha(ctx, LMATH_PHASE_MODE_SIN, amp, pha);
        lmath_goertzel_ctx_destroy(ctx);
    }
}


static void f_calc_params_goertzel(const double *sig, int size, double dt, t_sig_params *params) {
    int harm_idx;
    for (harm_idx = 0; harm_idx < params->harm_cnt; harm_idx++) {
        f_goertzel_amp_pha(sig, size, dt, params->freq * HARM_NUM(harm_idx),
                           &params->harms[harm_idx].amp, &params->harms[harm_idx].pha);
    }
}



static double fft_get_pha(double *pha_spectrum, double f, double df) {
    double rbin = f / df;
    int i = (int)rbin;
    double dpha = lmath_phase_conv_degree(pha_spectrum[i+1] - pha_spectrum[i]);
    return lmath_phase_normalize(fabs(pha_spectrum[i+1] - pha_spectrum[i]) * (rbin - i) + pha_spectrum[i]);
}


static void f_calc_params_fft(const double *sig, int size, double dt,  t_lmath_window_ctx winctx, t_sig_params *params) {
    int fft_size = size/2 + 1;
    double df;
    double *amp = (double*)malloc(sizeof(double)*fft_size);
    double *pha = (double*)malloc(sizeof(double)*fft_size);
    if (amp && pha) {
        lmath_amp_pha_spectrum_peak(sig, size, dt, 0, LMATH_PHASE_MODE_SIN, amp, pha, &df);

        int harm_idx;
        for (harm_idx = 0; harm_idx < params->harm_cnt; harm_idx++) {
            double harm_pwr;
            double harm_freq = params->freq * HARM_NUM(harm_idx);
            params->harms[harm_idx].pha = fft_get_pha(pha, harm_freq, df);
            lmath_spectrum_find_peak_freq(amp, fft_size, df, lmath_window_params(winctx), harm_freq, NULL, &harm_pwr);
            params->harms[harm_idx].amp = sqrt(harm_pwr);
        }
    }

    free(amp);
    free(pha);
}


static void print_results(const t_sig_params *ch_result, const t_sig_params *ref_ch) {
    int harm_idx;
    for (harm_idx = 0; harm_idx < ch_result->harm_cnt; harm_idx++) {
        double rms = ch_result->harms[harm_idx].amp/sqrt(2);
        double pha = ch_result->harms[harm_idx].pha;

        if (harm_idx == 0) {
            printf("  main freq: %f, %f (%f), %f", ch_result->freq, rms, rms * sqrt(2), lmath_phase_conv_degree(pha));
            if (ref_ch) {
                printf(" (%f)", lmath_phase_conv_degree(lmath_phase_normalize(pha - ref_ch->harms[0].pha)));
            }
        } else {
            printf("    harm %2d: %f, %f", HARM_NUM(harm_idx),
                   100. * ch_result->harms[harm_idx].amp/ch_result->harms[0].amp,
                   lmath_phase_conv_degree(calc_pha_delay(ch_result, harm_idx)));
            if (ref_ch) {
                printf(" (%f)", lmath_phase_conv_degree(lmath_phase_normalize(pha - ref_ch->harms[harm_idx].pha)));
            }
        }
        printf("\n");
    }
}


void lpw25_process_data(TLPW25_PROC_CTX *lpw25, double *data, int size) {
    LPW25_ProcessData(lpw25, data, data, size, 0);
}




void process_ch_data(const double *data, int points_cnt, double fd, t_sig_params *ch_result) {
    t_lmath_wintype wintype = LMATH_WINTYPE_BH_4TERM;
    double peak_freq, peak_pow;
    double *winsig = malloc(points_cnt * sizeof(winsig[0]));
    t_lmath_window_ctx winctx = lmath_window_ctx_create(wintype, 0, points_cnt);
    lmath_window_apply_scaled(data, winctx, winsig);


    lmath_find_peak_freq(data, points_cnt, 1./fd, -1, wintype, &peak_freq, &peak_pow);
    ch_result->freq = peak_freq;
    ch_result->harm_cnt = HARM_CNT;
    f_calc_params_goertzel(winsig, points_cnt, 1./fd, ch_result);
    //f_calc_params_fft(winsig, points_cnt, 1./fd, winctx, ch_result);
    lmath_window_ctx_destroy(winctx);
}




typedef struct {
    int slot;
    const char *serial;
    DWORD addr;
} t_open_param;


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

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




/* Разбор параметров командной строки. Если указано меньше, то используются
 * значения по умолчанию:
 * 1 параметр - номер слота (от 1 до 16)
 * 2 параметр - серийный номер крейта
 * 3 параметр - ip-адрес сервера */
static int f_get_params(int argc, char** argv, t_open_param* par) {
    int err = 0;
    par->slot = LTR_CC_CHNUM_MODULE1;
    par->serial = "";
    par->addr = LTRD_ADDR_DEFAULT;


    if (argc > 1)
        par->slot = atoi(argv[1]);
    if (argc > 2)
        par->serial = argv[2];
    if (argc > 3) {
        int a[4],i;
        if (sscanf(argv[3], "%d.%d.%d.%d", &a[0], &a[1], &a[2], &a[3])!=4) {
            fprintf(stderr, "Неверный формат IP-адреса!!\n");
            err = -1;
        }

        for (i=0; (i < 4) && !err; i++) {
            if ((a[i]<0) || (a[i] > 255)) {
                fprintf(stderr, "Недействительный IP-адрес!!\n");
                err = -1;
            }
        }

        if (!err) {
            par->addr = (a[0] << 24) | (a[1]<<16) | (a[2]<<8) | a[3];
        }
    }
    return err;
}





int read_file_sig(const char *filename, double *points, int size) {
    int ret_points = 0;
    FILE *f = fopen(filename, "rb");
    if (f) {
        ret_points = (int)fread(points, sizeof(double), (size_t)size, f);
    }
    return ret_points;
}


int main(int argc, char** argv) {
    INT err = LTR_OK;
    TLTR24 hltr24;
    t_open_param par;
#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*/
    setlocale(LC_ALL, "");
#endif

    t_sig_params ch_results[EN_CHANNELS_CNT];
    TLPW25_PROC_CTX lpw25[EN_CHANNELS_CNT];
    int ch_idx;

    err = f_get_params(argc, argv, &par);
    printf("TLTR24 size = %d, %d\n", sizeof (TLTR24), sizeof(TLTR24_MODULE_INFO));

    if (err==LTR_OK) {
        LTR24_Init(&hltr24);
        /* Устанавливаем соединение с модулем */
        err = LTR24_Open(&hltr24, par.addr, LTRD_PORT_DEFAULT, par.serial, par.slot);
        if (err!=LTR_OK) {
            fprintf(stderr, "Не удалось установить связь с модулем. Ошибка %d (%s)\n",
                    err, LTR24_GetErrorString(err));
        } else {
            INT close_err;
            err = LTR24_GetConfig(&hltr24);
            if (err!=LTR_OK) {
                fprintf(stderr, "Не удалось прочитать информацию о модуле из Flash-памяти. Ошибка %d (%s)\n",
                        err, LTR24_GetErrorString(err));
            } else {
                /* Выводим прочитанную информацию о модуле */
                printf("Модуль открыт успешно! Информация о модуле: \n");
                printf("  Название модуля    = %s\n", hltr24.ModuleInfo.Name);
                printf("  Серийный номер     = %s\n", hltr24.ModuleInfo.Serial);
                printf("  Версия PLD         = %d\n", hltr24.ModuleInfo.VerPLD);
                printf("  Поддержка ICP      = %s\n", hltr24.ModuleInfo.SupportICP ? "Есть" : "Нет");
                fflush(stdout);
            }

            /* Настройка модуля */
            hltr24.DataFmt = LTR24_FORMAT_20;
            hltr24.ADCFreqCode =  LTR24_FREQ_CODE;
            hltr24.ISrcValue = LTR24_I_SRC_VALUE_2_86;
            hltr24.TestMode = FALSE;
            /* Настройка режимов каналов */
            /*for (ch_idx = 0; ch_idx < LTR24_CHANNEL_CNT; ch_idx++) {
                hltr24.ChannelMode[ch_idx].Enable = FALSE;
            }*/

            for (ch_idx = 0; ch_idx < EN_CHANNELS_CNT; ch_idx++) {
                int ch_num = en_channels_nums[ch_idx];
                hltr24.ChannelMode[ch_num].Enable = TRUE;
                hltr24.ChannelMode[ch_num].ICPMode = TRUE;
                hltr24.ChannelMode[ch_num].Range = LTR24_ICP_RANGE_5;
#ifdef LPW25_EN_SENS_ROUT
                hltr24.ChannelMode[ch_num].SensorROut = (hltr24.ISrcValue == LTR24_I_SRC_VALUE_2_86) ?
                            LPW25_DEFAULT_ROUT_ISRC_2_86 : LPW25_DEFAULT_ROUT_ISRC_10;
#else
                hltr24.ChannelMode[ch_num].SensorROut = 0;
#endif
            }

            err = LTR24_SetADC(&hltr24);
            if (err!=LTR_OK) {
                fprintf(stderr, "Не удалось установить настройки АЦП: Ошибка %d (%s)\n",
                        err, LTR24_GetErrorString(err));
            } else {
                /* после SetADC() обновляется поле AdcFreq. Становится равной действительной
                 * установленной частоте */
                printf("Настройки АЦП установленны успешно. Частота = %.2f Гц\n",
                        hltr24.ADCFreq);

                for (ch_idx = 0; ch_idx < EN_CHANNELS_CNT; ch_idx++) {
                    lpw25[ch_idx].Fd = hltr24.ADCFreq;
                    lpw25[ch_idx].Sens = lpw_sens[ch_idx];
                }
            }

            if (err==LTR_OK) {
                DWORD recvd_blocks=0;
                INT recv_data_ch_cnt = hltr24.ADCFreq * RECV_TIME_MS /1000 + 0.5;
                INT recv_data_cnt = recv_data_ch_cnt * EN_CHANNELS_CNT;
                /* В 24-битном формате каждому отсчету соответствует два слова от модуля,
                   а в 20-битном - одно */
                INT   recv_wrd_cnt = recv_data_cnt*(hltr24.DataFmt == LTR24_FORMAT_20 ? 1 : 2);
                DWORD  *rbuf = (DWORD*)malloc(recv_wrd_cnt*sizeof(rbuf[0]));
                double *data = (double *)malloc(recv_data_cnt*sizeof(data[0]));
                double *ch_data = (double *)malloc(recv_data_ch_cnt * sizeof(ch_data[0]));
                BOOL   *ovlds = (BOOL *)malloc(recv_data_cnt*sizeof(ovlds[0]));

                if ((rbuf == NULL) || (data == NULL) || (ovlds == NULL) || (ch_data == NULL)) {
                    fprintf(stderr, "Ошибка выделения памяти!\n");
                    err = LTR_ERROR_MEMORY_ALLOC;
                }

                if (err==LTR_OK) {
                    /* Запуск сбора данных */
                    err=LTR24_Start(&hltr24);
                    if (err!=LTR_OK) {
                        fprintf(stderr, "Не удалось запустить сбор данных! Ошибка %d (%s)\n",
                                err, LTR24_GetErrorString(err));
                    }
                }

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


                /* ведем сбор данных до возникновения ошибки или до
                 * запроса на завершение */
                while (!f_out && (err==LTR_OK)) {
                    INT recvd;
                    /* В таймауте учитываем время сбора запрашиваемого числа отсчетов */
                    DWORD tout = RECV_TOUT + RECV_TIME_MS;

                    /* Прием данных от модуля.  */
                    recvd = LTR24_Recv(&hltr24, rbuf, NULL, recv_wrd_cnt, tout);

                    /* Значение меньше нуля соответствуют коду ошибки */
                    if (recvd<0) {
                        err = recvd;
                        fprintf(stderr, "Ошибка приема данных. Ошибка %d:%s\n",
                                err, LTR24_GetErrorString(err));
                    } else if (recvd!=recv_wrd_cnt) {
                        fprintf(stderr, "Принято недостаточно данных. Запрашивали %d, приняли %d\n",
                                recv_wrd_cnt, recvd);
                        err = LTR_ERROR_RECV_INSUFFICIENT_DATA;
                    } else {
                        err = LTR24_ProcessData(&hltr24, rbuf, data, &recvd,
                                                LTR24_PROC_FLAG_VOLT
                                                | LTR24_PROC_FLAG_CALIBR
                        #ifdef LTR_AFC_COR_FLAG
                                                | LTR_AFC_COR_FLAG
                        #endif
                        #ifdef EN_LTR_PHASE_COR
                                                | LTR24_PROC_FLAG_ICP_PHASE_COR
                        #endif
                                                , ovlds);
                        if (err!=LTR_OK) {
                            fprintf(stderr, "Ошибка обработки данных. Ошибка %d:%s\n",
                                    err, LTR24_GetErrorString(err));
                        } else {
                            recvd_blocks++;
                            /* выводим по первому слову на канал */
                            printf("Блок %4d: ", recvd_blocks);

                            for (ch_idx=0; ch_idx < EN_CHANNELS_CNT; ch_idx++) {

                                DWORD i;
                                DWORD ch_data_size = (DWORD)recvd / EN_CHANNELS_CNT;

                                for (i=0; i <  ch_data_size; i++) {
                                    double val = data[i*EN_CHANNELS_CNT + ch_idx];
                                    ch_data[i] = val;
                                }

                                process_ch_data(ch_data, (int)ch_data_size, hltr24.ADCFreq,
                                                &ch_results[ch_idx]);
                            }


                            for (ch_idx=0; ch_idx < EN_CHANNELS_CNT; ch_idx++) {
                                int ref_idx = ref_channels_idx[ch_idx];
                                printf("Канал %d\n", ch_idx);
                                print_results(&ch_results[ch_idx],
                                                  ((ref_idx >= 0) && (ref_idx != ch_idx)) ?
                                                      &ch_results[ref_idx] : NULL);
                                printf("\n");
                            }
                            fflush(stdout);

                        }
                    }

                    f_out = 1;

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




                /* по завершению останавливаем сбор, если был запущен */
                if (hltr24.Run) {
                    INT stop_err = LTR24_Stop(&hltr24);
                    if (stop_err!=LTR_OK) {
                        fprintf(stderr, "Не удалось остановить сбор данных. Ошибка %d:%s\n",
                                stop_err, LTR24_GetErrorString(stop_err));
                        if (err==LTR_OK)
                            err = stop_err;
                    } else {
                        printf("Сбор остановлен успешно.\n");
                    }
                }


                free(rbuf);
                free(data);
                free(ovlds);
            }

            /* закрываем связь с модулем */
            close_err = LTR24_Close(&hltr24);
            if (close_err!=LTR_OK) {
                fprintf(stderr, "Не удалось закрыть связь с модулем. Ошибка %d:%s\n",
                        close_err, LTR24_GetErrorString(close_err));
                if (err==LTR_OK)
                    err = close_err;
            } else {
                printf("Связь с модулем успешно закрыта.\n");
            }
        }
    }

    for (ch_idx = 0; ch_idx < EN_CHANNELS_CNT; ch_idx++) {
        LPW25_ProcessStop(&lpw25[ch_idx]);
    }

    return err;
}

