#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define ARDUINO_MEGA
#elif defined(__AVR_AT90USB646__)
#define TEENSY_PLUS_PLUS
#elif defined(__AVR_ATmega32U4__)
#define TEENSY_2
#elif defined(__AVR_AT90USB1286__)
#define TEENSY_PLUS_PLUS_2
#else
#define ARDUINO
#endif

#include <Wire.h>

byte regA, regB, regA_2, regB_2, regA_nd, regB_nd, regA_nd_2, regB_nd_2;
int regS;
bool flag, flag2;

byte variable[16];

#include <Encoder.h>
// Variables utilisées par les encoders
Encoder formeOndeA2(4, 5);
Encoder nbVoiesA2(3, 2);
Encoder formeOndeA1(17, 16);
Encoder nbVoiesA1(15, 14);
Encoder formeOndeB2(7, 6);
Encoder nbVoiesB2(8, 9);
Encoder formeOndeB1(12, 13);
Encoder nbVoiesB1(10, 11);

Encoder typeDisto(52, 53);

Encoder selEntreeFa(41, 40);
Encoder typeFiltreFa(37, 36);
Encoder selEntreeFb(39, 38);
Encoder typeFiltreFb(43, 42);

Encoder ampEnv1M1(46, 47);
Encoder ampEnv1M2(45, 44);
Encoder ampEnv2M1(48, 49);

Encoder octaveA1(28, 29);
Encoder noteA1(30, 31);
Encoder octaveA2(23, 22);
Encoder noteA2(51, 50);
Encoder octaveB1(32, 33);
Encoder noteB1(34, 35);
Encoder octaveB2(24, 25);
Encoder noteB2(26, 27);

// formeOndeA1
byte oldPosition5  = 1;
byte midipos5 = 0;
long newPosition5;

// nbVoiesA1
byte oldPosition6  = 1;
byte midipos6 = 0;
long newPosition6;

// formeOndeA2
byte oldPosition3  = 1;
byte midipos3 = 0;
long newPosition3;

// nbVoiesA2
byte oldPosition4  = 1;
byte midipos4 = 0;
long newPosition4;

// formeOndeB1
byte oldPosition7  = 1;
byte midipos7 = 0;
long newPosition7;

// nbVoiesB1
byte oldPosition8  = 1;
byte midipos8 = 0;
long newPosition8;

// formeOndeB2
byte oldPosition  = 1;
byte midipos = 0;
long newPosition;

// nbVoiesB2
byte oldPosition2  = 1;
byte midipos2 = 0;
long newPosition2;

// typeDisto
byte oldPosition9 = 5;
byte midipos9 = 0;
long newPosition9;

// selEntreeFa
byte oldPosition10 = 5;
byte midipos10 = 0;
long newPosition10;

// typeFiltreFa
byte oldPosition11 = 5;
byte midipos11 = 0;
long newPosition11;

// selEntreeFb
byte oldPosition12 = 5;
byte midipos12 = 0;
long newPosition12;

// typeFiltreFb
byte oldPosition13 = 5;
byte midipos13 = 0;
long newPosition13;

// ampEnv1M1
byte oldPosition14 = 5;
byte midipos14 = 0;
long newPosition14;

// ampEnv1M2
byte oldPosition15 = 5;
byte midipos15 = 0;
long newPosition15;

// ampEnv2M1
byte oldPosition16 = 5;
byte midipos16 = 0;
long newPosition16;

// octaveA1
byte oldPosition17 = 5;
byte midipos17 = 0;
long newPosition17;

// noteA1
byte oldPosition18 = 5;
byte midipos18 = 0;
long newPosition18;

// octaveA2
byte oldPosition19 = 5;
byte midipos19 = 0;
long newPosition19;

// noteA2
byte oldPosition20 = 5;
byte midipos20 = 0;
long newPosition20;

// octaveB1
byte oldPosition21 = 5;
byte midipos21 = 0;
long newPosition21;

// noteB1
byte oldPosition22 = 5;
byte midipos22 = 0;
long newPosition22;

// octaveB2
byte oldPosition23 = 5;
byte midipos23 = 0;
long newPosition23;

// noteB2
byte oldPosition24 = 5;
byte midipos24 = 0;
long newPosition24;

// Uncomment this line to send debug messages to the serial monitor
//#define DEBUG

// Uncomment this line to enable outputs corresponding to the MIDI Fighter so MF mappings can be used in Traktor.
//#define MIDI_FIGHTER

//#define FASTADC
// defines for setting and clearing register bits
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif

// MIDI mapping taken from http://www.nortonmusic.com/midi_cc.html
#define MIDI_CC_MODULATION 0x01
#define MIDI_CC_BREATH 0x02
#define MIDI_CC_VOLUME 0x07
#define MIDI_CC_BALANCE 0x08
#define MIDI_CC_PAN 0x0A
#define MIDI_CC_EXPRESSION 0x0B
#define MIDI_CC_EFFECT1 0x0C
#define MIDI_CC_EFFECT2 0x0D

#define MIDI_CC_GENERAL1 0x0E
#define MIDI_CC_GENERAL2 0x0F
#define MIDI_CC_GENERAL3 0x10
#define MIDI_CC_GENERAL4 0x11
#define MIDI_CC_GENERAL5 0x12
#define MIDI_CC_GENERAL6 0x13
#define MIDI_CC_GENERAL7 0x14
#define MIDI_CC_GENERAL8 0x15
#define MIDI_CC_GENERAL9 0x16
#define MIDI_CC_GENERAL10 0x17
#define MIDI_CC_GENERAL11 0x18
#define MIDI_CC_GENERAL12 0x19
#define MIDI_CC_GENERAL13 0x1A
#define MIDI_CC_GENERAL14 0x1B
#define MIDI_CC_GENERAL15 0x1C
#define MIDI_CC_GENERAL16 0x1D
#define MIDI_CC_GENERAL17 0x1E
#define MIDI_CC_GENERAL18 0x1F

#define MIDI_CC_GENERAL1_FINE 0x2E
#define MIDI_CC_GENERAL2_FINE 0x2F
#define MIDI_CC_GENERAL3_FINE 0x30
#define MIDI_CC_GENERAL4_FINE 0x31
#define MIDI_CC_GENERAL5_FINE 0x32
#define MIDI_CC_GENERAL6_FINE 0x33
#define MIDI_CC_GENERAL7_FINE 0x34
#define MIDI_CC_GENERAL8_FINE 0x35
#define MIDI_CC_GENERAL9_FINE 0x36
#define MIDI_CC_GENERAL10_FINE 0x37
#define MIDI_CC_GENERAL11_FINE 0x38
#define MIDI_CC_GENERAL12_FINE 0x39
#define MIDI_CC_GENERAL13_FINE 0x3A
#define MIDI_CC_GENERAL14_FINE 0x3B
#define MIDI_CC_GENERAL15_FINE 0x3C
#define MIDI_CC_GENERAL16_FINE 0x3D
#define MIDI_CC_GENERAL17_FINE 0x3E
#define MIDI_CC_GENERAL18_FINE 0x3F

