// Arduino 1

int i, pitch[5];

float mapfloat(float x, float in_min, float in_max, float out_min, float out_max){
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void setup() {
  OCR0A = 1; OCR2A = 249; OCR2B = 125; OCR1A = 32767;
  TCCR0A = _BV(WGM00) | _BV(WGM01) | _BV(COM0A0);
  TCCR0B = _BV(WGM02) | _BV(CS02)  | _BV(CS01);
  TCCR2A =  _BV(COM2B1) | _BV(COM2B0) | _BV(WGM21) | _BV(WGM20);
  TCCR2B = _BV(WGM22) | _BV(CS22) | _BV(CS21);
  TCCR1A = _BV(WGM10)  | _BV(WGM11) | _BV(COM1A0);
  TCCR1B =_BV(WGM12) | _BV(WGM13) | _BV(CS12)  | _BV(CS11);
  for(i=2; i<4; i++) pinMode(i, OUTPUT);
  for(i=6; i<19; i++) pinMode(i, OUTPUT);
  Serial.begin(115200);
}

void loop() {
  int freq, latest;
  float ffreq;
  while(digitalRead(6)){}
  while(!digitalRead(6)){}
  OCR1A = 32767; TCNT1 = 0;
  while(digitalRead(6)){}
  latest = 10000 - (int)TCNT1;
  for(i=4; i>0; i--) pitch[i] = pitch[i-1];
  pitch[0] = latest;
  freq = 0;
  for (i=0; i<5; i++) freq = freq + pitch[i];
  freq = freq / 5;
  if(freq < 635) ffreq = 0.;
  else if(freq > 1100) ffreq = 4096.;
  else ffreq = mapfloat((float)freq, 635., 1100., 128., 4096.);
  Serial.print("data = "); 
  Serial.print(latest);
  Serial.print(" , ffreq = "); 
  Serial.println(ffreq);
  if(ffreq < 64.0) freq = 0;
  else if(ffreq < 128.0)  freq = (int)(ffreq * 8.0) - 512;
  else if(ffreq < 256.0)  freq = 0b001000000000 | ((int)(ffreq * 4.0) - 512);
  else if(ffreq < 512.0)  freq = 0b010000000000 | ((int)(ffreq * 2.0) - 512);
  else if(ffreq < 1024.0) freq = 0b011000000000 | ((int) ffreq - 512);
  else if(ffreq < 2048.0) freq = 0b100000000000 | ((int)(ffreq / 2.0) - 512);
  else if(ffreq < 4096.0) freq = 0b101000000000 | ((int)(ffreq / 4.0) - 512);
  else if(ffreq < 8192.0) freq = 0b110000000000 | ((int)(ffreq / 8.0) - 512);
  else                    freq = 0b111000000000 | ((int)(ffreq / 16.0) - 512);
  digitalWrite(2,  (freq & 0b100000000000) >> 11);
  digitalWrite(7,  (freq & 0b010000000000) >> 10);
  digitalWrite(8,  (freq & 0b001000000000) >> 9);
  digitalWrite(18, (freq & 0b000100000000) >> 8);
  digitalWrite(10, (freq & 0b000010000000) >> 7);
  digitalWrite(11, (freq & 0b000001000000) >> 6);
  digitalWrite(12, (freq & 0b000000100000) >> 5);
  digitalWrite(13, (freq & 0b000000010000) >> 4);
  digitalWrite(14, (freq & 0b000000001000) >> 3);
  digitalWrite(15, (freq & 0b000000000100) >> 2);
  digitalWrite(16, (freq & 0b000000000010) >> 1);
  digitalWrite(17, (freq & 0b000000000001) );  
}


// Arduino 2

#include <MozziGuts.h>
#include <Phasor.h>
Phasor <AUDIO_RATE> aPhasor1;
Phasor <AUDIO_RATE> aPhasor2;
#define CONTROL_RATE 128
int i;

void setup() {
  for(i=2; i<4; i++) pinMode(i, INPUT);
  for(i=7; i<9; i++) pinMode(i, INPUT);
  for(i=10; i<18; i++) pinMode(i, INPUT);
  startMozzi(CONTROL_RATE);
}

void updateControl(){
  float man, pitch;
  int pw, sum = 0;
  sum = sum | (digitalRead(2)  << 11);
  sum = sum | (digitalRead(7)  << 10);
  sum = sum | (digitalRead(8)  << 9);
  sum = sum | (digitalRead(3)  << 8);
  sum = sum | (digitalRead(10) << 7);
  sum = sum | (digitalRead(11) << 6);
  sum = sum | (digitalRead(12) << 5);
  sum = sum | (digitalRead(13) << 4);
  sum = sum | (digitalRead(14) << 3);
  sum = sum | (digitalRead(15) << 2);
  sum = sum | (digitalRead(16) << 1);
  sum = sum |  digitalRead(17);
  if(sum == 0) pitch = 0;
  else{
    pw = (sum >> 9) + 7;
    man = ((float) (sum & 0b111111111 | 0b1000000000)) / 1024.;
    pitch = ((float) (1 << pw)) * man / 2.;
  }
  aPhasor1.setFreq(pitch);
  aPhasor2.setFreq(pitch * 0.99f);
}

AudioOutput_t updateAudio(){
  char asig1 = (char)(aPhasor1.next()>>24);
  char asig2 = (char)(aPhasor2.next()>>24);
  return ((int)asig1-asig2)/2;
}

void loop(){
  audioHook();
}