#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"

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

typedef struct{
  float ch0;
} PCM_float;

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

char toggle_switch;
short knob[3];

int input_index, output_index;
int offset;

void IIR_resonator(float fc, float Q, float a[], float b[])
{
  fc = tanf(M_PI * fc) / (2.0 * M_PI);

  a[0] = 1.0 + 2.0 * M_PI * fc / Q + 4.0 * M_PI * M_PI * fc * fc;
  a[1] = (8.0 * M_PI * M_PI * fc * fc - 2.0) / a[0];
  a[2] = (1.0 - 2.0 * M_PI * fc / Q + 4.0 * M_PI * M_PI * fc * fc) / a[0];
  b[0] = 2.0 * M_PI * fc / Q / a[0];
  b[1] = 0.0;
  b[2] = -2.0 * M_PI * fc / Q / a[0];

  a[0] = 1.0;
}

void sound_effector_init(void)
{
  int n;

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

  toggle_switch = 0;

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

  input_index = 0;
  output_index = 0 - 128 * 2 + 49152;
}

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 m;
  float fc, Q, a[3], b[3], s0[128 + 2], s1[128 + 2];

  PORTC.PDR.BIT.B0 = 1;

  sound_effector_init();
  sound_effector_start();

  while (1)
  {
    if (input_index % 128 == 0)
    {
      offset = input_index - 128;
      if (offset < 0)
      {
      	offset += 49152;
      }

      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 < 128; n++)
        {
          pcm1[offset + n].ch0 = pcm0[offset + n].ch0;
        }
      }
      else // toggle switch on
      {
        PORTC.PODR.BIT.B0 = 1;

        for (n = 0; n < 128 + 2; n++)
	{
          m = offset - 2 + n;
	  if (m < 0)
          {
            m += 49152;
          }
          s0[n] = pcm0[m].ch0;
	}

        for (n = 0; n < 2; n++)
	{
          m = offset - 2 + n;
	  if (m < 0)
          {
            m += 49152;
          }
          s1[n] = pcm1[m].ch0;
	}
        for (n = 2; n < 128 + 2; n++)
	{
	  s1[n] = 0.0;
	}
	
	fc = 1000.0;
        Q = 10.0;

        IIR_resonator(fc / FS, Q, a, b);

        for (n = 0; n < 128; n++)
        {
          for (m = 0; m <= 2; m++)
          {
            s1[2 + n] += b[m] * s0[2 + n - m];
          }
          for (m = 1; m <= 2; m++)
          {
            s1[2 + n] += -a[m] * s1[2 + n - m];
          }
        }

        for (n = 0; n < 128; n++)
        {
          pcm1[offset + n].ch0 = s1[2 + n];
        }
      }
    }
  }
}