#define MIDI_CC_SUSTAIN 0x40
#define MIDI_CC_REVERB 0x5B
#define MIDI_CC_CHORUS 0x5D
#define MIDI_CC_CONTROL_OFF 0x79
#define MIDI_CC_NOTES_OFF 0x78

#define NOTE_C0 0x00 // 0
#define NOTE_C1 0x12 // 18
#define NOTE_C2 0x24 // 36


#define NUM_ROT_MCP 50

#define MIDI_CHANNEL 1


#if defined(TEENSY_PLUS_PLUS) || defined(TEENSY_2) || defined(TEENSY_PLUS_PLUS_2)
#define LED_PIN PIN_D6
#else
#define LED_PIN 13
#endif

#define MIDI_CC MIDI_CC_GENERAL1

// Timeout is in microseconds
#define ANALOGUE_INPUT_CHANGE_TIMEOUT 250000



byte MCPVAL[NUM_ROT_MCP];
byte MCPVALND[NUM_ROT_MCP];

byte analogueInputs[NUM_ROT_MCP];

byte tempAnalogueInput;

// Preallocate the for loop index so we don't keep reallocating it for every program iteration.
byte i = 0;

// Variable to hold difference between current and new analogue input values.
byte analogueDiff = 0;
// This is used as a flag to indicate that an analogue input is changing.
boolean analogueInputChanging[NUM_ROT_MCP];
// Time the analogue input was last moved
unsigned long analogueInputTimer[NUM_ROT_MCP];

#ifdef DEBUG
unsigned long loopTime = 0;
unsigned long serialSendTime = 0;
#endif

void setup()
{
  // Taken from http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1208715493/11
#ifdef FASTADC
  // set prescale to 16
  sbi(ADCSRA, ADPS2) ;
  cbi(ADCSRA, ADPS1) ;
  cbi(ADCSRA, ADPS0) ;
#endif

  Wire.begin();
  attachInterrupt(digitalPinToInterrupt(18), routineInterruption, RISING);
  attachInterrupt(digitalPinToInterrupt(19), routineInterruption2, RISING);
  mcpInit();
  mcp2Init();
  mcpLedInit();

  // Only enable serial on the Arduino or when debugging. The Teensy board should be set as a usb-midi device so serial is not needed.
#if defined(ARDUINO) || defined(ARDUINO_MEGA) || defined(DEBUG)
  // Enable serial I/O at 115200 kbps. This is faster than the standard MIDI rate of 31250 kbps.
  // The PC application which we connect to will automatically take the higher sample rate and send MIDI
  // messages out at the correct rate. We only send things faster in case there is any latency.
  Serial.begin(31250);
#endif

  for (i = 0; i < NUM_ROT_MCP; i++)
  {
    analogueInputChanging[i] = true;
    analogueInputTimer[i] = 0;
  }

#ifdef DEBUG
  serialSendTime = millis();
#endif


  delay(1500);
}


