#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 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, d;
  float s, a;

  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;

        d = (int)((float)FS * 1.0 * (float)volume1 / 4096.0);
        a = (float)volume2 / 4096.0;

        for (n = 0; n < 128; n++)
        {
          s = pcm0[offset + n].ch0;

          m = offset + n - d;
          if (m < 0)
          {
            m += 49152;
          }

          s += pcm0[m].ch0 * a;

          pcm1[offset + n].ch0 = s;
        }
      }
    }
  }
}