/*
 * Copyright (c) 2019, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

/*
 * Copyright 2009 Shawn Stevenson
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:

 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
*/

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <memory.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>

#include "lstm_infer.h"
#include "psensors.h"
#include "motor_pdm_loc.h"
#include "motor_pdm.h"

#include <iostream>
#include <thread>
#include <chrono>

/* FIR initialization */
void firFloatInit( double *insamp, int len_delay_line)
{
    int i;
    for (i = 0; i < len_delay_line; i ++) {
        insamp[i] = 0.0;
    }
}

/* The FIR filter with downsampling by DRATION*/
void firFloat( double *coeffs, double *insamp, int filterLength,
               double *input,  int length,
               double *output)
{
    double acc;     /* accumulator for MACs */
    double *coeffp; /* pointer to coefficients */
    double *inputp; /* pointer to input samples */
    int n,j=0;
    int k;

    /* put the new samples at the high end of the buffer */
    memcpy( &insamp[filterLength - 1], input,
    length * sizeof(double) );
    /* apply the filter to each input sample */
    for ( n = 0; n < length; n=n+DRATIO ) {
        /* calculate output n */
        coeffp = coeffs;
        inputp = &insamp[filterLength - 1 + n];
        acc = 0;
        for ( k = 0; k < filterLength; k++ ) {
            acc += (*coeffp++) * (*inputp--);
        }
        output[j++] = acc;
    }
    /* shift input samples back in time for next time */
    memmove( &insamp[0], &insamp[length], (filterLength - 1) * sizeof(double) );
}

/* Send the buffered sensor data and PdM detection results downstream for Qt display */
int motor_pdm_get_data(int samples_count, double *samples_array)
{
    int rd_idx;
    /* When the number of sensor data points reach samples_count, send them for Qt display*/
    if(display_wr_data_cnt >= (display_rd_data_cnt + samples_count*DISPLAY_VECTOR)) {
        for (int i = 0; i < samples_count; i ++) {
            rd_idx = display_rd_data_cnt % DISPLAY_MAX_BUFFER;
            for (int j=0; j<DISPLAY_VECTOR; j++) {
                samples_array[DISPLAY_VECTOR*i + j] = display_data_ready_to_use[rd_idx + j];
            }
            display_rd_data_cnt += DISPLAY_VECTOR;
        }
        /* Do data overlapping to make the time serires move slowly */
        display_rd_data_cnt -= (127 * samples_count*DISPLAY_VECTOR) / 128;
        return samples_count;
    } else {
        return 0;
    }
}

/*****************************************************************************************
 * Main call flow for anomaly detection using RNN (LSTM)
 *
 * Step 1: read sensor data, including motor position and the two phase currents
 * Step 2: pre-process the sensor data: downsampling by 200 and lowpass filtering
 * Step 3: normalization for the input: phase currents at t[n]
 * Step 4: RNN prediction: predict the future output from the current input with LSTM
 * Step 5: de-normalization for the predicted output: predicted phase currents at t[n+1]
 * Step 6: calculate the predicator error by comparing the predicted phase current at
 *         t[n+1] with the actual phase currents at t[n+1].
 *         The beginning predication errors are for the normal scenario, and used for
 *         calibration to find the error threshold for future anomaly detection.
 *         After the calibration, it enters the anomaly detection mode:
 *         predication errors larger than the threahold (with hangover) are declared
 *         as anomalies
 *
 * Decimated sensor data and detection results are also recoreded for Qt display
 *****************************************************************************************/
extern pdmContext_t pdmContext;
#define max(a,b) (a > b ? a : b)

