#ifndef FSW_PROCESSING_H_INCLUDED
#define FSW_PROCESSING_H_INCLUDED

#include <rtems.h>
#include <grspw.h>
#include <math.h>
#include <stdlib.h> // abs() is in the stdlib
#include <stdio.h>  // printf()
#include <math.h>
#include <grlib_regs.h>

#include "fsw_params.h"
#include "fsw_spacewire.h"

typedef struct ring_node_asm
{
    struct ring_node_asm *next;
    float  matrix[ TOTAL_SIZE_SM ];
    unsigned int status;
} ring_node_asm;

typedef struct
{
    unsigned char targetLogicalAddress;
    unsigned char protocolIdentifier;
    unsigned char reserved;
    unsigned char userApplication;
    unsigned char packetID[2];
    unsigned char packetSequenceControl[2];
    unsigned char packetLength[2];
    // DATA FIELD HEADER
    unsigned char spare1_pusVersion_spare2;
    unsigned char serviceType;
    unsigned char serviceSubType;
    unsigned char destinationID;
    unsigned char time[6];
    // AUXILIARY HEADER
    unsigned char sid;
    unsigned char biaStatusInfo;
    unsigned char acquisitionTime[6];
    unsigned char pa_lfr_bp_blk_nr[2];
    // SOURCE DATA
    unsigned char data[ 30 * 22 ];   // MAX size is 22 * 30 [TM_LFR_SCIENCE_BURST_BP2_F1]
} bp_packet;

typedef struct
{
    Header_TM_LFR_SCIENCE_BP_with_spare_t header;
    unsigned char data[ 9 * 13 ];   // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
} bp_packet_with_spare;

typedef struct
{
    ring_node_asm *norm;
    ring_node_asm *burst_sbm;
    rtems_event_set event;
    unsigned int coarseTimeNORM;
    unsigned int fineTimeNORM;
    unsigned int coarseTimeSBM;
    unsigned int fineTimeSBM;
} asm_msg;

extern volatile int sm_f0[ ];
extern volatile int sm_f1[ ];
extern volatile int sm_f2[ ];

// parameters
extern struct param_local_str param_local;

// registers
extern time_management_regs_t *time_management_regs;
extern volatile spectral_matrix_regs_t *spectral_matrix_regs;

extern rtems_name  misc_name[5];
extern rtems_id    Task_id[20];         /* array of task ids */

//
ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
// ISR
rtems_isr spectral_matrices_isr( rtems_vector_number vector );
rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector );

//******************
// Spectral Matrices
void reset_nb_sm( void );
// SM
void SM_init_rings( void );
void SM_reset_current_ring_nodes( void );
// ASM
void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );

//*****************
// Basic Parameters

void BP_reset_current_ring_nodes( void );
void BP_init_header(bp_packet *header,
                     unsigned int apid, unsigned char sid,
                     unsigned int packetLength , unsigned char blkNr);
void BP_init_header_with_spare( Header_TM_LFR_SCIENCE_BP_with_spare_t *header,
                                unsigned int apid, unsigned char sid,
                                unsigned int packetLength, unsigned char blkNr );
void BP_send( char *data,
              rtems_id queue_id ,
              unsigned int nbBytesToSend , unsigned int sid );

//******************
// general functions
void reset_sm_status( void );
void reset_spectral_matrix_regs( void );
void set_time(unsigned char *time, unsigned char *timeInBuffer );
unsigned long long int get_acquisition_time( unsigned char *timePtr );
unsigned char getSID( rtems_event_set event );

extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );

//***************************************
// DEFINITIONS OF STATIC INLINE FUNCTIONS
static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
                              ring_node *ring_node_tab[],
                              unsigned int nbAverageNORM, unsigned int nbAverageSBM,
                              asm_msg *msgForMATR );
static inline void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
                                     ring_node *ring_node_tab[],
                                     unsigned int nbAverageNORM, unsigned int nbAverageSBM );
static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
                               float divider );
static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
                                  float divider,
                                  unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);