void loop()
{
#ifdef DEBUG
  loopTime = micros();
#endif

  bool secFlag = digitalRead(18);
  if (secFlag)
    flag = 1;
  bool secFlag2 = digitalRead(19);
  if (secFlag2)
    flag2 = 1;


  // formeOndeA1
  newPosition5 = (formeOndeA1.read() / 4) + 4;
  if ((newPosition5 != oldPosition5))
  {
    if (newPosition5 < 0)
    {
      formeOndeA1.write(-16);
    }
    if (newPosition5 > 7)
    {
      formeOndeA1.write(12);
    }
    oldPosition5 = newPosition5;
    if (-1 < newPosition5 && newPosition5 < 8)
    {
      midipos5 = (newPosition5 * 15.5) + 10;
      if (newPosition5 == 0)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x12); // address port A
        Wire.write(0x01); // value to send
        Wire.endTransmission();

        midipos5 = 25;
      }
      if (newPosition5 == 1)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x12); // address port A
        Wire.write(0x02); // value to send
        Wire.endTransmission();

        midipos5 = 10;
      }
      if (newPosition5 == 2)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x12); // address port A
        Wire.write(0x04); // value to send
        Wire.endTransmission();
      }
      if (newPosition5 == 3)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x12); // address port A
        Wire.write(0x08); // value to send
        Wire.endTransmission();
      }
      if (newPosition5 == 4)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x12); // address port A
        Wire.write(0x10); // value to send
        Wire.endTransmission();
      }
      if (newPosition5 == 5)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x12); // address port A
        Wire.write(0x20); // value to send
        Wire.endTransmission();
      }
      if (newPosition5 == 6)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x12); // address port A
        Wire.write(0x40); // value to send
        Wire.endTransmission();
      }
      if (newPosition5 == 7)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x12); // address port A
        Wire.write(0x80); // value to send
        Wire.endTransmission();
      }
    }
  }

  // nbVoiesA1
  newPosition6 = (nbVoiesA1.read() / 4) + 4;
  if ((newPosition6 != oldPosition6))
  {
    if (newPosition6 < 0)
    {
      nbVoiesA1.write(-16);
    }
    if (newPosition6 > 8)
    {
      nbVoiesA1.write(16);
    }
    oldPosition6 = newPosition6;
    if (-1 < newPosition6 && newPosition6 < 9)
    {
      midipos6 = (newPosition6 * 14) + 10;
      if (newPosition6 == 0)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x13); // address port A
        Wire.write(0x00); // value to send
        Wire.endTransmission();
      }
      if (newPosition6 == 1)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x13); // address port A
        Wire.write(0x80); // value to send
        Wire.endTransmission();
      }
      if (newPosition6 == 2)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x13); // address port A
        Wire.write(0xC0); // value to send
        Wire.endTransmission();
      }
      if (newPosition6 == 3)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x13); // address port A
        Wire.write(0xE0); // value to send
        Wire.endTransmission();
      }
      if (newPosition6 == 4)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x13); // address port A
        Wire.write(0xF0); // value to send
        Wire.endTransmission();
      }
      if (newPosition6 == 5)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x13); // address port A
        Wire.write(0xF8); // value to send
        Wire.endTransmission();
      }
      if (newPosition6 == 6)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x13); // address port A
        Wire.write(0xFC); // value to send
        Wire.endTransmission();
      }
      if (newPosition6 == 7)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x13); // address port A
        Wire.write(0xFE); // value to send
        Wire.endTransmission();
      }
      if (newPosition6 == 8)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x13); // address port A
        Wire.write(0xFF); // value to send
        Wire.endTransmission();
      }
    }

  }

  // formeOndeA2
  newPosition3 = (formeOndeA2.read() / 4) + 4;
  if ((newPosition3 != oldPosition3))
  {
    if (newPosition3 < 0)
    {
      formeOndeA2.write(-16);
    }
    if (newPosition3 > 7)
    {
      formeOndeA2.write(12);
    }
    oldPosition3 = newPosition3;
    if (-1 < newPosition3 && newPosition3 < 8)
    {
      midipos3 = (newPosition3 * 15.5) + 10;
      if (newPosition3 == 0)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x12); // address port A
        Wire.write(0x01); // value to send
        Wire.endTransmission();

        midipos3 = 25;
      }
      if (newPosition3 == 1)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x12); // address port A
        Wire.write(0x02); // value to send
        Wire.endTransmission();

        midipos3 = 10;
      }
      if (newPosition3 == 2)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x12); // address port A
        Wire.write(0x04); // value to send
        Wire.endTransmission();
      }
      if (newPosition3 == 3)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x12); // address port A
        Wire.write(0x08); // value to send
        Wire.endTransmission();
      }
      if (newPosition3 == 4)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x12); // address port A
        Wire.write(0x10); // value to send
        Wire.endTransmission();
      }
      if (newPosition3 == 5)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x12); // address port A
        Wire.write(0x20); // value to send
        Wire.endTransmission();
      }
      if (newPosition3 == 6)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x12); // address port A
        Wire.write(0x40); // value to send
        Wire.endTransmission();
      }
      if (newPosition3 == 7)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x12); // address port A
        Wire.write(0x80); // value to send
        Wire.endTransmission();
      }
    }
  }

  // nbVoiesA2
  newPosition4 = (nbVoiesA2.read() / 4) + 4;
  if ((newPosition4 != oldPosition4))
  {
    if (newPosition4 < 0)
    {
      nbVoiesA2.write(-16);
    }
    if (newPosition4 > 8)
    {
      nbVoiesA2.write(16);
    }
    oldPosition4 = newPosition4;
    if (-1 < newPosition4 && newPosition4 < 9)
    {
      midipos4 = (newPosition4 * 14) + 10;
      if (newPosition4 == 0)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x13); // address port A
        Wire.write(0x00); // value to send
        Wire.endTransmission();
      }
      if (newPosition4 == 1)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x13); // address port A
        Wire.write(0x80); // value to send
        Wire.endTransmission();
      }
      if (newPosition4 == 2)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x13); // address port A
        Wire.write(0xC0); // value to send
        Wire.endTransmission();
      }
      if (newPosition4 == 3)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x13); // address port A
        Wire.write(0xE0); // value to send
        Wire.endTransmission();
      }
      if (newPosition4 == 4)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x13); // address port A
        Wire.write(0xF0); // value to send
        Wire.endTransmission();
      }
      if (newPosition4 == 5)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x13); // address port A
        Wire.write(0xF8); // value to send
        Wire.endTransmission();
      }
      if (newPosition4 == 6)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x13); // address port A
        Wire.write(0xFC); // value to send
        Wire.endTransmission();
      }
      if (newPosition4 == 7)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x13); // address port A
        Wire.write(0xFE); // value to send
        Wire.endTransmission();
      }
      if (newPosition4 == 8)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x13); // address port A
        Wire.write(0xFF); // value to send
        Wire.endTransmission();
      }
    }

  }

  // formeOndeB1
  newPosition7 = (formeOndeB1.read() / 4) + 4;
  if ((newPosition7 != oldPosition7))
  {
    if (newPosition7 < 0)
    {
      formeOndeB1.write(-16);
    }
    if (newPosition7 > 7)
    {
      formeOndeB1.write(12);
    }
    oldPosition7 = newPosition7;
    if (-1 < newPosition7 && newPosition7 < 8)
    {
      if (newPosition7 == 0)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x13); // address port A
        Wire.write(0x01); // value to send
        Wire.endTransmission();

        midipos7 = 118;
      }
      if (newPosition7 == 1)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x13); // address port A
        Wire.write(0x02); // value to send
        Wire.endTransmission();

        midipos7 = 103;
      }
      if (newPosition7 == 2)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x13); // address port A
        Wire.write(0x04); // value to send
        Wire.endTransmission();

        midipos7 = 87;
      }
      if (newPosition7 == 3)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x13); // address port A
        Wire.write(0x08); // value to send
        Wire.endTransmission();

        midipos7 = 72;
      }
      if (newPosition7 == 4)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x13); // address port A
        Wire.write(0x10); // value to send
        Wire.endTransmission();

        midipos7 = 56;
      }
      if (newPosition7 == 5)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x13); // address port A
        Wire.write(0x20); // value to send
        Wire.endTransmission();

        midipos7 = 41;
      }
      if (newPosition7 == 6)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x13); // address port A
        Wire.write(0x40); // value to send
        Wire.endTransmission();

        midipos7 = 10;
      }
      if (newPosition7 == 7)
      {
        Wire.beginTransmission(0x24);
        Wire.write(0x13); // address port A
        Wire.write(0x80); // value to send
        Wire.endTransmission();

        midipos7 = 25;
      }
    }
  }

  // nbVoiesB1
  newPosition8 = (nbVoiesB1.read() / 4) + 4;
  if ((newPosition8 != oldPosition8))
  {
    if (newPosition8 < 0)
    {
      nbVoiesB1.write(-16);
    }
    if (newPosition8 > 8)
    {
      nbVoiesB1.write(16);
    }
    oldPosition8 = newPosition8;
    if (-1 < newPosition8 && newPosition8 < 9)
    {
      midipos8 = (newPosition8 * 14) + 10;
      if (newPosition8 == 0)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x12); // address port A
        Wire.write(0x00); // value to send
        Wire.endTransmission();
      }
      if (newPosition8 == 1)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x12); // address port A
        Wire.write(0x01); // value to send
        Wire.endTransmission();
      }
      if (newPosition8 == 2)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x12); // address port A
        Wire.write(0x03); // value to send
        Wire.endTransmission();
      }
      if (newPosition8 == 3)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x12); // address port A
        Wire.write(0x07); // value to send
        Wire.endTransmission();
      }
      if (newPosition8 == 4)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x12); // address port A
        Wire.write(0x0F); // value to send
        Wire.endTransmission();
      }
      if (newPosition8 == 5)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x12); // address port A
        Wire.write(0x1F); // value to send
        Wire.endTransmission();
      }
      if (newPosition8 == 6)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x12); // address port A
        Wire.write(0x3F); // value to send
        Wire.endTransmission();
      }
      if (newPosition8 == 7)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x12); // address port A
        Wire.write(0x7F); // value to send
        Wire.endTransmission();
      }
      if (newPosition8 == 8)
      {
        Wire.beginTransmission(0x21);
        Wire.write(0x12); // address port A
        Wire.write(0xFF); // value to send
        Wire.endTransmission();;
      }
    }

  }

  // formeOndeB2
  newPosition = (formeOndeB2.read() / 4) + 4;
  if ((newPosition != oldPosition))
  {
    if (newPosition < 0)
    {
      formeOndeB2.write(-16);
    }
    if (newPosition > 7)
    {
      formeOndeB2.write(12);
    }
    oldPosition = newPosition;
    if (-1 < newPosition && newPosition < 8)
    {
      if (newPosition == 0)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x13); // address port A
        Wire.write(0x01); // value to send
        Wire.endTransmission();

        midipos = 118;
      }
      if (newPosition == 1)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x13); // address port A
        Wire.write(0x02); // value to send
        Wire.endTransmission();

        midipos = 103;
      }
      if (newPosition == 2)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x13); // address port A
        Wire.write(0x04); // value to send
        Wire.endTransmission();

        midipos = 87;
      }
      if (newPosition == 3)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x13); // address port A
        Wire.write(0x08); // value to send
        Wire.endTransmission();

        midipos = 72;
      }
      if (newPosition == 4)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x13); // address port A
        Wire.write(0x10); // value to send
        Wire.endTransmission();

        midipos = 56;
      }
      if (newPosition == 5)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x13); // address port A
        Wire.write(0x20); // value to send
        Wire.endTransmission();

        midipos = 41;
      }
      if (newPosition == 6)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x13); // address port A
        Wire.write(0x40); // value to send
        Wire.endTransmission();

        midipos = 10;
      }
      if (newPosition == 7)
      {
        Wire.beginTransmission(0x23);
        Wire.write(0x13); // address port A
        Wire.write(0x80); // value to send
        Wire.endTransmission();

        midipos = 25;
      }
    }
  }

  // nbVoiesB2
  newPosition2 = (nbVoiesB2.read() / 4) + 4;
  if ((newPosition2 != oldPosition2))
  {
    if (newPosition2 < 0)
    {
      nbVoiesB2.write(-16);
    }
    if (newPosition2 > 8)
    {
      nbVoiesB2.write(16);
    }
    oldPosition2 = newPosition2;
    if (-1 < newPosition2 && newPosition2 < 9)
    {
      midipos2 = (newPosition2 * 14) + 10;
      if (newPosition2 == 0)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x12); // address port A
        Wire.write(0x00); // value to send
        Wire.endTransmission();
      }
      if (newPosition2 == 1)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x12); // address port A
        Wire.write(0x01); // value to send
        Wire.endTransmission();
      }
      if (newPosition2 == 2)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x12); // address port A
        Wire.write(0x03); // value to send
        Wire.endTransmission();
      }
      if (newPosition2 == 3)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x12); // address port A
        Wire.write(0x07); // value to send
        Wire.endTransmission();
      }
      if (newPosition2 == 4)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x12); // address port A
        Wire.write(0x0F); // value to send
        Wire.endTransmission();
      }
      if (newPosition2 == 5)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x12); // address port A
        Wire.write(0x1F); // value to send
        Wire.endTransmission();
      }
      if (newPosition2 == 6)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x12); // address port A
        Wire.write(0x3F); // value to send
        Wire.endTransmission();
      }
      if (newPosition2 == 7)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x12); // address port A
        Wire.write(0x7F); // value to send
        Wire.endTransmission();
      }
      if (newPosition2 == 8)
      {
        Wire.beginTransmission(0x26);
        Wire.write(0x12); // address port A
        Wire.write(0xFF); // value to send
        Wire.endTransmission();;
      }
    }

  }


  // typeDisto
  newPosition9 = (typeDisto.read() / 4) + 4;
  if ((newPosition9 != oldPosition9))
  {
    if (newPosition9 < 0)
    {
      typeDisto.write(-16);
    }
    if (newPosition9 > 4)
    {
      typeDisto.write(0);
    }
    oldPosition9 = newPosition9;
    if (-1 < newPosition9 && newPosition9 < 5)
    {
      midipos9 = (newPosition9 * 25) + 10;
      if (newPosition9 == 0)
      {
        Wire.beginTransmission(0x20);
        Wire.write(0x12); // address port A
        Wire.write(0x80); // value to send
        Wire.endTransmission();

        Wire.beginTransmission(0x20);
        Wire.write(0x13); // address port A
        Wire.write(0x00); // value to send
        Wire.endTransmission();
      }
      if (newPosition9 == 1)
      {
        Wire.beginTransmission(0x20);
        Wire.write(0x12); // address port A
        Wire.write(0x40); // value to send
        Wire.endTransmission();

        Wire.beginTransmission(0x20);
        Wire.write(0x13); // address port A
        Wire.write(0x00); // value to send
        Wire.endTransmission();
      }
      if (newPosition9 == 2)
      {
        Wire.beginTransmission(0x20);
        Wire.write(0x12); // address port A
        Wire.write(0x20); // value to send
        Wire.endTransmission();

        Wire.beginTransmission(0x20);
        Wire.write(0x13); // address port A
        Wire.write(0x00); // value to send
        Wire.endTransmission();
      }
      if (newPosition9 == 3)
      {
        Wire.beginTransmission(0x20);
        Wire.write(0x13); // address port A
        Wire.write(0x04); // value to send
        Wire.endTransmission();

        Wire.beginTransmission(0x20);
        Wire.write(0x12); // address port A
        Wire.write(0x00); // value to send
        Wire.endTransmission();
      }
      if (newPosition9 == 4)
      {
        Wire.beginTransmission(0x20);
        Wire.write(0x13); // address port A
        Wire.write(0x08); // value to send
        Wire.endTransmission();

        Wire.beginTransmission(0x20);
        Wire.write(0x12); // address port A
        Wire.write(0x00); // value to send
        Wire.endTransmission();
      }
    }
  }
  // selEntreeFa
  newPosition10 = (selEntreeFa.read() / 4) + 4;
  if ((newPosition10 != oldPosition10))
  {
    if (newPosition10 < 0)
    {
      selEntreeFa.write(-16);
    }
    if (newPosition10 > 2)
    {
      selEntreeFa.write(-8);
    }
    oldPosition10 = newPosition10;
    if (-1 < newPosition10 && newPosition10 < 3)
    {
      if (newPosition10 == 0)
        midipos10 = 110;
      if (newPosition10 == 1)
        midipos10 = 0;
      if (newPosition10 == 2)
        midipos10 = 70;
    }
  }

  // typeFiltreFa
  newPosition11 = (typeFiltreFa.read() / 4) + 4;
  if ((newPosition11 != oldPosition11))
  {
    if (newPosition11 < 0)
    {
      typeFiltreFa.write(-16);
    }
    if (newPosition11 > 3)
    {
      typeFiltreFa.write(-4);
    }
    oldPosition11 = newPosition11;
    if (-1 < newPosition11 && newPosition11 < 4)
    {
      midipos11 = (newPosition11 * 40);
    }
  }

  // selEntreeFb
  newPosition12 = (selEntreeFb.read() / 4) + 4;
  if ((newPosition12 != oldPosition12))
  {
    if (newPosition12 < 0)
    {
      selEntreeFb.write(-16);
    }
    if (newPosition12 > 2)
    {
      selEntreeFb.write(-8);
    }
    oldPosition12 = newPosition12;
    if (-1 < newPosition12 && newPosition12 < 3)
    {
      if (newPosition12 == 0)
        midipos12 = 110;
      if (newPosition12 == 1)
        midipos12 = 0;
      if (newPosition12 == 2)
        midipos12 = 70;
    }
  }

  // typeFiltreFb
  newPosition13 = (typeFiltreFb.read() / 4) + 4;
  if ((newPosition13 != oldPosition13))
  {
    if (newPosition13 < 0)
    {
      typeFiltreFb.write(-16);
    }
    if (newPosition13 > 3)
    {
      typeFiltreFb.write(-4);
    }
    oldPosition13 = newPosition13;
    if (-1 < newPosition13 && newPosition13 < 4)
    {
      midipos13 = (newPosition13 * 40);
    }
  }

  // ampEnv1M1
  newPosition14 = (ampEnv1M1.read() / 4) + 4;
  if ((newPosition14 != oldPosition14))
  {
    if (newPosition14 < 0)
    {
      ampEnv1M1.write(-16);
    }
    if (newPosition14 > 28)
    {
      ampEnv1M1.write(96);
    }
    oldPosition14 = newPosition14;
    if (-1 < newPosition14 && newPosition14 < 29)
    {
      if (newPosition14 == 0)
        midipos14 = 0;
      if (newPosition14 == 1)
        midipos14 = 5;
      if (newPosition14 == 2)
        midipos14 = 10;
      if (newPosition14 == 3)
        midipos14 = 14;
      if (newPosition14 == 4)
        midipos14 = 17;
      if (newPosition14 == 5)
        midipos14 = 21;
      if (newPosition14 == 6)
        midipos14 = 25;
      if (newPosition14 == 7)
        midipos14 = 29;
      if (newPosition14 == 8)
        midipos14 = 33;
      if (newPosition14 == 9)
        midipos14 = 36;
      if (newPosition14 == 10)
        midipos14 = 40;
      if (newPosition14 == 11)
        midipos14 = 45;
      if (newPosition14 == 12)
        midipos14 = 48;
      if (newPosition14 == 13)
        midipos14 = 51;
      if (newPosition14 == 14)
        midipos14 = 55;
      if (newPosition14 == 15)
        midipos14 = 60;
      if (newPosition14 == 16)
        midipos14 = 63;
      if (newPosition14 == 17)
        midipos14 = 68;
      if (newPosition14 == 18)
        midipos14 = 71;
      if (newPosition14 == 19)
        midipos14 = 74;
      if (newPosition14 == 20)
        midipos14 = 79;
      if (newPosition14 == 21)
        midipos14 = 82;
      if (newPosition14 == 22)
        midipos14 = 85;
      if (newPosition14 == 23)
        midipos14 = 90;
      if (newPosition14 == 24)
        midipos14 = 95;
      if (newPosition14 == 25)
        midipos14 = 98;
      if (newPosition14 == 26)
        midipos14 = 102;
      if (newPosition14 == 27)
        midipos14 = 115;
    }
  }

  // ampEnv1M2
  newPosition15 = (ampEnv1M2.read() / 4) + 4;
  if ((newPosition15 != oldPosition15))
  {
    if (newPosition15 < 0)
    {
      ampEnv1M2.write(-16);
    }
    if (newPosition15 > 28)
    {
      ampEnv1M2.write(96);
    }
    oldPosition15 = newPosition15;
    if (-1 < newPosition15 && newPosition15 < 29)
    {
      if (newPosition15 == 0)
        midipos15 = 0;
      if (newPosition15 == 1)
        midipos15 = 5;
      if (newPosition15 == 2)
        midipos15 = 10;
      if (newPosition15 == 3)
        midipos15 = 14;
      if (newPosition15 == 4)
        midipos15 = 17;
      if (newPosition15 == 5)
        midipos15 = 21;
      if (newPosition15 == 6)
        midipos15 = 25;
      if (newPosition15 == 7)
        midipos15 = 29;
      if (newPosition15 == 8)
        midipos15 = 33;
      if (newPosition15 == 9)
        midipos15 = 36;
      if (newPosition15 == 10)
        midipos15 = 40;
      if (newPosition15 == 11)
        midipos15 = 45;
      if (newPosition15 == 12)
        midipos15 = 48;
      if (newPosition15 == 13)
        midipos15 = 51;
      if (newPosition15 == 14)
        midipos15 = 55;
      if (newPosition15 == 15)
        midipos15 = 60;
      if (newPosition15 == 16)
        midipos15 = 63;
      if (newPosition15 == 17)
        midipos15 = 68;
      if (newPosition15 == 18)
        midipos15 = 71;
      if (newPosition15 == 19)
        midipos15 = 74;
      if (newPosition15 == 20)
        midipos15 = 79;
      if (newPosition15 == 21)
        midipos15 = 82;
      if (newPosition15 == 22)
        midipos15 = 85;
      if (newPosition15 == 23)
        midipos15 = 90;
      if (newPosition15 == 24)
        midipos15 = 95;
      if (newPosition15 == 25)
        midipos15 = 98;
      if (newPosition15 == 26)
        midipos15 = 102;
      if (newPosition15 == 27)
        midipos15 = 115;
    }
  }


  // ampEnv2M1
  newPosition16 = (ampEnv2M1.read() / 4) + 4;
  if ((newPosition16 != oldPosition16))
  {
    if (newPosition16 < 0)
    {
      ampEnv2M1.write(-16);
    }
    if (newPosition16 > 28)
    {
      ampEnv2M1.write(96);
    }
    oldPosition16 = newPosition16;
    if (-1 < newPosition16 && newPosition16 < 29)
    {
      if (newPosition16 == 0)
        midipos16 = 0;
      if (newPosition16 == 1)
        midipos16 = 5;
      if (newPosition16 == 2)
        midipos16 = 10;
      if (newPosition16 == 3)
        midipos16 = 14;
      if (newPosition16 == 4)
        midipos16 = 17;
      if (newPosition16 == 5)
        midipos16 = 21;
      if (newPosition16 == 6)
        midipos16 = 25;
      if (newPosition16 == 7)
        midipos16 = 29;
      if (newPosition16 == 8)
        midipos16 = 33;
      if (newPosition16 == 9)
        midipos16 = 36;
      if (newPosition16 == 10)
        midipos16 = 40;
      if (newPosition16 == 11)
        midipos16 = 45;
      if (newPosition16 == 12)
        midipos16 = 48;
      if (newPosition16 == 13)
        midipos16 = 51;
      if (newPosition16 == 14)
        midipos16 = 55;
      if (newPosition16 == 15)
        midipos16 = 60;
      if (newPosition16 == 16)
        midipos16 = 63;
      if (newPosition16 == 17)
        midipos16 = 68;
      if (newPosition16 == 18)
        midipos16 = 71;
      if (newPosition16 == 19)
        midipos16 = 74;
      if (newPosition16 == 20)
        midipos16 = 79;
      if (newPosition16 == 21)
        midipos16 = 82;
      if (newPosition16 == 22)
        midipos16 = 85;
      if (newPosition16 == 23)
        midipos16 = 90;
      if (newPosition16 == 24)
        midipos16 = 95;
      if (newPosition16 == 25)
        midipos16 = 98;
      if (newPosition16 == 26)
        midipos16 = 102;
      if (newPosition16 == 27)
        midipos16 = 115;
    }
  }

  // octaveA1
  newPosition17 = (octaveA1.read() / 4) + 4;
  if ((newPosition17 != oldPosition17))
  {
    if (newPosition17 < 0)
    {
      octaveA1.write(-16);
    }
    if (newPosition17 > 6)
    {
      octaveA1.write(8);
    }
    oldPosition17 = newPosition17;
    if (-1 < newPosition17 && newPosition17 < 7)
    {
      midipos17 = (newPosition17 * 21);
    }
  }
  // noteA1
  newPosition18 = (noteA1.read() / 4) + 4;
  if ((newPosition18 != oldPosition18))
  {
    if (newPosition18 < 0)
    {
      noteA1.write(-16);
    }
    if (newPosition18 > 14)
    {
      noteA1.write(40);
    }
    oldPosition18 = newPosition18;
    if (-1 < newPosition18 && newPosition18 < 15)
    {
      midipos18 = (newPosition18 * 9);
    }
  }

  // octaveA2
  newPosition19 = (octaveA2.read() / 4) + 4;
  if ((newPosition19 != oldPosition19))
  {
    if (newPosition19 < 0)
    {
      octaveA2.write(-16);
    }
    if (newPosition19 > 6)
    {
      octaveA2.write(8);
    }
    oldPosition19 = newPosition19;
    if (-1 < newPosition19 && newPosition19 < 7)
    {
      midipos19 = (newPosition19 * 21);
    }
  }
  // noteA2
  newPosition20 = (noteA2.read() / 4) + 4;
  if ((newPosition20 != oldPosition20))
  {
    if (newPosition20 < 0)
    {
      noteA2.write(-16);
    }
    if (newPosition20 > 14)
    {
      noteA2.write(40);
    }
    oldPosition20 = newPosition20;
    if (-1 < newPosition20 && newPosition20 < 15)
    {
      midipos20 = (newPosition20 * 9);
    }
  }

  // octaveB1
  newPosition21 = (octaveB1.read() / 4) + 4;
  if ((newPosition21 != oldPosition21))
  {
    if (newPosition21 < 0)
    {
      octaveB1.write(-16);
    }
    if (newPosition21 > 6)
    {
      octaveB1.write(8);
    }
    oldPosition21 = newPosition21;
    if (-1 < newPosition21 && newPosition21 < 7)
    {
      midipos21 = (newPosition21 * 21);
    }
  }
  // noteB1
  newPosition22 = (noteB1.read() / 4) + 4;
  if ((newPosition22 != oldPosition22))
  {
    if (newPosition22 < 0)
    {
      noteB1.write(-16);
    }
    if (newPosition22 > 14)
    {
      noteB1.write(40);
    }
    oldPosition22 = newPosition22;
    if (-1 < newPosition22 && newPosition22 < 15)
    {
      midipos22 = (newPosition22 * 9);
    }
  }

  // octaveB2
  newPosition23 = (octaveB2.read() / 4) + 4;
  if ((newPosition23 != oldPosition23))
  {
    if (newPosition23 < 0)
    {
      octaveB2.write(-16);
    }
    if (newPosition23 > 6)
    {
      octaveB2.write(8);
    }
    oldPosition23 = newPosition23;
    if (-1 < newPosition23 && newPosition23 < 7)
    {
      midipos23 = (newPosition23 * 21);
    }
  }
  // noteB2
  newPosition24 = (noteB2.read() / 4) + 4;
  if ((newPosition24 != oldPosition24))
  {
    if (newPosition24 < 0)
    {
      noteB2.write(-16);
    }
    if (newPosition24 > 14)
    {
      noteB2.write(40);
    }
    oldPosition24 = newPosition24;
    if (-1 < newPosition24 && newPosition24 < 15)
    {
      midipos24 = (newPosition24 * 9);
    }
  }

