//#define SIMULATE

#ifdef SIMULATE
    #include <stdio.h>
#endif
#include <stdint.h>
#include "signalProcessing/include/signal_path.h"
#define CSS_CMD                         0xC00004

// Define the specific interrupt that the cm3 will trigger when it wants
// us to perform a calculation
#define CSS_CMD_0                       (1<<0)
#define CSS_CMD_1                       (1<<1)

// Define the shared memory between the ARM and LPDSP processors
#define INPUT_PORT0_ADD 0x800000  // TODO: if BLOCK_LEN >1 is used, the data is interleaved: ch0ch1, ch0ch1 ....
//#define INPUT_PORT1_ADD INPUT_PORT0_ADD + 2 //DMB
#define OUTPUT_PORT_ADD (INPUT_PORT0_ADD + 16) // 2* for 2 channels

// Define the interrupt register to notify the ARM of a completed operation
volatile static unsigned char chess_storage(DMIO:CSS_CMD) CssCmdGen;

// handling requests from the CM3 is activated
static volatile int actionRequired;

static SingleSignalPath cSensorSignal;
static SingleSignalPath accSensorSignal;

#if BLOCK_LEN == 1
static volatile int16_t chess_storage(DMB:INPUT_PORT0_ADD) intputPort[4]; //TODO: if BLOCK_LEN >1 is used, the data is interleaved: ch0ch1, ch0ch1 .... chess_storage(DMA % alignof(int)) ?
//static volatile int16_t chess_storage(DMB:INPUT_PORT1_ADD) intputPort1[BLOCK_LEN];
static volatile int16_t chess_storage(DMB:OUTPUT_PORT_ADD) outputPort[4];

static volatile int16_t chess_storage(DMB) *inPtr0;
static volatile int16_t chess_storage(DMB) *inPtr1;
static volatile int16_t chess_storage(DMB) *outPtr;
static volatile int16_t chess_storage(DMB) sample;
static volatile int16_t chess_storage(DMB) *sample_ptr;

#else
static int16_t chess_storage(DMA) intputPort[BLOCK_LEN]; //chess_storage(DMA:INPUT_PORT_ADD) TODO: volatile?  chess_storage(DMA % alignof(int))
//static int16_t chess_storage(DMA) intputPort1[BLOCK_LEN]; //chess_storage(DMA:INPUT_PORT_ADD)
static int16_t chess_storage(DMB) outputPort[BLOCK_LEN]; // chess_storage(DMB:OUTPUT_PORT_ADD) TODO: determine output port add
#endif


extern "C" void isr0() property (isr) {
	// raise the flag indicating something needs to be processed
	actionRequired = 1;
	}

#ifdef __chess__
extern "C"
#endif
int main(void) {
    static OutputMode mode = OUTPUT_MODE_FIR_LMS;

    // Initialize the signal path
    // Initialize the csensor signal subpath
    // Instanciate the signal path state structs

    // Deactivate preemphasis filter by initializing with coefficients {1., 0., 0., 0., 0.}
    // biquad filter coefficients - off
    double b0[5]={0.75, 0., 0., 0., 0.};
    double b1[5]={0.75, 0., 0., 0., 0.};
    int N_lms_fir_coeffs = MAX_FIR_COEFFS; // always test with max coeffs

    init(
        &cSensorSignal, &accSensorSignal,
        //&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs,
        b0,
        b1,
        2,      // sample delay
        2,
        0.9,     // weight
        0.9,
        0.01,   // lms learning rate
        N_lms_fir_coeffs // Numer of lms fir coefficients
    );

    if (mode == OUTPUT_MODE_FIR){ //FIR filter mit fixen coeffizienten wenn nicht adaptiv
        for (int i=0; i<N_lms_fir_coeffs; i++){
            #ifdef LPDSP16
            ptr_fir_lms_coeffs.ptr_start[i] = ((pow(2, 15)-1) /N_lms_fir_coeffs);
            #else
            ptr_fir_lms_coeffs.ptr_start[i] = ((pow(2, 31)-1) /N_lms_fir_coeffs);
            #endif 
        }
    }

    #ifdef SIMULATE // use the simulator with file I/O
        FILE *fp1 = fopen("./test/testdata/input/chirp_disturber.txt", "r");
        FILE *fp2 = fopen("./test/testdata/input/disturber.txt", "r");
        FILE *fp3 = fopen("./test/testdata/output/out_simulated.txt", "w");

        int d0, d1;

        while (!(feof(fp1) || feof(fp2))){

            for (int i=0; i<BLOCK_LEN; i++){
                fscanf(fp1, "%d", &d0); //load blocks
                fscanf(fp2, "%d", &d1);
                intputPort[i] = (int16_t) d0;
                intputPort[i+1] = (int16_t) d1;
            }

            calc(
                &cSensorSignal, &accSensorSignal,
                //&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs,
                mode,
                &intputPort[0],
                &intputPort[1],
                outputPort
                );

            for (int i=0; i<BLOCK_LEN; i++){
                fprintf(fp3, "%d\n", outputPort[i]);
            }
        }
        fclose(fp1);
        fclose(fp2);
        fclose(fp3);

     #else // how its done in hw
        // enable the interrupts
        enable_interrupts();
        outPtr = &outputPort[1]; // start with second half of buffer
        sample_ptr = &sample;
               
        /*Signalprocessing, called by an interrupt*/
        actionRequired = 0;
        while (1){
            CssCmdGen = CSS_CMD_1; // indicate going to sleep to the cm3
            core_halt();
            if (actionRequired == 1) {
                CssCmdGen = CSS_CMD_0; // indicate wakeup to the cm3
                actionRequired = 0;     

                outPtr = cyclic_add(outPtr, 2, outputPort, 4);
                *outPtr = *sample_ptr;
                calc(&cSensorSignal, &accSensorSignal, mode, &intputPort[1], &intputPort[0], sample_ptr);
            }
        }
    #endif
}