void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
                 ring_node *ring_node_tab[],
                 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
                 asm_msg *msgForMATR )
{
    float sum;
    unsigned int i;

    for(i=0; i<TOTAL_SIZE_SM; i++)
    {
        sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
                + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
                + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
                + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
                + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
                + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
                + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
                + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];

        if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
        {
            averaged_spec_mat_NORM[ i ] = sum;
            averaged_spec_mat_SBM[  i ] = sum;
            msgForMATR->coarseTimeNORM  = ring_node_tab[0]->coarseTime;
            msgForMATR->fineTimeNORM    = ring_node_tab[0]->fineTime;
            msgForMATR->coarseTimeSBM   = ring_node_tab[0]->coarseTime;
            msgForMATR->fineTimeSBM     = ring_node_tab[0]->fineTime;
        }
        else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
        {
            averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[  i ] + sum );
            averaged_spec_mat_SBM[  i ] = ( averaged_spec_mat_SBM[   i ] + sum );
        }
        else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
        {
            averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
            averaged_spec_mat_SBM[  i ] = sum;
            msgForMATR->coarseTimeSBM   = ring_node_tab[0]->coarseTime;
            msgForMATR->fineTimeSBM     = ring_node_tab[0]->fineTime;
        }
        else
        {
            PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
        }
    }
}

void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
                  ring_node *ring_node_tab[],
                  unsigned int nbAverageNORM, unsigned int nbAverageSBM )
{
    float sum;
    unsigned int i;

    for(i=0; i<TOTAL_SIZE_SM; i++)
    {
        sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ];

        if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
        {
            averaged_spec_mat_NORM[ i ] = sum;
            averaged_spec_mat_SBM[  i ] = sum;
        }
        else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
        {
            averaged_spec_mat_NORM[ i ] = sum;
            averaged_spec_mat_SBM[  i ] = sum;
        }
        else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
        {
            averaged_spec_mat_NORM[ i ] = sum;
            averaged_spec_mat_SBM[  i ] = sum;
        }
        else
        {
            PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
        }
    }
}

void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
{
    int frequencyBin;
    int asmComponent;
    unsigned int offsetASM;
    unsigned int offsetASMReorganized;

    // BUILD DATA
    for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
    {
        for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
        {
            offsetASMReorganized =
                    frequencyBin * NB_VALUES_PER_SM
                    + asmComponent;
            offsetASM            =
                    asmComponent * NB_BINS_PER_SM
                    + frequencyBin;
            averaged_spec_mat_reorganized[offsetASMReorganized  ] =
                    averaged_spec_mat[ offsetASM ] / divider;
        }
    }
}

void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
                                 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
{
    int frequencyBin;
    int asmComponent;
    int offsetASM;
    int offsetCompressed;
    int k;

    // BUILD DATA
    for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
    {
        for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
        {
            offsetCompressed =  // NO TIME OFFSET
                    frequencyBin * NB_VALUES_PER_SM
                    + asmComponent;
            offsetASM =         // NO TIME OFFSET
                    asmComponent * NB_BINS_PER_SM
                    + ASMIndexStart
                    + frequencyBin * nbBinsToAverage;
            compressed_spec_mat[ offsetCompressed ] = 0;
            for ( k = 0; k < nbBinsToAverage; k++ )
            {
                compressed_spec_mat[offsetCompressed ] =
                        ( compressed_spec_mat[ offsetCompressed ]
                        + averaged_spec_mat[ offsetASM + k ] );
            }
            compressed_spec_mat[ offsetCompressed ] =
                    compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
        }
    }
}

void ASM_convert( volatile float *input_matrix, char *output_matrix)
{
    unsigned int frequencyBin;
    unsigned int asmComponent;
    char * pt_char_input;
    char * pt_char_output;
    unsigned int offsetInput;
    unsigned int offsetOutput;

    pt_char_input = (char*) &input_matrix;
    pt_char_output = (char*) &output_matrix;

    // convert all other data
    for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
    {
        for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
        {
            offsetInput  =       (frequencyBin*NB_VALUES_PER_SM) + asmComponent   ;
            offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
            pt_char_input =  (char*) &input_matrix [ offsetInput  ];
            pt_char_output = (char*) &output_matrix[ offsetOutput ];
            pt_char_output[0] = pt_char_input[0];   // bits 31 downto 24 of the float
            pt_char_output[1] = pt_char_input[1];   // bits 23 downto 16 of the float
        }
    }
}

#endif // FSW_PROCESSING_H_INCLUDED