#include <TM1638plus_Model2.h>
#include <elapsedMillis.h>
#include <limits.h>

#define STROBE_TM 9
#define CLOCK_TM 8
#define DIO_TM 7
#define MIDI_CH_BIT3 5
#define MIDI_CH_BIT2 4
#define MIDI_CH_BIT1 3
#define MIDI_CH_BIT0 2

bool swap_nibbles = false;
bool high_freq = false;

#define BRIGHTNESS 6
#define INITIAL_MIDI_CHANNEL 1
#define BUTTON_DEBOUNCE_TIME_MS 50
#define BUTTON_DOUBLE_PRESS_MS 200
#define BLINK_FREQUENCY_MS 500

// Constructor object
TM1638plus_Model2 tm(STROBE_TM, CLOCK_TM, DIO_TM, swap_nibbles, high_freq);
elapsedMillis button_debounce_timer;
elapsedMillis blink_timer;
elapsedMillis double_press_timer;

void setup() {
  Serial.begin(9600);
  delay(100);

  Serial.println("<SETUP>");

  tm.displayBegin();
  tm.brightness(BRIGHTNESS);
  tm.DisplayStr("MIDIHOST", 0);

  pinMode(MIDI_CH_BIT3, OUTPUT);
  pinMode(MIDI_CH_BIT2, OUTPUT);
  pinMode(MIDI_CH_BIT1, OUTPUT);
  pinMode(MIDI_CH_BIT0, OUTPUT);

  SetMidiChannel(INITIAL_MIDI_CHANNEL);

  delay(500);

  Serial.println("</SETUP>");
}

void loop() {
  uint8_t button;
  static uint8_t actual_channel;
  static uint8_t new_channel;

  if (actual_channel == 0) {
    actual_channel = 1;
    tm.DisplayDecNum(actual_channel, 0, false, TMAlignTextRight);
  }

  button = tm.ReadKey16();

  if (button > 0 && button_debounce_timer > BUTTON_DEBOUNCE_TIME_MS && double_press_timer > BUTTON_DOUBLE_PRESS_MS) {
    // Button pressed once
    Serial.print("Button once: ");
    Serial.println(button, DEC);
    button_debounce_timer = 0;
    blink_timer = 0;
    new_channel = button;
    tm.DisplayDecNumNibble(new_channel, actual_channel, 0, false, TMAlignTextRight);
  } else if (button > 0 && button_debounce_timer > BUTTON_DEBOUNCE_TIME_MS && double_press_timer < BUTTON_DOUBLE_PRESS_MS && button == new_channel) {
    // Button pressed twice
    Serial.print("Button twice: ");
    Serial.println(button, DEC);
    actual_channel = new_channel;
    SetMidiChannel(actual_channel);
    double_press_timer = 0;
  } else if (actual_channel != new_channel)
    if (blink_timer > BLINK_FREQUENCY_MS) {
      blink_timer = 0;
      tm.DisplayDecNum(actual_channel, 0, false, TMAlignTextRight);
    } else if (blink_timer > BLINK_FREQUENCY_MS / 2) {
      tm.DisplayDecNumNibble(new_channel, actual_channel, 0, false, TMAlignTextRight);
    }
}

void SetMidiChannel(uint8_t channel) {
  Serial.print("Setting MIDI channel: ");
  Serial.print(channel, DEC);
  channel--;
  Serial.print(" (");
  printBinary(channel, 4);
  Serial.println(")");

  digitalWrite(MIDI_CH_BIT0, channel & 0x00);
  digitalWrite(MIDI_CH_BIT1, channel & 0x01);
  digitalWrite(MIDI_CH_BIT2, channel & 0x02);
  digitalWrite(MIDI_CH_BIT3, channel & 0x08);
}

void printBinary(uint32_t value, uint8_t len) {
  for (len; len >= 0; len--) {
    Serial.print((char)('0' + ((value >> len) & 1)));
  }
  Serial.println();
}