/*
*     Example Code Two
*/
#include "FIRMWARE_DEF.h"
#include "FIRMWARE.h"

static byte pins_keyboard_row[N_KEYBOARD_ROW] = {PIN_K_R0, PIN_K_R1, PIN_K_R2, PIN_K_R3};
static byte pins_keyboard_col[N_KEYBOARD_COL] = {PIN_K_C0, PIN_K_C1, PIN_K_C2};

Keyboard keyboard(N_KEYBOARD_ROW, N_KEYBOARD_COL, pins_keyboard_row, pins_keyboard_col);

Adafruit_MCP4728 MCP4728;
MCP4728_channel_t cvMap[N_CV_GATES] = {MCP4728_CHANNEL_A, MCP4728_CHANNEL_B};
uint16_t keyToVoltage[N_KEYBOARD_ROW*N_KEYBOARD_COL] = {
    1*83, 5*83, 9*83,
    2*83, 6*83, 10*83,
    3*83, 7*83, 11*83,
    4*83, 8*83, 12*83
};

CV cv(&MCP4728, &Wire, N_CV_GATES, cvMap, keyToVoltage, N_KEYBOARD_ROW, N_KEYBOARD_COL);

void setup()
{
  Serial.begin(BAUDRATE);
  keyboard.begin();
  cv.begin(PIN_SDA, PIN_SCL);
}

void loop() 
{
  keyboard.update();

  int n = keyboard.getQueueLength();

  if(n > 0)
  {
    Serial.printf("\n\rCurrent queue length: %i", n);
    if(n == 1) 
    {
      cv.setVoltage(0, keyboard.getQueue(0));
      cv.setVoltage(1, NOT_A_KEY);
    }
    else if(n >= 2) 
    {
      cv.setVoltage(0, keyboard.getQueue(0));
      cv.setVoltage(1, keyboard.getQueue(1));
    }

    for(int i = 0; (i < N_CV_GATES) && (i < n); i++)
    {
      Key k = keyboard.getQueue(i);
      if(isNotKey(k)) Serial.printf("\n\rQueue position %i: NOT A KEY", i);
      else Serial.printf("\n\rQueue position %i: R%iC%i", i, k.row, k.col);
    }
  }
  else cv.clearAll();

  delay(50);
}