#include <machine.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <mathf.h>
#include <complex.h>
#include "r_cg_macrodriver.h"
#include "Config_CMT0.h"
#include "Config_TPU1.h"
#include "Config_RSPI0.h"
#include "Config_DMAC0.h"
#include "r_dsp_typedefs.h"
#include "r_dsp_types.h"
#include "r_dsp_transform.h"
#include "r_dsp_complex.h"

#define M_PI 3.14159265358979
#define FS 48000
#define AD_THRESHOLD 3

typedef struct{
  float ch0;
  float ch1;
} PCM_float;

// RAM(0x800000-)
#pragma section extra_ram
PCM_float pcm0[24576];
PCM_float pcm1[24576];
#pragma section

char toggle_switch;
short knob[3];

int input_index, output_index;
int offset;

void sound_effector_init(void)
{
  int n;

  for (n = 0; n < 24576; n++)
  {
    pcm0[n].ch0 = 0;
    pcm0[n].ch1 = 0;
    pcm1[n].ch0 = 0;
    pcm1[n].ch1 = 0;
  }

  toggle_switch = 0;

  knob[0] = 0;
  knob[1] = 0;
  knob[2] = 0;

  input_index = 0;
  output_index = 0 - 512 * 3 + 24576;
}

void sound_effector_start(void)
{
  int i;

  SYSTEM.PRCR.WORD = 0xA503;

  SYSTEM.SCKCR.BIT.PSTOP1 = 1;

  while (SYSTEM.SCKCR.BIT.PSTOP1 != 1)
  {
    nop();
  }

  MPC.PFBCR0.BIT.BCLKO = 1;

  SYSTEM.SCKCR.BIT.PSTOP1 = 0;

  while (SYSTEM.SCKCR.BIT.PSTOP1 != 0)
  {
    nop();
  }

  SYSTEM.PRCR.WORD = 0xA500;

  R_Config_RSPI0_Start();
  R_Config_TPU1_Start();
  R_Config_CMT0_Start();
  R_Config_DMAC0_Start();

  for (i = 0; i < 2000; i++)
  {
    nop(); // wait (>20.83us)
  }
}

