#include <Wire.h>
#include "Adafruit_MPR121.h"

#ifndef _BV
#define _BV(bit) (1 << (bit)) 
#endif

// You can have up to 4 on one i2c bus
Adafruit_MPR121 cap1 = Adafruit_MPR121();
Adafruit_MPR121 cap2 = Adafruit_MPR121();


// defines pins numbers
const int stepPin1 = 4; 
const int stepPin2 = 5;
const int stepPin3 = 6;
const int dirPin3 = 8; 
const int dirPin1 = 9; 
const int dirPin2 = 10;
const int stepPin4 = 7;
const int dirPin4 = 11; 
const int enable1 = A0; 
const int enable2 = A1;
const int enable3 = A2;

// here comes a bunch of 'useful' vars
int coun;
bool dir1=0;
bool dir2=0;
bool dir3=0;
bool dir4=0;
int del;
float c = 261.63;
float cs = 277.18;
float d = 293.66;
float ds = 311.13;
float e = 329.63;
float f = 349.23;
float fs = 369.99;
float g = 392;
float gs = 415.3;
float a = 440;
float as = 466.16;
float b = 493.88;
float c1 = 523.25;
float cs1 = 554.37;
float d1 = 587.33;
float ds1 = 622.25;
float e1 = 659.25;
float f1 = 698.46;
float fs1 = 739.99;
float g1 = 783.99;
float gs1 = 830.61;
float a1 = 880;
float as1 = 932.33;
float b1 = 987.77;

float myNote[] = {ds1/8, e1/8, f1/8, fs1/8, g1/8, gs1/8, a1/8, as1/8, b1/8, c/2, cs/2, d/2, ds/2, e/2, f/2, fs/2, g/2, gs/2, a/2, as/2, b/2, c1/2, cs1/2, d1/2 };
static unsigned long previousMotor1Time = micros();
static unsigned long currentMotor1Time = 0;
static int motor1Freq = 0;
static int motor1PinState = 0;
static int motor1ON = 100;

static unsigned long previousMotor2Time = micros();
static unsigned long currentMotor2Time = 0;
static int motor2Freq = 0;
static int motor2PinState = 0;
static int motor2ON = 100;

static unsigned long previousMotor3Time = micros();
static unsigned long currentMotor3Time = 0;
static int motor3Freq = 0;
static int motor3PinState = 0;
static int motor3ON = 100;

static unsigned long previousMotor4Time = micros();
static unsigned long currentMotor4Time = 0;
static int motor4Freq = 0;
static int motor4PinState = 0;
static int motor4ON = 100;


static int previousCap1 = 0;
static int previousCap2 = 0;
static unsigned long previousMeasureTime = millis();
static unsigned long currentMeasureTime = 0;
static unsigned long previousMeasureTime2 = millis();
static unsigned long currentMeasureTime2 = 0;

int interruptPin1 = 3;
int interruptPin2 = 2;
volatile byte checkSensor1 = HIGH;
volatile byte checkSensor2 = HIGH;

void setup() {
  cap1.begin(0x5A);
  cap2.begin(0x5B);
  delay(5);
  // Sets pins as Outputs
  pinMode(stepPin1,OUTPUT); 
  pinMode(stepPin2,OUTPUT);
  pinMode(stepPin3,OUTPUT);
  pinMode(stepPin4,OUTPUT);
  pinMode(dirPin1,OUTPUT);
  pinMode(dirPin2,OUTPUT);
  pinMode(dirPin3,OUTPUT);
  pinMode(dirPin4,OUTPUT);
  pinMode(enable1,OUTPUT);
  pinMode(enable2,OUTPUT);
  pinMode(enable3,OUTPUT);
  
  digitalWrite(dirPin1,dir1);
  digitalWrite(dirPin2,dir2);
  digitalWrite(dirPin3,dir3);
  digitalWrite(dirPin4,dir4);
  digitalWrite(enable1, HIGH);
  digitalWrite(enable2, HIGH);
  digitalWrite(enable3, HIGH);
  
  pinMode(interruptPin1, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(interruptPin1), sensorCheck1, FALLING);

  pinMode(interruptPin2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(interruptPin2), sensorCheck2, FALLING);
}

