#include <main.h>
#include <math.h>
#include "cwt.h"
#include "string.h"
#include "stdio.h"
#include "MyBandFitter.h"
#include "MyAlgorithm.h"

extern int FitterCount_CheckBed;
extern char FitterCollectDoneFlag_CheckBed;

extern float ampSum[HEART_ARRAY_LENGTH];
extern float ampSumRr[BREATH_ARRAY_LENGTH];
/************************ PRIVATE ************************/

void convolve(const float Signal[/* SignalLen */], int SignalLen,
              const float Kernel[/* KernelLen */], int KernelLen,
              float Result[/* SignalLen + KernelLen - 1 */])
{
    int n;
    for (n = 0; n < SignalLen + KernelLen - 1; n++)
    {
        int kmin, kmax, k;
        Result[n] = 0;
        kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0;
        kmax = (n < SignalLen - 1) ? n : SignalLen - 1;
        for (k = kmin; k <= kmax; k++)
        {
            Result[n] += Signal[k] * Kernel[n - k];
        }
    }
}

/************************ PUBLIC ************************/

int cwt_hr(float *s, int n, float amp[][n])
{
    /* variables for computing cwt */
    float a;
    double stepp = 0.009775171065493637;
    for (int dy = 943; dy < 988; dy++)
    {
        a = (2 * 0.7 * 1024) / (1024 - dy);
        int len_jj = ceil(10 * a) + 1;
        // int jj[len_jj];
        int *jj = (int *)malloc(len_jj * sizeof(int));
        for (int i = 0; i < len_jj; i++)
        {
            jj[i] = floor(i / (a * stepp));
        }
        int len_jjj = len_jj;
        if (jj[len_jj - 1] >= 1024)
        {
            for (int i = len_jj - 1; i >= 0; i--)
            {
                if (jj[i] < 1024)
                {
                    len_jjj = i + 1;
                    break;
                }
            }
        }
        free(jj);
        // int jjj[len_jjj];
        // float int_psi_scale_real[len_jjj];
        // float int_psi_scale_imag[len_jjj];
        int *jjj = (int *)malloc(len_jjj * sizeof(int));
        float *int_psi_scale_real = (float *)malloc(len_jjj * sizeof(float));
        float *int_psi_scale_imag = (float *)malloc(len_jjj * sizeof(float));
        for (int i = 0; i < len_jjj; i++)
        {
            jjj[i] = jj[i];
            int_psi_scale_real[len_jjj - 1 - i] = int_psi_real[jjj[i]];
            int_psi_scale_imag[len_jjj - 1 - i] = int_psi_imag[jjj[i]];
        }
        float result_r[(n + len_jjj - 1)];
        float result_i[(n + len_jjj - 1)];
        if (n > len_jjj)
        {
            convolve(s, n, int_psi_scale_real, len_jjj, result_r);
            convolve(s, n, int_psi_scale_imag, len_jjj, result_i);
        }
        else
        {
            convolve(int_psi_scale_real, len_jjj, s, n, result_r);
            convolve(int_psi_scale_imag, len_jjj, s, n, result_i);
        }
        free(jjj);
        free(int_psi_scale_real);
        free(int_psi_scale_imag);
        float SqrtValue_a = 0.0f;  //
        float PowSqrtValue = 0.0f; //
        // float diff_conv_r[n + len_jjj - 2];
        // float diff_conv_i[n + len_jjj - 2];
        float *diff_conv_r = (float *)malloc((n + len_jjj - 2) * sizeof(float));
        float *diff_conv_i = (float *)malloc((n + len_jjj - 2) * sizeof(float));
        arm_sqrt_f32(a, &(SqrtValue_a));
        for (int i = 1; i < (n + len_jjj - 1); i++)
        {
            diff_conv_r[i - 1] = (result_r[i] - result_r[i - 1]) * (-1 * SqrtValue_a);
            diff_conv_i[i - 1] = (result_i[i] - result_i[i - 1]) * (-1 * SqrtValue_a);
        }
        float cut_dim = (float)(len_jjj - 2) / 2;
        for (int i = floor(cut_dim); i < (n + len_jjj - 2 - ceil(cut_dim)); i++)
        {
            arm_sqrt_f32(pow(diff_conv_r[i], 2.0) + pow(diff_conv_i[i], 2.0), &PowSqrtValue);
            // amp[dy - 942][i - (int)floor(cut_dim)] = PowSqrtValue;
            ampSum[dy - 943] += PowSqrtValue;
        }
        free(diff_conv_r);
        free(diff_conv_i);
        get_optical_power();
        get_check_result();
        if (FitterCollectDoneFlag_CheckBed)
        {
            FitterCollectDoneFlag_CheckBed = 0;
            breathInBedCheck(FitterOPMData_CheckBed);
            memcpy(FitterOPMData_CheckBed, FitterOPMData_CheckBed + 50,
                   sizeof(float32_t) * (LENGTH_SAMPLES_IN_OFF_BED - 50));
            FitterCount_CheckBed = LENGTH_SAMPLES_IN_OFF_BED - 50;
        }
    }
    return 0;
}