#ifdef DEBUG
  Serial.println("Midi Pos ** : ");
  Serial.println(midipos9);
  //Serial.println(oldPosition9);
#endif

  if (flag)
    ifFlag();
  if (regA != regA_2)
    regA = regA_2;
  if (regB != regB_2)
    regB = regB_2;

  if (flag2)
    ifFlag2();
  if (regA_nd != regA_nd_2)
    regA_nd = regA_nd_2;
  if (regB_nd != regB_nd_2)
    regB_nd = regB_nd_2;

  regS = ((regA << 8) | (regB));
  crackByte(regS, variable);

  MCPVAL[0] = variable[0] * 127;
  MCPVAL[1] = variable[1] * 127;
  MCPVAL[2] = variable[2] * 127;
  MCPVAL[3] = variable[3] * 127;
  MCPVAL[4] = variable[4] * 127;
  MCPVAL[5] = variable[5] * 127;
  MCPVAL[6] = variable[6] * 127;
  MCPVAL[7] = variable[7] * 127;

  MCPVAL[8] = variable[8] * 127;
  MCPVAL[9] = variable[9] * 127;
  MCPVAL[10] = variable[10] * 127;
  MCPVAL[11] = variable[11] * 127;
  MCPVAL[12] = variable[12] * 127;
  MCPVAL[13] = variable[13] * 127;
  MCPVAL[14] = variable[14] * 127;
  MCPVAL[15] = variable[15] * 127;

  regS = ((regA_nd << 8) | (regB_nd));
  crackByte(regS, variable);

  MCPVAL[16] = variable[4] * 127;
  MCPVAL[17] = variable[5] * 127;
  MCPVAL[18] = variable[6] * 127;
  MCPVAL[19] = variable[7] * 127;
  MCPVAL[20] = variable[8] * 127;
  MCPVAL[21] = variable[9] * 127;
  MCPVAL[22] = variable[10] * 127;
  MCPVAL[23] = variable[11] * 127;
  MCPVAL[24] = variable[12] * 127;
  MCPVAL[25] = variable[13] * 127;
  MCPVAL[26] = midipos9;              // midipos9 = Distorsion type
  MCPVAL[27] = midipos5;              // midipos5 = A1 Forme d'onde
  MCPVAL[28] = midipos7;              // midipos7 = B1 Forme d'onde
  MCPVAL[29] = midipos;               // midipos = A2 Forme d'onde
  MCPVAL[30] = midipos3;              // midipos3 = B2 Forme d'onde
  MCPVAL[31] = midipos6;              // midipos6 = A1 Number of Voices
  MCPVAL[32] = midipos4;              // midipos4 = A2 Number of Voices
  MCPVAL[33] = midipos8;              // midipos6 = B1 Number of Voices
  MCPVAL[34] = midipos2;              // midipos4 = B2 Number of Voices
  MCPVAL[35] = midipos10;             // midipos10 = Filter A : Sel Entree
  MCPVAL[36] = midipos11;             // midipos11 = Filter A : Type de Filtre
  MCPVAL[37] = midipos12;             // midipos12 = Filter B : Sel Entree
  MCPVAL[38] = midipos13;             // midipos13 = Filter B : Type de Filtre
  MCPVAL[39] = midipos14;             // midipos14 = Amp Enveloppe 1 : Mod 1
  MCPVAL[40] = midipos15;             // midipos15 = Amp Enveloppe 1 : Mod 2
  MCPVAL[41] = midipos16;             // midipos16 = Amp Enveloppe 2 : Mod 1
  MCPVAL[42] = midipos17;             // midipos17 = Octave A1
  MCPVAL[43] = midipos18;             // midipos18 = Note A1
  MCPVAL[44] = midipos19;             // midipos19 = Octave A2
  MCPVAL[45] = midipos20;             // midipos20 = Note A2
  MCPVAL[46] = midipos21;             // midipos21 = Octave B1
  MCPVAL[47] = midipos22;             // midipos22 = Note B1
  MCPVAL[48] = midipos23;             // midipos23 = Octave B2
  MCPVAL[49] = midipos24;             // midipos24 = Note B2

  /*
    #ifdef DEBUG
    Serial.println("");
    Serial.println(regS, BIN);
    delay(750);
    #endif
  */
  for (i = 0; i < NUM_ROT_MCP; i++)
  {
    // Read the analogue input pin, dividing it by 8 so the 10-bit ADC value (0-1023) is converted to a 7-bit MIDI value (0-127).
    tempAnalogueInput = MCPVAL[i];

#ifdef ANALOGUE_FILTER
    // Take the absolute value of the difference between the curent and new values
    analogueDiff = abs(tempAnalogueInput - MCPVALND[i]);
    // Only continue if the threshold was exceeded, or the input was already changing
    if ((analogueDiff > 0 && analogueInputChanging[i] == true) || analogueDiff >= FILTER_AMOUNT)
    {
      // Only restart the timer if we're sure the input isn't 'between' a value
      // ie. It's moved more than FILTER_AMOUNT
      if (analogueInputChanging[i] == false || analogueDiff >= FILTER_AMOUNT)
      {
        // Reset the last time the input was moved
        analogueInputTimer[i] = micros();

        // The analogue input is moving
        analogueInputChanging[i] = true;
      }
      else if (micros() - analogueInputTimer[i] > ANALOGUE_INPUT_CHANGE_TIMEOUT)
      {
        analogueInputChanging[i] = false;
      }

      // Only send data if we know the analogue input is moving
      if (analogueInputChanging[i] == true)
      {
        // Record the new analogue value
        MCPVALND[i] = tempAnalogueInput;

        // Send the analogue value out on the general MIDI CC (see definitions at beginning of this file)
        controlChange(MIDI_CHANNEL, MIDI_CC + i, MCPVALND[i]);
      }
    }
#else
    if (analogueInputs[i] != tempAnalogueInput)
    {
      // Record the new analogue value
      analogueInputs[i] = tempAnalogueInput;

      // Send the analogue value out on the general MIDI CC (see definitions at beginning of this file)
      controlChange((MIDI_CHANNEL + 4), MIDI_CC + i, analogueInputs[i]);
    }
#endif
  }

