/*Test enviornment for testing of the signal path on a generic platform (pc)*/
#include <stdio.h>
#include "../signalProcessing/include/signal_path.h"

int main(int argc, char* argv[]){

    /*Some environment related prints*/
    #ifdef DEBUG_PRINTS
        printf("The size of int in bytes is: %lu\n", sizeof(int));
        printf("The size of accum_t in bytes is: %lu\n", sizeof(accum_t));
    #endif

    static SingleSignalPath cSensorSignal;
    static SingleSignalPath accSensorSignal;
    //static LmsFilter lms;
    //int lms_fir_coeffs[MAX_FIR_COEFFS];

    static int16_t cSensor[BLOCK_LEN], accSensor[BLOCK_LEN];
    static int16_t output_port[BLOCK_LEN];

    // Deactivate preemphasis filter by initializing with coefficients {1., 0., 0., 0., 0.}
    // can not be modeled on a generic platform (pc)
    // biquad filter coefficients - off
    double b0[5]={1., 0., 0., 0., 0.};
    double b1[5]={1., 0., 0., 0., 0.};
    int N_lms_fir_coeffs = MAX_FIR_COEFFS;
    // get parameters from command line
    OutputMode mode = (OutputMode) (*argv[1] - '0'); // Convert char to int (only works for single digits)
    int pipe_input = (*argv[2] - '0');
    int return_coeffs = (*argv[3] - '0');

    init(
        &cSensorSignal, &accSensorSignal, 
        //&lms, lms_fir_coeffs,
        b0, 
        b1,
        0,      // sample delay
        0, 
        1.,     // weight
        1.,
        0.005,   // lms learning rate
        N_lms_fir_coeffs // Numer of lms fir coefficients
    );

    if (mode == OUTPUT_MODE_FIR){ //FIR filter mit fixen coeffizienten 
        for (int i=0; i<N_lms_fir_coeffs; i++){
            ptr_fir_lms_coeffs.ptr_start[i] = (int) ((pow(2,31)-1) / N_lms_fir_coeffs);
        }
    }

    /*Sample wise signalprocessing, called by an interrupt, use a for loop to simulate it*/
    FILE *fp1 = fopen("testdata/input/chirp_disturber.txt", "r");
    FILE *fp2 = fopen("testdata/input/disturber.txt", "r");
    FILE *fp3 = fopen("testdata/output/out_sim_generic.txt", "w");

    int loop_cnt=0;
    int keep_loopin=1;
    while (keep_loopin){
        // Load a new block from the input
        for (uint32_t i = 0; i < BLOCK_LEN; i++){ // process block wise
            if (pipe_input){ // input from pipe
                if ((scanf("%hd", &cSensor[i]) !=1) || (scanf("%hd", &accSensor[i])!=1)) {
                    keep_loopin=0;
                    break;
                }
            }
            else{// input from file
                if (feof(fp1)==1 || feof(fp2)==1){
                    keep_loopin=0;
                    break;
                }         
                fscanf(fp1, "%hd", &cSensor[i]);
                fscanf(fp2, "%hd", &accSensor[i]);
            }
        }
        loop_cnt++;
        calc(
            &cSensorSignal, &accSensorSignal, 
            //&lms, lms_fir_coeffs, 
            mode, cSensor, accSensor, output_port);

        
        if (pipe_input){ // output to pipe
            for (uint32_t i = 0; i < BLOCK_LEN; i++){ // return calculation results
                printf("%d,", output_port[i]); // output to stdout
                if (return_coeffs == 1){ // print the current coefficients
                    for (int i=0; i< N_lms_fir_coeffs; i++){
                        printf("%d,", ptr_fir_lms_coeffs.ptr_start[i]);
                    }
                    printf("\n");
                }
            }
        }
        else{
            for (uint32_t i = 0; i < BLOCK_LEN; i++){ // return calculation results
                printf("%d\n, ", output_port[i]); // output to stdout
                if (return_coeffs == 1){ // print the current coefficients
                    for (int i=0; i< N_lms_fir_coeffs; i++){
                        printf("%d,", ptr_fir_lms_coeffs.ptr_start[i]);
                    }
                    printf("\n");
                }
                fprintf(fp3, "%d\n", output_port[i]); // output to file
            }
        }
    }

    if (!pipe_input){
        fclose(fp1);
        fclose(fp2);
        fclose(fp3);
    }
    return 0;
}