int motor_pdm_process(void)
{
    int size;
    /* Input sample array */
    double floatInput1[SAMPLES];
    double floatInput2[SAMPLES];
    double floatInput3[SAMPLES];
    /* Onput sample array after lowpass filtering and downsampling */
    double floatOutput1[SAMPLES/DRATIO];
    double floatOutput2[SAMPLES/DRATIO];
    double floatOutput3[SAMPLES/DRATIO];
    /* Delay line to hold input samples */
    double insampPH1[ BUFFER_LEN ];
    double insampPH2[ BUFFER_LEN ];
    double insampPH3[ BUFFER_LEN ];
    /* File storing the coefficients of the FIR filter */
    FILE *coeff_fid;

    double predict_threshold_err = 0.0;
    double predict1 = 0.0;
    double predict2 = 0.0;

    int    samples_counter = 0;
    int    restart_hangover_timer = 0;
    int    ad_detected = 0;
    int    pdmState = 0;

    int display_wr_idx = 0;

    /* For storing the incoming sensor data */
    float  samples_array[3*SAMPLES];

    /* Read decimating FIR filter coefficients from file */
    coeff_fid = fopen( "coeff.bin", "rb" );
    if ( coeff_fid == 0 ) {
        printf("\n...couldn't open coeff.bin file (Decimating FIR filter coefficients)\n");
        return -1;
    }
    fread(coeffs, sizeof(double), FIRFLT_LEN, coeff_fid );
    fclose( coeff_fid );

    /* Initialize LSTM layers */
    lstmSetup();

    /* Initialize the filter delay lines */
    firFloatInit(insampPH1, BUFFER_LEN);
    firFloatInit(insampPH2, BUFFER_LEN);
    firFloatInit(insampPH3, BUFFER_LEN);

    /* Start the UART reading and parsing thread. Parsed data are logged into a buffer */
    std::thread sensorStream (uart_stream_parser, 0);

    /* Processs loop */
    while(1) {
        /* Read samples from UART pipe */
        size = uart_get_data(SAMPLES, samples_array);

        if(size == SAMPLES) {  /* It means, at least this many SAMPLES are available */
            for(int i = 0; i < SAMPLES; i ++) {
                floatInput1[i] = (double)samples_array[3*i + 0];
                floatInput2[i] = (double)samples_array[3*i + 1];
                floatInput3[i] = (double)samples_array[3*i + 2];
            }
            /* Perform the filtering: one decimated sample generated */
            firFloat( coeffs, insampPH1, FIRFLT_LEN, floatInput1, SAMPLES, floatOutput1 );
            firFloat( coeffs, insampPH2, FIRFLT_LEN, floatInput2, SAMPLES, floatOutput2 );
            firFloat( coeffs, insampPH3, FIRFLT_LEN, floatInput3, SAMPLES, floatOutput3 );

            /* Writting too fast, do write side throttling */
            if(display_wr_data_cnt > (display_rd_data_cnt + DISPLAY_MAX_BUFFER - 16)) {
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
            }

            /* Write the decimated sensor data to the buffer array to prepare for the Qt display*/
            for (int i = 0; i < SAMPLES/DRATIO; i ++) {
                display_wr_idx = display_wr_data_cnt % DISPLAY_MAX_BUFFER;
                display_data_ready_to_use[display_wr_idx+0] = floatOutput1[i];
                display_data_ready_to_use[display_wr_idx+1] = floatOutput2[i];
                display_data_ready_to_use[display_wr_idx+2] = floatOutput3[i];
                display_wr_data_cnt += 3;
            }

            /* Calculate prediction error (using previously predicted samples) */
            double predict_error  = (floatOutput1[0] - predict1) * (floatOutput1[0] - predict1);
                   predict_error += (floatOutput2[0] - predict2) * (floatOutput2[0] - predict2);

            ad_detected = 0;

            if(samples_counter > START_FIND_THRESHOLD) {
                if(samples_counter < STOP_FIND_THRESHOLD) {
                    /* Calibration at the beginning to find the error threshold */
                    pdmState = 100;
                    predict_threshold_err = max(predict_error, predict_threshold_err);
                    restart_hangover_timer = 0;
                } else {
                    pdmState = -100;
                    /* Anomaly detection mode with hangover to increase robustness */
                    if(predict_error > pdmContext.relative_threshold * predict_threshold_err) {
                        restart_hangover_timer = samples_counter;
                    }
                    if((samples_counter - restart_hangover_timer) < HANGOVER_TIME) {
                        ad_detected = 1;
                    }
                }
            }

            /* Write the detection results to the buffer array to prepare for the Qt display*/
            display_wr_idx = display_wr_data_cnt % DISPLAY_MAX_BUFFER;
            display_data_ready_to_use[display_wr_idx+0] = predict_error;
            display_data_ready_to_use[display_wr_idx+1] = predict_threshold_err*pdmContext.relative_threshold + pdmState;
            display_data_ready_to_use[display_wr_idx+2] = ad_detected * predict_threshold_err *pdmContext.relative_threshold * 1.25;
            display_wr_data_cnt += 3;

            /* Normalize */
            double lstm_out1, lstm_in1 = (floatOutput1[0] - pdmContext.mu1) / pdmContext.sig1;
            double lstm_out2, lstm_in2 = (floatOutput2[0] - pdmContext.mu2) / pdmContext.sig2;
            /* Run LSTM network for prediction */
            runLstm(lstm_in1, lstm_in2, &lstm_out1, &lstm_out2);
            /* Denormalize */
            predict1 = lstm_out1 * pdmContext.sig1 + pdmContext.mu1;
            predict2 = lstm_out2 * pdmContext.sig2 + pdmContext.mu2;

            /* Increment the sample counter at the decimated rate */
            samples_counter ++;
        } else {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }
    return 0;
}