#ifdef DEBUG
  loopTime = micros() - loopTime;

  // Print the loop execution time once per second
  if (millis() - serialSendTime > 1000)
  {
    Serial.print("Loop execution time (us): ");
    Serial.println(loopTime);

    serialSendTime = millis();
  }
#endif
}

// Send a MIDI note on message
void noteOn(byte channel, byte pitch, byte velocity)
{
  // 0x90 is the first of 16 note on channels. Subtract one to go from MIDI's 1-16 channels to 0-15
  channel += 0x90 - 1;

  // Ensure we're between channels 1 and 16 for a note on message
  if (channel >= 0x90 && channel <= 0x9F)
  {
#ifdef DEBUG
    Serial.print("Button pressed: ");
    Serial.println(pitch);
#elif defined(TEENSY_PLUS_PLUS) || defined(TEENSY_2) || defined(TEENSY_PLUS_PLUS_2)
    usbMIDI.sendNoteOn(pitch, velocity, channel);
#else
    Serial.write(channel);
    Serial.write(pitch);
    Serial.write(velocity);
#endif
  }
}

// Send a MIDI note off message
void noteOff(byte channel, byte pitch)
{
  // 0x80 is the first of 16 note off channels. Subtract one to go from MIDI's 1-16 channels to 0-15
  channel += 0x80 - 1;

  // Ensure we're between channels 1 and 16 for a note off message
  if (channel >= 0x80 && channel <= 0x8F)
  {
#ifdef DEBUG
    Serial.print("Button released: ");
    Serial.println(pitch);
#elif defined(TEENSY_PLUS_PLUS) || defined(TEENSY_2) || defined(TEENSY_PLUS_PLUS_2)
    usbMIDI.sendNoteOff(pitch, 0x00, channel);
#else
    Serial.write(channel);
    Serial.write(pitch);
    Serial.write((byte)0x00);
#endif
  }
}

