#include <stdio.h>
#include <dos.h>
#include "kbd.h"

static void interrupt (*oldKbdISR)() = NULL;

void kbd_cleanup() {
  if (oldKbdISR != NULL) {
    setvect(KBD_INT, oldKbdISR);
    oldKbdISR = NULL;
  }
}

volatile unsigned char keybuf[128] = { 0 };
volatile char kbd_triggered = 0;

static void interrupt kbd_isr() {
  unsigned char raw;
  char ctl;

  disable();

  raw = inp(0x60);
  ctl = inp(0x61) | 0x82;
  outp(0x61, ctl);
  outp(0x61, ctl & 0x7f);

  if (raw & 0x80) {
    keybuf[raw & 0x7f] &= ~KEY_SIGNAL;
  } else {
    keybuf[raw] |= KEY_SIGNAL;
  }
  kbd_triggered = raw;
  outp(0x20, 0x20);
  enable();
}

unsigned char kbd_wait() {
  kbd_triggered = 0;
  while (!kbd_triggered) {}
  return kbd_triggered;
}

void kbd_init() {
  if (oldKbdISR == NULL) {
    memset(keybuf, 0, 128);
    oldKbdISR = getvect(KBD_INT);
    setvect(KBD_INT, kbd_isr);
    atexit(kbd_cleanup);
  }
}

void kbd_debounce() {
  int i = 0;

  disable();
  for (i = 0; i < 128; i ++) {
    unsigned char signal = keybuf[i] & KEY_SIGNAL;
    unsigned char keystate = keybuf[i] & 0x0f;

    if (!signal) {
      if (keystate == KEY_RELEASED) {
	keystate = KEY_OFF;
      } else if (keystate != KEY_OFF) {
	keystate = KEY_RELEASED;
      }
    } else {
      if (keystate == KEY_OFF) {
	keystate = KEY_PRESSED;
      } else if (keystate == KEY_PRESSED) {
	keystate = KEY_DOWN;
      }
    }

    keybuf[i] = signal | keystate;
  }
  enable();
}