#include "base_midi.h"

#include "driverlib/uart.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "inc/hw_uart.h"
#include "inc/hw_ints.h"
#include "inc/hw_memmap.h"
#include <math.h>

void UART1IntHandler(void) {
  UARTIntClear(UART1_BASE, UART_INT_RX);
  while (UARTCharsAvail(UART1_BASE)) {
    parse_midi_stream(UARTCharGet(UART1_BASE));
  }
}

void UARTWrite(uint8_t byte) {
  UARTCharPut(UART1_BASE, byte);
}

static void nothing0(void) {}
static void nothing1(uint8_t) {}
static void nothing2(uint8_t, uint8_t) {}
static void nothing3(uint8_t, uint8_t, uint8_t) {}
static bool quit_parsing(uint8_t) { // TODO: fix library parsers so this works.
  return false;
}


typedef struct {
  uint8_t head;
  uint16_t array[256];
} AxisRingbuf;

inline void ringbuf_advance(AxisRingbuf f) {
  f.array[head] = 0;
  ++f.head;
}

inline void ringbuf_set(AxisRingbuf f, uint8_t idx, uint16_t val) {
  f.array[head + idx] = val;
}

inline uint16_t ringbuf_get(AxisRingbuf f, unit8_t idx) {
  return f.array[head + idx];
}

typedef struct {
  bool expecting_type;
  uint8_t received_type;
  AxisRingbuf outstanding_presses;
} AxisParse;

bool clock_high = false;
uint32_t clock_count = 0;
uint8_t preparing_row_byte = 0;
uint8_t preparing_col_byte = 0;

AxisParse column = {true, 0, {0, {0}}};
AxisParse row = {true, 0, {0, {0}}};


typedef struct {
  uint8_t column;
  uint8_t row;
  uint32_t clock_count;
} Keypress;

typedef struct {
  uint8_t head;
  Keypress presses[32];
} PressBuffer;

PressBuffer outstanding;

void press_add(uint8_t column, uint8_t row) {
  outstanding.presses[outstanding.head + 31 % 32] = {column, row, clock_count};
}

uint32_t press_search(uint8_t column, uint8_t row) {
  for (int i = 0; i < 32; i++) {
    Keypress test = outstanding.presses[outstanding.head + i % 32];
    if (test.column == column && test.row == row)
      return clock_count - test.clock_count;
  }

  return 0;

}

void press_advance() {
  outstanding.presses[outstanding.head] = 0;
  if (outstanding.head == 31)
    outstanding.head = 0;
  else
    ++outstanding.head;
}


#define KEY_PRESSED  0b1000'0000
#define KEY_RELEASED 0b1010'0000
#define VEL_PRESSED  0b1100'0000
#define VEL_RELEASED 0b1110'0000
double uint32_to_uint7 = 1.0 * 127 / UINT32_MAX;
void key_handler(uint8_t type, uint8_t column, uint8_t row) {
  switch (type) {
  case KEY_PRESSED:
    press_add(column, row);
    break;
  case KEY_RELEASED:
    note_off(row, column);
    break;
  case VEL_PRESSED:
    // TODO: think about exponential response/timing analysis wrt final hardware.
    note_on(row, column, floor(uint32_to_uint7 * press_search(column, row) + 0.5));
    break;
  case VEL_RELEASED:
    break; // No release velocity for now.
  }

  press_advance();

}

void column_handler(uint8_t byte) {
  if (column.expecting_type) {
    if (byte > 0b1000'0000) {
      column.received_type = byte;
      column.expecting_type = false;
    }
  } else {
    uint16_t corresp = ringbuf_get(row.outstanding_presses, byte - 1);
    if (coresp >> 8 == column.received_type) // Found other coord.
      key_handler(column.received_type, byte, (uint8_t)corresp);
    else
      ringbuf_set(column.outstanding_presses, byte - 1, (received_type << 8) | byte);

    column.expecting_type = true;
  }

  ringbuf_advance(column.outstanding_presses);

}

void row_handler(uint8_t byte) {
  if (row.expecting_type) {
    if (byte > 0b1000'0000) {
      row.received_type = byte;
      row.expecting_type = false;
    }
  } else {
    uint16_t corresp = ringbuf_get(column.outstanding_presses, byte - 1);
    if (coresp >> 8 == row.received_type) // Found other coord.
      key_handler(row.received_type, (uint8_t)corresp, byte);
    else
      ringbuf_set(row.outstanding_presses, byte - 1, (received_type << 8) | byte);

    row.expecting_type = true;
  }

  ringbuf_advance(row.outstanding_presses);

}