void main(void)
{
  int n, offset = 0;
  short volume1 = 0, volume2 = 0, volume3 = 0;
  int i, j, k, m;
  float a;

  float w[1024];

  // FFT
  float x0[1024];
  float x1[1024];
  cplxf32_t X0[512];
  cplxf32_t X1[512];
  vector_t x0_vector = {1024, (void *)x0};
  vector_t x1_vector = {1024, (void *)x1};
  vector_t X0_vector = {512, (void *)X0};
  vector_t X1_vector = {512, (void *)X1};
  cplxf32_t fft_twiddles1[768]; // 32:24, 64:48, 128:96, 256:192, 512:384, 1024:768, 2048:1536
  uint32_t fft_bitrev1[240]; // 32:6, 64:12, 128:28, 256:56, 512:120, 1024:240, 2048:496
  r_dsp_fft_t h1 = {1024, 0, fft_twiddles1, fft_bitrev1, NULL, NULL};

  // amplitude spectrum
  float B[512];
  vector_t B_vector = {512, (void *)B};

  // IFFT
  float y[1024];
  cplxf32_t Y[512];
  vector_t y_vector = {1024, (void *)y};
  vector_t Y_vector = {512, (void *)Y};
  cplxf32_t fft_twiddles2[768]; // 32:24, 64:48, 128:96, 256:192, 512:384, 1024:768, 2048:1536
  uint32_t fft_bitrev2[240]; // 32:6, 64:12, 128:28, 256:56, 512:120, 1024:240, 2048:496
  r_dsp_fft_t h2 = {1024, 0, fft_twiddles2, fft_bitrev2, NULL, NULL};

  R_DSP_FFT_Init_f32cf32(&h1);
  R_DSP_FFT_Init_cf32f32(&h2);

  // Hanning window
  for (n = 0; n < 1024; n++)
  {
    w[n] = 0.5 - 0.5 * cosf(2.0 * M_PI * n / 1024.0);
  }

  PORTC.PDR.BIT.B0 = 1;

  sound_effector_init();
  sound_effector_start();

  while (1)
  {
    if (input_index % 512 == 0)
    {
      offset = input_index - 512 * 2;
      if (offset < 0)
      {
      	offset += 24576;
      }

      if (abs(volume1 - knob[0]) >= AD_THRESHOLD)
      {
        volume1 = knob[0];
      }
      if (abs(volume2 - knob[1]) >= AD_THRESHOLD)
      {
        volume2 = knob[1];
      }
      if (abs(volume3 - knob[2]) >= AD_THRESHOLD)
      {
        volume3 = knob[2];
      }

      if (toggle_switch == 1) // toggle switch off
      {
        PORTC.PODR.BIT.B0 = 0;

        for (n = 0; n < 1024; n++)
        {
          m = offset + n;
          if (m >= 24576)
          {
            m -= 24576;
          }
          x0[n] = pcm0[m].ch0 * w[n];
          x1[n] = pcm0[m].ch1 * w[n];
        }

        for (n = 0; n < 512; n++)
        {
          m = offset + n;
          if (m >= 24576)
          {
            m -= 24576;
          }
          pcm1[m].ch0 += x0[n];
          pcm1[m].ch1 += x1[n];
        }
        for (n = 512; n < 1024; n++)
        {
          m = offset + n;
          if (m >= 24576)
          {
            m -= 24576;
          }
          pcm1[m].ch0 = x0[n];
          pcm1[m].ch1 = x1[n];
        }
      }
      else // toggle switch on
      {
        PORTC.PODR.BIT.B0 = 1;

        for (n = 0; n < 1024; n++)
        {
          m = offset + n;
          if (m >= 24576)
          {
            m -= 24576;
          }
          x0[n] = pcm0[m].ch0 * w[n];
	}
	
	for (n = 0; n < 1024; n++)
        {
          m = offset + n;
          if (m >= 24576)
          {
            m -= 24576;
          }
          x1[n] = pcm0[m].ch1;
        }
	for (n = 0; n < 1024; n++)
        {
          m = offset + n - 1;
	  if (m >= 24576)
          {
            m -= 24576;
          }
          if (m < 0)
          {
            m += 24576;
          }
          x1[n] -= 0.98 * pcm0[m].ch1;
	  x1[n] *= w[n];
        }

        // FFT x[0-1024](real)) -> X[0-512](complex)
	R_DSP_FFT_f32cf32(&h1, &x0_vector, &X0_vector);
	R_DSP_FFT_f32cf32(&h1, &x1_vector, &X1_vector);

	// amplitude spectrum
	R_DSP_VecCplxMag_cf32f32(&X1_vector, &B_vector, 512);
	for (i = 0; i < 64; i++)
	{
	  j = 8 * i;	
	  a = 0.0;
	  for(k = 0; k < 8; k++)
	  {
	    a += B[j + k];
	  }
	  a /= 8.0;
	  for (k = 0; k < 8; k++)
	  {
	    B[j + k] = a;
	  }
	}
        B[0] = 0.0;

        // DFT filter
        for (k = 0; k < 512; k++)
        {
          Y[k].re = X0[k].re * B[k];
          Y[k].im = X0[k].im * B[k];
        }

        // IFFT Y[0-512](complex) -> y[0-1024](real)
	R_DSP_IFFT_CCS_cf32f32(&h2, &Y_vector, &y_vector);

        for (n = 0; n < 512; n++)
        {
          m = offset + n;
          if (m >= 24576)
          {
            m -= 24576;
          }
          pcm1[m].ch0 += y[n];
          pcm1[m].ch1 += y[n];
        }
        for (n = 512; n < 1024; n++)
        {
          m = offset + n;
          if (m >= 24576)
          {
            m -= 24576;
          }
          pcm1[m].ch0 = y[n];
          pcm1[m].ch1 = y[n];
        }
      }
    }
  }
}