// TEENSY - MALINETTE
// Control inputs and outputs with MIDI messages
// 30/05/2015 - http://reso-nance.org
 
#include <Servo.h> //servo library
 
// Pins
int anaPins[] = {16,17,18,19,20,21}; // analog pins
int outPins[] = {9,10}; // output pins
const int anaNb = 6; // number of inputs
const int outNb = 2; // number of outputs
 
// Midi Mapping
int anaCtl[] = {5,4,3,2,1,0}; // controller in
int outCtl[] = {50, 51}; // controller out
int anaStateCtl[] = {20, 21} ; // sensor on (20), sensor off (21)
int outStateCtl[] = {60, 61}; // controller out modes (0,1,2,3) (none, digital, pwm, servo)
 
// Ana Sensors
int anaState[anaNb]; // state : on/off
const int channel = 1;
int anaValues[anaNb]; // current analog values
int anaLastValues[anaNb]; // previous analog values
 
// Sampling rate
const long interval = 30;
unsigned long currentMillis;
unsigned long previousMillis = 0;
 
// Output
Servo servo0,servo1;
int outState[outNb]; // states : stop, digital, pwm, servo
 
void setup() { 
  for (int i = 0; i < outNb; i++) {
    pinMode(outPins[i], OUTPUT);
  }
  usbMIDI.setHandleControlChange(OnControlChange);
} 
 
void loop() {
  currentMillis = millis();
  if(currentMillis - previousMillis >= interval)  {
      previousMillis = currentMillis;
      for (int i = 0; i < anaNb; i++) {
          if(anaState[i] == 1)  { // check first if the sensor is on
              anaValues[i] = analogRead(anaPins[i])  / 8 ;
              if (anaValues[i] != anaLastValues[i]) {
                  usbMIDI.sendControlChange(anaCtl[i], anaValues[i], channel);
                  anaLastValues[i] = anaValues[i];
			  }
          }
       }
    }
 
  //Discard incoming MIDI messages.
  while (usbMIDI.read()) {
  }
}
 
// Receive Midi Control Change
void OnControlChange(byte channel, byte control, byte value) {
 
   // Analog sensors on/off
   if (control == anaStateCtl[0] ) {anaState[-value+(anaNb-1)] = 0;}
   else if (control == anaStateCtl[1] ) {anaState[-value+(anaNb-1)] = 1;}
 
   // Set Outputs Values
   else if (control == outCtl[0] ) {setOutValue(0, value, servo0);}
   else if (control == outCtl[1] ) {setOutValue(1, value, servo1);}
 
   // Set Outputs States (none, digital, pwm, servo)
   else if (control == outStateCtl[0] ) {setOutState(0, value, servo0);}
   else if (control == outStateCtl[1] ) {setOutState(1, value, servo1);}
}
 
// Set outputs
void setOutValue(int outId, int value, Servo servo) {
  int state = outState[outId];
  if( state == 1) { // digital mode
       if(value == 0) {digitalWrite(outPins[outId], LOW);}
       if(value >= 1) {digitalWrite(outPins[outId], HIGH);}
     } else if( state == 2){ // pwm mode
        analogWrite(outPins[outId], map(value,0,127,0,255));
     } else if( state == 3){ // servo mode
        if (servo.attached()) {servo.write(map(value,0,127,0,180));}
     }
}
 
void setOutState(int outId, int value, Servo servo) {
  // Check last state to turn off
  if ( outState[outId] == 1) {digitalWrite(outPins[outId], LOW);}
  else if ( outState[outId] == 2) {analogWrite(outPins[outId], 0);}
 
  if (value == 3) {
      if (!servo.attached()) {servo.attach(outPins[outId]);}
  } else {
    if (servo.attached()) {servo.detach();}
  }
 
  outState[outId] = value;
}