void systick_handler(void) {
  if (clock_high) {
    uint32_t port_state = GPIOPinRead(GPIO_PORTE_BASE, GPIO_PIN_2 | GPIO_PIN_1);
    GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_0, 0);
    uint8_t row_pin_state = (port_state >> GPIO_PIN_1) & 0x01;
    uint8_t col_pin_state = (port_state >> GPIO_PIN_2) & 0x01;
    uint8_t bit = clock_count % 8;
    preparing_row_byte |= (row_pin_state) << bit;
    preparing_col_byte |= (col_pin_state) << bit;
    if (bit == 7) {
      row_handler(preparing_row_byte);
      column_handler(preparing_column_byte);
      preparing_row_byte = 0;
      preparing_column_byte = 0;
    }
    ++clock_count;
    clock_high = false;
  } else {
    GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_0, 1);
    clock_high = true;
  }
}


int main() {
  // Set main clock to 80MHz.
  SysCtlClockSet(SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_SYSDIV_2_5 | SYSCTL_XTAL_16MHZ);

  SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);

  // Initialize UART1.
  SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1);


  while(!SysCtlPeripheralReady(SYSCTL_PERIPH_UART1));

  // Initialize the GPIO pins for UART1.
  GPIOPinConfigure(GPIO_PC4_U1RX);
  GPIOPinConfigure(GPIO_PC5_U1TX);
  GPIOPinTypeUART(GPIO_PORTC_BASE, GPIO_PIN_4 | GPIO_PIN_5);

  // Configure the UART using the specifications found in the minipix_uart_interface.
  // Cource code: word length - 8, 1 stop bit, no parity.
  UARTConfigSetExpClk(UART1_BASE, SysCtlClockGet(), 31250, UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE);
  UARTEnable(UART1_BASE);
  UARTIntEnable(UART1_BASE, UART_INT_RX);
  UARTIntRegister(UART1_BASE, UART1IntHandler);

  // Set up MIDI 1.0 library to ignore basically everything.
  ConsumerBehavior new_pfns = {.uart_write = UARTWrite,
			       .note_on_handler = nothing3,
			       .note_off_handler = nothing3,
			       .poly_key_handler = nothing3,
			       .control_change_handler = nothing3,
			       .program_change_handler = nothing2,
			       .all_sound_off_handler = nothing1,
			       .local_control_handler = nothing2,
			       .all_notes_off_handler = nothing1,
			       .poly_on_handler = nothing1,
			       .mtc_quarter_frame_handler = nothing2,
			       .song_position_pointer_handler = nothing2,
			       .song_select_handler = nothing1,
			       .tune_request_handler = nothing0,
			       .timing_clock_handler = nothing0,
			       .start_handler = nothing0,
			       .continue_handler = nothing0,
			       .stop_handler = nothing0,
			       .active_sensing_handler = nothing0,
			       .system_reset_handler = nothing0, // TODO: change.
			       .sysex_collector = quit_parsing,
			       .end_of_sysex_handler = nothing0,
			       /* .bulk_tuning_dump_request_handler = nothing, // TODO: change. */
			       /* .bulk_tuning_dump_handler = nothing, */
			       /* .single_note_tuning_change_handler = nothing, */
			       .unimplemented_universal_sysex_collector = quit_parsing};

  midi_init(new_pfns);

  // Must emulate Atmel's three-wire serial in software with GPIO: we need to count SPI clock cycles.
  // Uses SysTick to manage both clocks.
  SysTickPeriodSet(320); // System clock is 80MHz, so for bus clock of 500kHz set to 160---double for both edges.
  SysTickEnable();
  SysTickIntEnable();
  SysTickIntRegister(systick_handler);


  // TODO: understand the various pad config and what is expected by the slaves.
  GPIOPinTypeGPIOInput(GPIO_PORTE_BASE, GPIO_PIN_0); // Row.
  GPIOPinTypeGPIOInput(GPIO_PORTE_BASE, GPIO_PIN_1); // Column.
  GPIOPinTypeGPIOOutput(GPIO_PORTE_BASE, GPIO_PIN_2); // Clock.
  GPIOPadConfigSet(GPIO_PORTE_BASE, GPIO_PIN_2, GPIO_STRENGTH_4MA, GPIO_PIN_TYPE_STD);

  while(true); // Main application loop.

  // TODO: think about making slaves dependent on master for power, so resetting master resets slaves.
  // Possibly cycle a transistor here?
}