#include "BIQUADQ31.h"
#include <stdio.h>
#include "Error.h"


#define SNR_THRESHOLD 115

#define ABS_ERROR_Q31 ((q31_t)1000)

#define SNR_32x64_THRESHOLD 140

#define ABS_32x64_ERROR_Q31 ((q31_t)25)

static void checkInnerTail(q31_t *b)
{
    ASSERT_TRUE(b[0] == 0);
    ASSERT_TRUE(b[1] == 0);
    ASSERT_TRUE(b[2] == 0);
    ASSERT_TRUE(b[3] == 0);
}

    void BIQUADQ31::test_biquad_cascade_df1()
    {


        q31_t *statep = state.ptr();
        const q31_t *coefsp = coefs.ptr();
        
        const q31_t *inputp = inputs.ptr();
        q31_t *outp = output.ptr();

        int blockSize;

        

        /*

        Python script is generating different tests with
        different blockSize and numTaps.

        We loop on those configs.

        */
        
           blockSize = inputs.nbSamples() >> 1;

           /*

           The filter is initialized with the coefs, blockSize and numTaps.

           */

           arm_biquad_cascade_df1_init_q31(&this->S,3,coefsp,statep,2);


           /*
           
           Python script is filtering a 2*blockSize number of samples.
           We do the same filtering in two pass to check (indirectly that
           the state management of the fir is working.)

           */

           arm_biquad_cascade_df1_q31(&this->S,inputp,outp,blockSize);
           outp += blockSize;
           
           inputp += blockSize;
           arm_biquad_cascade_df1_q31(&this->S,inputp,outp,blockSize);
           outp += blockSize;




           ASSERT_EMPTY_TAIL(output);

           ASSERT_SNR(output,ref,(q31_t)SNR_THRESHOLD);

           ASSERT_NEAR_EQ(output,ref,ABS_ERROR_Q31);
  

    } 

    void BIQUADQ31::test_biquad_cascade_df1_32x64()
    {
        q63_t *statep = state64.ptr();
        const q31_t *coefsp = coefs.ptr();
        
        q31_t *inputp = inputs.ptr();
        q31_t *outp = output.ptr();

        int blockSize;

        

        /*

        Python script is generating different tests with
        different blockSize and numTaps.

        We loop on those configs.

        */
        
           blockSize = inputs.nbSamples() >> 1;

           /*

           The filter is initialized with the coefs, blockSize and numTaps.

           */

           arm_biquad_cas_df1_32x64_init_q31(&this->S32x64,3,coefsp,statep,2);


           /*
           
           Python script is filtering a 2*blockSize number of samples.
           We do the same filtering in two pass to check (indirectly that
           the state management of the fir is working.)

           */
#if 0
           arm_biquad_cas_df1_32x64_q31(&this->S32x64,inputp,outp,blockSize);
           outp += blockSize;
           
           inputp += blockSize;
           arm_biquad_cas_df1_32x64_q31(&this->S32x64,inputp,outp,blockSize);
           outp += blockSize;

#else
           int delta=1;
           int k;
           for(k=0;k + delta <2*blockSize ; k+=delta)
           {
             arm_biquad_cas_df1_32x64_q31(&this->S32x64,inputp,outp,delta);
             outp += delta;
             checkInnerTail(outp);
           
             inputp += delta;
           }
           if (k < 2*blockSize)
           {
             delta = 2*blockSize - k;
             arm_biquad_cas_df1_32x64_q31(&this->S32x64,inputp,outp,delta);
             outp += delta;
             checkInnerTail(outp);
           
             inputp += delta;
           }
#endif


           ASSERT_EMPTY_TAIL(output);

           ASSERT_SNR(output,ref,(q31_t)SNR_32x64_THRESHOLD);

           ASSERT_NEAR_EQ(output,ref,ABS_32x64_ERROR_Q31);
  
    }

 
    void BIQUADQ31::setUp(Testing::testID_t id,std::vector<Testing::param_t>& params,Client::PatternMgr *mgr)
    {
      
       (void)params;
       switch(id)
       {
        case BIQUADQ31::TEST_BIQUAD_CASCADE_DF1_1:
          /* max 4 * nbTaps as generated by Python script */
          /* Same OUTID is reused. So linked to same output file. If it is dumped
             it may overwrite the output
          */
          state.create(32,BIQUADQ31::OUT_Q31_ID,mgr);
          
        break;

        case BIQUADQ31::TEST_BIQUAD_CASCADE_DF1_32X64_2:
          state64.create(32,BIQUADQ31::STATE_Q64_ID,mgr);
        break;

       }
      
       inputs.reload(BIQUADQ31::BIQUADINPUTS_Q31_ID,mgr);
       coefs.reload(BIQUADQ31::BIQUADCOEFS_Q31_ID,mgr);
       ref.reload(BIQUADQ31::BIQUADREFS_Q31_ID,mgr);
       output.create(ref.nbSamples(),BIQUADQ31::OUT_Q31_ID,mgr);

       
    }

    void BIQUADQ31::tearDown(Testing::testID_t id,Client::PatternMgr *mgr)
    {
        (void)id;
        output.dump(mgr);
    }