#include <MozziGuts.h>
#include <Oscil.h>
#include <tables/cos2048_int8.h>
#include <mozzi_midi.h> // for mtof
#include <mozzi_fixmath.h>
#include <AutoMap.h>
#define CONTROL_RATE 16 // Hz
Oscil<COS2048_NUM_CELLS, AUDIO_RATE> aCos(COS2048_DATA);
Oscil<COS2048_NUM_CELLS, AUDIO_RATE> aVibrato(COS2048_DATA);
const byte intensity = 255;
AutoMap map_A0_oscFreq(0,650,1800,150);
AutoMap map_A1_modFreq(0,650,2,50);
AutoMap map_A0_red(0,650,150,254);
AutoMap map_A1_blue(0,650,150,254);
int green_timer;

void setup(){
  pinMode(6, OUTPUT); // Blue
  pinMode(3, OUTPUT); // Red
  pinMode(5, OUTPUT); // Green
  green_timer = 254;
  startMozzi(CONTROL_RATE);
}

void updateControl(){
  if(--green_timer < 215) green_timer = 254;
  analogWrite(5, green_timer);
  int input_A0 = mozziAnalogRead(0);
  int osc_freq = map_A0_oscFreq(input_A0);
  aCos.setFreq(osc_freq);
  int red_data = map_A0_red(input_A0);
  analogWrite(3, red_data);
  int input_A1 = mozziAnalogRead(1);
  int vib_freq = map_A1_modFreq(input_A1);
  aVibrato.setFreq(vib_freq);
  int blue_data = map_A1_blue(input_A1);
  analogWrite(6, blue_data);
}

int updateAudio(){
    Q15n16 vibrato = (Q15n16) intensity * aVibrato.next();
    return aCos.phMod(vibrato); // phase modulation to modulate frequency
}

void loop(){
  audioHook();
}