// Send a MIDI control change message
void controlChange(byte channel, byte control, byte value)
{
  // 0xB0 is the first of 16 control change channels. Subtract one to go from MIDI's 1-16 channels to 0-15
  channel += 0xB0 - 1;

  // Ensure we're between channels 1 and 16 for a CC message
  if (channel >= 0xB0 && channel <= 0xBF)
  {
#ifdef DEBUG
    Serial.print(control - MIDI_CC);
    Serial.print(": ");
    Serial.println(value);
#elif defined(TEENSY_PLUS_PLUS) || defined(TEENSY_2) || defined(TEENSY_PLUS_PLUS_2)
    usbMIDI.sendControlChange(control, value, channel);
#else
    if (control < 41)
    {
      Serial.write(channel);
      Serial.write(control);
    }
    else if (40 < control && control < 81)
    {
      Serial.write((channel + 1));
      Serial.write((control - 40));
    }
    else if (81 <= control)
    {
      Serial.write((channel + 2));
      Serial.write((control - 80));
    }
    Serial.write(value);
#endif
    // delayMicroseconds(208);        // ?
  }
}

void mcpInit()
{
  // 1 - Set everything as input
  Wire.beginTransmission(0x22);
  Wire.write(0x00); // set MCP23017 memory pointer to IODIRA address
  Wire.write(0xFF);
  Wire.endTransmission();
  Wire.beginTransmission(0x22);
  Wire.write(0x01); // set MCP23017 memory pointer to IODIRB address
  Wire.write(0xFF);
  Wire.endTransmission();

  // 2 - Set normal polarity
  Wire.beginTransmission(0x22);
  Wire.write(0x02); // set MCP23017 memory pointer to IPOLA address
  Wire.write(0x00);
  Wire.endTransmission();
  Wire.beginTransmission(0x22);
  Wire.write(0x03); // set MCP23017 memory pointer to IPOLB address
  Wire.write(0x00);
  Wire.endTransmission();


  // 3 - Activate the interrupts on each ports
  Wire.beginTransmission(0x22);
  Wire.write(0x04); // set MCP23017 memory pointer to GPINTENA address
  Wire.write(0xFF);
  Wire.endTransmission();
  Wire.beginTransmission(0x22);
  Wire.write(0x05); // set MCP23017 memory pointer to GPINTENB address
  Wire.write(0xFF);
  Wire.endTransmission();



  // 4 - Define a default value : Nothing to do, UNUSED


  // 5 - Set interrupt on change for each ports
  Wire.beginTransmission(0x22);
  Wire.write(0x09); // set MCP23017 memory pointer to INTCONA address
  Wire.write(0x00);
  Wire.endTransmission();
  Wire.beginTransmission(0x22);
  Wire.write(0x09); // set MCP23017 memory pointer to INTCONB address
  Wire.write(0x00);
  Wire.endTransmission();

  // 6 - Configuration register : BANK = 0, MIRROR = 1, SEQ = 0, DIS = 0, HAE = X, ODR = 0, INTPOL = 0, U0 = X
  Wire.beginTransmission(0x22);
  Wire.write(0x0A); // set MCP23017 memory pointer to IOCONA/B address
  Wire.write(0x42);
  Wire.endTransmission();


  // 7 - Set Pullup everywhere
  Wire.beginTransmission(0x22);
  Wire.write(0x0C); // set MCP23017 memory pointer to GPPUA address
  Wire.write(0xFF);
  Wire.endTransmission();
  Wire.beginTransmission(0x22);
  Wire.write(0x0D); // set MCP23017 memory pointer to GPPUB address
  Wire.write(0xFF);
  Wire.endTransmission();

  // Read 1 time the GPIOs
  Wire.beginTransmission(0x22);
  Wire.write(0x12); // set MCP23017 memory pointer to GPIOB address
  Wire.endTransmission();
  Wire.requestFrom(0x22, 1);
  regA = Wire.read();
  regA_2 = regA;

  Wire.beginTransmission(0x22);
  Wire.write(0x13); // set MCP23017 memory pointer to GPIOB address
  Wire.endTransmission();
  Wire.requestFrom(0x22, 1);
  regB = Wire.read();
  regB_2 = regB;
}