int cwt_rr(float *s, int n, float amp[][n])
{
    /* variables for computing cwt */
    float a = 0.0f;
    double stepp = 0.009775171065493637;
    for (int dy = 1004; dy < 1016; dy++)
    {
        a = (2 * 0.7 * 1024) / (1024 - dy);
        int len_jj = ceil(10 * a) + 1;

        // int jj[len_jj];
        int *jj = (int *)malloc(len_jj * sizeof(int));
        for (int i = 0; i < len_jj; i++)
        {
            jj[i] = floor(i / (a * stepp));
        }
        int len_jjj = len_jj;
        if (jj[len_jj - 1] >= 1024)
        {
            for (int i = len_jj - 1; i >= 0; i--)
            {
                if (jj[i] < 1024)
                {
                    len_jjj = i + 1;
                    break;
                }
            }
        }
        free(jj);

        // int jjj[len_jjj];
        // float int_psi_scale_real[len_jjj];
        // float int_psi_scale_imag[len_jjj];

        int *jjj = (int *)malloc(len_jjj * sizeof(int));
        float *int_psi_scale_real = (float *)malloc(len_jjj * sizeof(float));
        float *int_psi_scale_imag = (float *)malloc(len_jjj * sizeof(float));
        for (int i = 0; i < len_jjj; i++)
        {
            jjj[i] = jj[i];
            int_psi_scale_real[len_jjj - 1 - i] = int_psi_real[jjj[i]];
            int_psi_scale_imag[len_jjj - 1 - i] = int_psi_imag[jjj[i]];
        }
        float result_r[(n + len_jjj - 1)];
        float result_i[(n + len_jjj - 1)];
        if (n > len_jjj)
        {
            convolve(s, n, int_psi_scale_real, len_jjj, result_r);
            convolve(s, n, int_psi_scale_imag, len_jjj, result_i);
        }
        else
        {
            convolve(int_psi_scale_real, len_jjj, s, n, result_r);
            convolve(int_psi_scale_imag, len_jjj, s, n, result_i);
        }
        free(jjj);
        free(int_psi_scale_real);
        free(int_psi_scale_imag);

        float SqrtValue_a = 0.0f;  //
        float PowSqrtValue = 0.0f; //
        // float diff_conv_r[n + len_jjj - 2];
        // float diff_conv_i[n + len_jjj - 2];
        float *diff_conv_r = (float *)malloc((n + len_jjj - 2) * sizeof(float));
        float *diff_conv_i = (float *)malloc((n + len_jjj - 2) * sizeof(float));
        arm_sqrt_f32(a, &(SqrtValue_a));
        for (int i = 1; i < (n + len_jjj - 1); i++)
        {
            diff_conv_r[i - 1] = (result_r[i] - result_r[i - 1]) * (-1 * SqrtValue_a);
            diff_conv_i[i - 1] = (result_i[i] - result_i[i - 1]) * (-1 * SqrtValue_a);
        }
        float cut_dim = (float)(len_jjj - 2) / 2;
        for (int i = floor(cut_dim); i < (n + len_jjj - 2 - ceil(cut_dim)); i++)
        {
            arm_sqrt_f32(pow(diff_conv_r[i], 2.0) + pow(diff_conv_i[i], 2.0), &PowSqrtValue);
            ampSumRr[dy - 1004] += PowSqrtValue;
        }
        free(diff_conv_r);
        free(diff_conv_i);
        get_optical_power();
        get_check_result();
        // if (FitterCollectDoneFlag_CheckBed)
        // {
        //     FitterCount_CheckBed = 0;
        //     FitterCollectDoneFlag_CheckBed = 0;
        //     breathInBedCheck(FitterOPMData_CheckBed);
        // }
    }
    return 0;
}