#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 m0, m1;
  float depth, rate;
  float s, s0, s1, delta, tau, d, t, p, x = 0.0;

  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 = (float)FS * 0.002;
        depth = (float)FS * 0.002 * volume1 / 4096.0;
        rate = (float)volume2 / 4096.0 * 10.0;

        for (n = 0; n < 128; n++)
        {
          tau = d + depth * sinf(2.0 * M_PI * x);

          delta = rate / FS;
          x += delta;
          if (x >= 1.0)
          {
            x -= 1.0;
          }

          t = (float)(offset + n) - tau;
          if (t < 0.0)
          {
            t += 49152.0;
          }

          m0 = (int)t;
          m1 = m0 + 1;
          if (m1 >= 49152)
          {
            m1 -= 49152;
          }
          p = t - (float)m0;

          s0 = pcm0[m0].ch0;
          s1 = pcm0[m1].ch0;

          s = p * s1 + (1.0 - p) * s0;

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