void routineInterruption()
{
  flag = 1;
}

void routineInterruption2()
{
  flag2 = 1;
}


void ifFlag()
{
  Wire.beginTransmission(0x22);
  Wire.write(0x12); // set MCP23017 memory pointer to GPIOB address
  Wire.endTransmission();
  Wire.requestFrom(0x22, 1);
  regA_2 = Wire.read();

  Wire.beginTransmission(0x22);
  Wire.write(0x13); // set MCP23017 memory pointer to GPIOB address
  Wire.endTransmission();
  Wire.requestFrom(0x22, 1);
  regB_2 = Wire.read();
  flag = 0;
}

void crackByte(int regS, byte variable[16])
{
  int i;
  for (i = 0; i < 16; ++i)
  {
    variable[i] = regS & 1;
    regS = regS >> 1;
  }
}

void mcp2Init()
{
  // 1 - Set everything as input
  Wire.beginTransmission(0x20);
  Wire.write(0x00); // set MCP23017 memory pointer to IODIRA address
  Wire.write(0x1F);
  Wire.endTransmission();
  Wire.beginTransmission(0x20);
  Wire.write(0x01); // set MCP23017 memory pointer to IODIRB address
  Wire.write(0xF3);
  Wire.endTransmission();

  // 2 - Set normal polarity
  Wire.beginTransmission(0x20);
  Wire.write(0x02); // set MCP23017 memory pointer to IPOLA address
  Wire.write(0x00);
  Wire.endTransmission();
  Wire.beginTransmission(0x20);
  Wire.write(0x03); // set MCP23017 memory pointer to IPOLB address
  Wire.write(0x00);
  Wire.endTransmission();


  // 3 - Activate the interrupts on each ports
  Wire.beginTransmission(0x20);
  Wire.write(0x04); // set MCP23017 memory pointer to GPINTENA address
  Wire.write(0x1F);
  Wire.endTransmission();
  Wire.beginTransmission(0x20);
  Wire.write(0x05); // set MCP23017 memory pointer to GPINTENB address
  Wire.write(0xF3);
  Wire.endTransmission();



  // 4 - Define a default value : Nothing to do, UNUSED


  // 5 - Set interrupt on change for each ports
  Wire.beginTransmission(0x20);
  Wire.write(0x09); // set MCP23017 memory pointer to INTCONA address
  Wire.write(0x00);
  Wire.endTransmission();
  Wire.beginTransmission(0x20);
  Wire.write(0x09); // set MCP23017 memory pointer to INTCONB address
  Wire.write(0x00);
  Wire.endTransmission();

  // 6 - Configuration register : BANK = 0, MIRROR = 1, SEQ = 0, DIS = 0, HAE = X, ODR = 0, INTPOL = 0, U0 = X
  Wire.beginTransmission(0x20);
  Wire.write(0x0A); // set MCP23017 memory pointer to IOCONA/B address
  Wire.write(0x42);
  Wire.endTransmission();


  // 7 - Set Pullup everywhere
  Wire.beginTransmission(0x20);
  Wire.write(0x0C); // set MCP23017 memory pointer to GPPUA address
  Wire.write(0x1F);
  Wire.endTransmission();
  Wire.beginTransmission(0x20);
  Wire.write(0x0D); // set MCP23017 memory pointer to GPPUB address
  Wire.write(0xF3);
  Wire.endTransmission();

  // Read 1 time the GPIOs
  Wire.beginTransmission(0x20);
  Wire.write(0x12); // set MCP23017 memory pointer to GPIOB address
  Wire.endTransmission();
  Wire.requestFrom(0x20, 1);
  regA_nd = Wire.read();
  regA_nd_2 = regA_nd;

  Wire.beginTransmission(0x20);
  Wire.write(0x13); // set MCP23017 memory pointer to GPIOB address
  Wire.endTransmission();
  Wire.requestFrom(0x20, 1);
  regB_nd = Wire.read();
  regB_nd_2 = regB_nd;
}