void loop() {
  currentMeasureTime = millis();
  currentMeasureTime2 = millis();

  if((checkSensor1 == LOW)&&((currentMeasureTime - previousMeasureTime) >= 50)){//if(((currentMeasureTime - previousMeasureTime) >= 100) ){//&& ((motor1PinState == 0)||(motor2PinState == 0))){
    previousMeasureTime = currentMeasureTime;
    checkSensor1 = HIGH;
    int currtouched1 = cap1.touched();
    if(currtouched1 != previousCap1){
      for (uint8_t i=0; i<12; i++) {
        if ((currtouched1 & _BV(i)) && !(previousCap1 & _BV(i))) {
          if(motor1ON == 100){
            motor1ON = i;
            motor1Freq = getPeriod(myNote[i]);
            dir1=!dir1;
            digitalWrite(enable1, LOW);
            digitalWrite(dirPin1,dir1);
          } else if (motor2ON == 100){
            motor2ON = i;
            motor2Freq = getPeriod(myNote[i]);
            dir2=!dir2;
            digitalWrite(enable2, LOW);
            digitalWrite(dirPin2,dir2);
          } else if (motor3ON == 100){
            motor3ON = i;
            motor3Freq = getPeriod(myNote[i]);
            dir3=!dir3;
            digitalWrite(enable3, LOW);
            digitalWrite(dirPin3,dir3);
          } else if (motor4ON == 100){
            motor4ON = i;
            motor4Freq = getPeriod(myNote[i]);
            dir4=!dir4;
            digitalWrite(enable3, LOW);
            digitalWrite(dirPin4,dir4);
          }
          
        } else if (!(currtouched1 & _BV(i))){
          if(motor1ON == i){
            motor1ON = 100;
            digitalWrite(stepPin1,LOW);
            digitalWrite(enable1, HIGH);
            motor1PinState = 0;
          }else if(motor2ON == i){
            motor2ON = 100;
            digitalWrite(stepPin2,LOW);
            digitalWrite(enable2, HIGH);
            motor2PinState = 0;
          }else if(motor3ON == i){
            motor3ON = 100;
            digitalWrite(stepPin3,LOW);
            motor3PinState = 0;
          }else if(motor4ON == i){
            motor4ON = 100;
            digitalWrite(stepPin4,LOW);
            
            motor4PinState = 0;
          }
          if((motor3ON == 100)&&(motor4ON == 100)){
              digitalWrite(enable3, HIGH);
            }
        }
      }
    }
    previousCap1 = currtouched1;
  }

if((checkSensor2 == LOW)&&((currentMeasureTime2 - previousMeasureTime2) >= 50)){//if(((currentMeasureTime - previousMeasureTime) >= 100) ){//&& ((motor1PinState == 0)||(motor2PinState == 0))){
    previousMeasureTime2 = currentMeasureTime2;
    checkSensor2 = HIGH;
    int currtouched2 = cap2.touched();
    if(currtouched2 != previousCap2){
      for (uint8_t i=0; i<12; i++) {
        if ((currtouched2 & _BV(i)) && !(previousCap2 & _BV(i))) {
          if(motor1ON == 100){
            motor1ON = i+12;
            motor1Freq = getPeriod(myNote[i+12]);
            dir1=!dir1;
            digitalWrite(enable1, LOW);
            digitalWrite(dirPin1,dir1);
          } else if (motor2ON == 100){
            motor2ON = i+12;
            motor2Freq = getPeriod(myNote[i+12]);
            dir2=!dir2;
            digitalWrite(enable2, LOW);
            digitalWrite(dirPin2,dir2);
          } else if (motor3ON == 100){
            motor3ON = i+12;
            motor3Freq = getPeriod(myNote[i+12]);
            dir3=!dir3;
            digitalWrite(enable3, LOW);
            digitalWrite(dirPin3,dir3);
          } else if (motor4ON == 100){
            motor4ON = i+12;
            motor4Freq = getPeriod(myNote[i+12]);
            dir4=!dir4;
            digitalWrite(enable3, LOW);
            digitalWrite(dirPin4,dir4);
          }
          
        } else if (!(currtouched2 & _BV(i))){
          if(motor1ON == i+12){
            motor1ON = 100;
            digitalWrite(stepPin1,LOW);
            digitalWrite(enable1, HIGH);
            motor1PinState = 0;
          }else if(motor2ON == i+12){
            motor2ON = 100;
            digitalWrite(stepPin2,LOW);
            digitalWrite(enable2, HIGH);
            motor2PinState = 0;
          }else if(motor3ON == i+12){
            motor3ON = 100;
            digitalWrite(stepPin3,LOW);
            
          }else if(motor4ON == i+12){
            motor4ON = 100;
            digitalWrite(stepPin4,LOW);
            motor4PinState = 0;
          }
          if((motor3ON == 100)&&(motor4ON == 100)){
              digitalWrite(enable3, HIGH);
            }
        }
      }
    }
    previousCap2 = currtouched2;
  }

    currentMotor1Time = micros();
    if((currentMotor1Time - previousMotor1Time) >= motor1Freq){
      if(motor1ON < 100){
        if(motor1PinState == 0){
          digitalWrite(stepPin1,HIGH);
          motor1PinState = 1;
        } else {
          digitalWrite(stepPin1,LOW);
          motor1PinState = 0;
        }
    }
    previousMotor1Time = currentMotor1Time;
    }

    currentMotor2Time = micros();
    if((currentMotor2Time - previousMotor2Time) >= motor2Freq){
      if(motor2ON < 100){
        if(motor2PinState == 0){
          digitalWrite(stepPin2,HIGH);
          motor2PinState = 1;
        } else {
          digitalWrite(stepPin2,LOW);
          motor2PinState = 0;
        }
      }
      previousMotor2Time = currentMotor2Time;
    }

    currentMotor3Time = micros();
    if((currentMotor3Time - previousMotor3Time) >= motor3Freq){
      if(motor3ON < 100){
        if(motor3PinState == 0){
          digitalWrite(stepPin3,HIGH);
          motor3PinState = 1;
        } else {
          digitalWrite(stepPin3,LOW);
          motor3PinState = 0;
        }
      }
      previousMotor3Time = currentMotor3Time;
    }

    currentMotor4Time = micros();
    if((currentMotor4Time - previousMotor4Time) >= motor4Freq){
      if(motor4ON < 100){
        if(motor4PinState == 0){
          digitalWrite(stepPin4,HIGH);
          motor4PinState = 1;
        } else {
          digitalWrite(stepPin4,LOW);
          motor4PinState = 0;
        }
      }
      previousMotor4Time = currentMotor4Time;
    }

}

void sensorCheck1(){
  checkSensor1 = LOW;
}

void sensorCheck2(){
  checkSensor2 = LOW;
}

int getPeriod(float freq){
  int period;
  period = (1/freq)*300000;
  return period;
}