// #include <sys/_types.h>
#include <sys/_types.h>
#include <xgpio.h>
#include <xil_printf.h>
#include <xil_types.h>
#include <xil_exception.h>
#include <xintc_l.h>
#include <xstatus.h>
#include <xparameters.h>
#include <xintc.h>

#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>

#define NOTE_ID XPAR_AXI_GPIO_AUX_OUT_BASEADDR
#define PS2_ID XPAR_AXI_GPIO_PS2_IN_BASEADDR
#define AUX_CHANNEL 1
#define PS2_CHANNEL 1
#define AUX_MASK 0xff
#define PS2_MASK 0xff

#define LOWER_NOTE_INDEX 0
#define UPPER_NOTE_INDEX 7

#define NOTE_F 0
#define NOTE_G 1
#define NOTE_A 2
#define NOTE_B 3
#define NOTE_C 4
#define NOTE_D 5
#define NOTE_E 6
#define NOTE_F_HIGH 7

#define KEYCODE_F 0x2b
#define KEYCODE_G 0x34
#define KEYCODE_A 0x1c
#define KEYCODE_B 0x32
#define KEYCODE_C 0x21
#define KEYCODE_D 0x23
#define KEYCODE_E 0x24
#define KEYCODE_UP 0xf0

#define TIMER_AANTAL 2
#define PlayNoteTimer 0
#define WrongNoteTimer 1

XIntc int_ctrl;
XGpio note_device, ps2_device;

// game state
bool game_continue = true;
bool correct_note = false;
uint32_t target_note = 0;

// quick 'n dirty delay
void sleep(unsigned long millis) {
  unsigned arbitary_multiplier = 1000;
  xil_printf("%lums delay ...", millis);
  for (unsigned long i = 0; i < millis * arbitary_multiplier; i++)
    asm("nop");
  xil_printf(" done\r\n");
}

// top.vhd NOTE_IDX
#define NOTE_IDX_MASK ((uint32_t) (0x000f))
// top.vhd NOTE_PLAY
#define NOTE_PLAY_BIT ((uint32_t) (1 << 4))
// top.vhd NOTE_WRONG
#define NOTE_WRONG_BIT ((uint32_t) (1 << 5))

uint32_t note_output = 0;
void _set_note_idx(uint32_t idx) {
  note_output &= ~NOTE_IDX_MASK;
  note_output |= (idx & NOTE_IDX_MASK);
}
void _set_note_play(bool play) {
  note_output &= ~NOTE_PLAY_BIT;
  if (play) note_output |= NOTE_PLAY_BIT;
}
void _set_note_wrong(bool wrong) {
  note_output &= ~NOTE_WRONG_BIT;
  if (wrong) note_output |= NOTE_WRONG_BIT;
}
void _note_flush() {
  XGpio_DiscreteWrite(&note_device, AUX_CHANNEL, note_output);
}

void note_play(uint8_t note) {
  _set_note_idx(note);
  _set_note_play(true);
  _set_note_wrong(false);
  _note_flush();
}
void note_wrong() {
  _set_note_wrong(true);
  _set_note_play(true);
  _note_flush();
}
void note_stop() {
  _set_note_play(false);
  _note_flush();
}

uint32_t note_to_key() {
  if (target_note == NOTE_C) return KEYCODE_C;
  if (target_note == NOTE_D) return KEYCODE_D;
  if (target_note == NOTE_E) return KEYCODE_E;
  if (target_note == NOTE_F) return KEYCODE_F;
  if (target_note == NOTE_F_HIGH) return KEYCODE_F;
  if (target_note == NOTE_G) return KEYCODE_G;
  if (target_note == NOTE_A) return KEYCODE_A;
  if (target_note == NOTE_B) return KEYCODE_B;
  return KEYCODE_F;
}

bool valid_key(uint8_t ps2_input) {
  if (ps2_input == KEYCODE_C) return true;
  if (ps2_input == KEYCODE_D) return true;
  if (ps2_input == KEYCODE_E) return true;
  if (ps2_input == KEYCODE_F) return true;
  if (ps2_input == KEYCODE_G) return true;
  if (ps2_input == KEYCODE_A) return true;
  if (ps2_input == KEYCODE_B) return true;
  return false;
}

void handle_key(uint8_t ps2_input) {
  xil_printf("ps2 0x%02x", ps2_input);

  // only react to keycodes for note names
  if (!valid_key(ps2_input)) {
    xil_printf(" (not a note)\r\n");
    return;
  }

  if (game_continue) {
    xil_printf(" (ignored, game running)\r\n");
    return;
  }

  correct_note = ps2_input == note_to_key();
  xil_printf(" %s\r\n", correct_note ? "CORRECT" : "INCORRECT");
  game_continue = true;
}

void ps2_int() {
  static int ignore_cnt = 0;

  if (ignore_cnt > 0) {
    ignore_cnt--;
    return;
  }

  uint32_t ps2_input = XGpio_DiscreteRead(&ps2_device, PS2_CHANNEL) & PS2_MASK;

  // ignore 0xf0 and next scancode (key up)
  if (ps2_input == 0xf0) {
    ignore_cnt = 1;
    return;
  }

  handle_key(ps2_input);
}

int main() {
  srand(6643);

  XGpio_Config *cfg_ptr;

  cfg_ptr = XGpio_LookupConfig(NOTE_ID);
  XGpio_CfgInitialize(&note_device, cfg_ptr, cfg_ptr->BaseAddress);
  cfg_ptr = XGpio_LookupConfig(PS2_ID);
  XGpio_CfgInitialize(&ps2_device, cfg_ptr, cfg_ptr->BaseAddress);
  XGpio_SetDataDirection(&ps2_device, PS2_CHANNEL, PS2_MASK);
  XGpio_SetDataDirection(&note_device, AUX_CHANNEL, 0);

  // ps2 interrupt
  XIntc_Initialize(&int_ctrl, XPAR_XINTC_0_BASEADDR);
  XIntc_Connect(&int_ctrl, 0x0U, (XInterruptHandler) ps2_int, NULL);
  XIntc_Start(&int_ctrl, XIN_REAL_MODE);
  XIntc_Enable(&int_ctrl, 0x0U);
  Xil_ExceptionInit();
  Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT, (Xil_ExceptionHandler)XIntc_InterruptHandler, &int_ctrl);
  Xil_ExceptionEnable();

  xil_printf("boot'd!\r\n");

  // game loop
  // game_continue is set from the key handler because the interrupt should return asap to prevent repeated calls
  while (1) {
    if (!game_continue) continue;

    if (!correct_note) {
      note_wrong();
      sleep(500);
    }

    target_note = (rand() % (UPPER_NOTE_INDEX - LOWER_NOTE_INDEX + 1)) + LOWER_NOTE_INDEX;
    xil_printf("chose new note: %d\r\n", target_note);
    note_play(target_note);
    sleep(2000);
    note_stop();

    game_continue = false;

  }
}