void ifFlag2()
{
  Wire.beginTransmission(0x20);
  Wire.write(0x12); // set MCP23017 memory pointer to GPIOB address
  Wire.endTransmission();
  Wire.requestFrom(0x20, 1);
  regA_nd_2 = Wire.read();

  Wire.beginTransmission(0x20);
  Wire.write(0x13); // set MCP23017 memory pointer to GPIOB address
  Wire.endTransmission();
  Wire.requestFrom(0x20, 1);
  regB_nd_2 = Wire.read();
  flag2 = 0;
}
void mcpLedInit(void)
{
  Wire.beginTransmission(0x26);
  Wire.write(0x00); // IODIRA register
  Wire.write(0x00); // set all of port A to outputs
  Wire.endTransmission();

  Wire.beginTransmission(0x26);
  Wire.write(0x01); // IODIRA register
  Wire.write(0x00); // set all of port A to outputs
  Wire.endTransmission();

  Wire.beginTransmission(0x23);
  Wire.write(0x00); // IODIRA register
  Wire.write(0x00); // set all of port A to outputs
  Wire.endTransmission();

  Wire.beginTransmission(0x23);
  Wire.write(0x01); // IODIRA register
  Wire.write(0x00); // set all of port A to outputs
  Wire.endTransmission();

  Wire.beginTransmission(0x24);
  Wire.write(0x00); // IODIRA register
  Wire.write(0x00); // set all of port A to outputs
  Wire.endTransmission();

  Wire.beginTransmission(0x24);
  Wire.write(0x01); // IODIRA register
  Wire.write(0x00); // set all of port A to outputs
  Wire.endTransmission();

  Wire.beginTransmission(0x21);
  Wire.write(0x00); // IODIRA register
  Wire.write(0x00); // set all of port A to outputs
  Wire.endTransmission();

  Wire.beginTransmission(0x21);
  Wire.write(0x01); // IODIRA register
  Wire.write(0x00); // set all of port A to outputs
  Wire.endTransmission();
}