/* Pinout Config */
#define SEL_A_IN    D2
#define SEL_A_LED   D3
#define SEL_B_IN    D4
#define SEL_B_LED   D5
#define SEL_C_IN    D6
#define SEL_C_LED   D7
#define SEL_D_IN    D8
#define SEL_D_LED   D9

#define SUST_IN     D10
#define EXPR_IN     A0

/* MIDI codes */
#define NOTE_OFF       0x8
#define NOTE_ON        0x9
#define AFTERTOUCH     0xA
#define CONTROL_CHANGE 0xB
#define PATCH_CHANGE   0xC
#define CHANNEL_PRESSURE 0xD
#define PITCH_BEND     0xE
#define OTHER          0xF

void MIDImessage(int cmd_id, int data1, int data2) {
  int channel = 0;
  int cmd_byte = (cmd_id << 4) | (channel & 0xf);
  Serial.write(cmd_byte);//send command byte
  Serial.write(data1);//send data byte #1
  Serial.write(data2);//send data byte #2
}

void MIDImessage2(int cmd_id, int data1) {
  int channel = 0;
  int cmd_byte = (cmd_id << 4) | (channel & 0xf);
  Serial.write(cmd_byte);//send command byte
  Serial.write(data1);//send data byte #1
}

void setup() {
  pinMode( SEL_A_IN, INPUT_PULLUP );
  pinMode( SEL_A_LED, OUTPUT );
  pinMode( SEL_B_IN, INPUT_PULLUP );
  pinMode( SEL_B_LED, OUTPUT );
  pinMode( SEL_C_IN, INPUT_PULLUP );
  pinMode( SEL_C_LED, OUTPUT );
  pinMode( SEL_D_IN, INPUT_PULLUP );
  pinMode( SEL_D_LED, OUTPUT );

  pinMode( SUST_IN, INPUT_PULLUP );
  pinMode( EXPR_IN, INPUT_PULLUP );

  Serial.begin(31250);
}

unsigned sust_stable_count = 0;
unsigned last_sust_val = 0;
float last_expr_val = 0.0;
unsigned last_program_sel = -1;

void loop() {
  delay(1);


  /* Sustain
   */

  if( digitalRead( SUST_IN ) == LOW ) {
    if( last_sust_val == 1 ) {
      if( sust_stable_count == 10 ) {
        // Sustain On
        MIDImessage( CONTROL_CHANGE, 0x42, 0x7f );
        sust_stable_count ++;
      } else if ( sust_stable_count < 10 ) {
        sust_stable_count ++;
      }
    } else {
      // sustain pressed down
      last_sust_val = 1;
      sust_stable_count = 0;
    }
  } else {
    if( last_sust_val == 0 ) {
      if( sust_stable_count == 10 ) {
        // Sustain Off
        MIDImessage( CONTROL_CHANGE, 0x42, 0x00 );
        sust_stable_count ++;
      } else if ( sust_stable_count < 10 ) {
        sust_stable_count ++;
      }
    } else {
      // sustain released
      last_sust_val = 0;
      sust_stable_count = 0;
    }
  }

  /* Expression
   */

  int expr_val_in = analogRead( EXPR_IN );
  float expr_val_f = (float)(expr_val_in - 66) / 590.0;
  if( expr_val_f < 0.0 ){
    expr_val_f = 0.0;
  }
  if( expr_val_f > 1.0 ){
    expr_val_f = 1.0;
  }

  if( fabs(last_expr_val - expr_val_f) > 0.0086 ) {
    last_expr_val = expr_val_f;
    unsigned expr_val = (unsigned)(expr_val_f * 128.0);
    MIDImessage( CONTROL_CHANGE, 0x0b, expr_val );
  }

  /* Program Select
   */

  if( digitalRead( SEL_A_IN ) == LOW ) {
    if( last_program_sel != 0 ) {
      last_program_sel = 0;
      MIDImessage2( PATCH_CHANGE, 0x0 );
      digitalWrite( SEL_A_LED, HIGH );
      digitalWrite( SEL_B_LED, LOW );
      digitalWrite( SEL_C_LED, LOW );
      digitalWrite( SEL_D_LED, LOW );
    }
  }
  if( digitalRead( SEL_B_IN ) == LOW ) {
    if( last_program_sel != 1 ) {
      last_program_sel = 1;
      MIDImessage2( PATCH_CHANGE, 0x1 );
      digitalWrite( SEL_A_LED, LOW );
      digitalWrite( SEL_B_LED, HIGH );
      digitalWrite( SEL_C_LED, LOW );
      digitalWrite( SEL_D_LED, LOW );
    }
  }
  if( digitalRead( SEL_C_IN ) == LOW ) {
    if( last_program_sel != 2 ) {
      last_program_sel = 2;
      MIDImessage2( PATCH_CHANGE, 0x2 );
      digitalWrite( SEL_A_LED, LOW );
      digitalWrite( SEL_B_LED, LOW );
      digitalWrite( SEL_C_LED, HIGH );
      digitalWrite( SEL_D_LED, LOW );
    }
  }
  if( digitalRead( SEL_D_IN ) == LOW ) {
    if( last_program_sel != 3 ) {
      last_program_sel = 3;
      MIDImessage2( PATCH_CHANGE, 0x3 );
      digitalWrite( SEL_A_LED, LOW );
      digitalWrite( SEL_B_LED, LOW );
      digitalWrite( SEL_C_LED, LOW );
      digitalWrite( SEL_D_LED, HIGH );
    }
  }
}