#include <system.h>
#include <drivers/keyboard.h>

const char KeyMap[] = {
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, '\t', '`', 0,
      0, 0, 0, 0, 0, 'q', '1', 0, 0, 0, 'z', 's', 'a', 'w', '2', 0,
      0, 'c', 'x', 'd', 'e', '4', '3', 0, 0, ' ', 'v', 'f', 't', 'r', '5', 0,
      0, 'n', 'b', 'h', 'g', 'y', '6', 0, 0, 0, 'm', 'j', 'u', '7', '8', 0,
      0, ',', 'k', 'i', 'o', '0', '9', 0, 0, '.', '/', 'l', ';', 'p', '-', 0,
      0, 0, '\'', 0, '[', '=', 0, 0, 0, '\n', '\n', ']', 0, '\\', 0, 0,
      0, 0, 0x7F, 0, 0, 0, '\b', 0, 0, '1', '/', '4', '7', 0, 0, 0,
      '0', '.', '2', '5', '6', '8', 0, 0, 0, '+', '3', '-', '*', '9', 0, 0
      };

const char KeyMapShift[] = {
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, '\t', '~', 0,
      0, 0, 0, 0, 0, 'Q', '!', 0, 0, 0, 'Z', 'S', 'A', 'W', '@', 0,
      0, 'C', 'X', 'D', 'E', '$', '#', 0, 0, ' ', 'V', 'F', 'T', 'R', '%', 0,
      0, 'N', 'B', 'H', 'G', 'Y', '^', 0, 0, 0, 'M', 'J', 'U', '&', '*', 0,
      0, '<', 'K', 'I', 'O', ')', '(', 0, 0, '>', '?', 'L', ':', 'P', '_', 0,
      0, 0, '\"', 0, '{', '+', 0, 0, 0, '\n', '\n', '}', 0, '|', 0, 0,
      0, 0, 0x7F, 0, 0, 0, '\b', 0, 0, '1', '/', '4', '7', 0, 0, 0,
      '0', '.', '2', '5', '6', '8', 0, 0, 0, '+', '3', '-', '*', '9', 0, 0
      };

volatile unsigned char KeyArray[16];
volatile unsigned char KeyboardNewData;
volatile unsigned char KeyModifierStatus;
volatile unsigned char KeyScancodePrefix;
volatile unsigned char KeyLightsStatus;
unsigned char KeyboardScancodeSet;


void KeyboardSetKey(unsigned char scancode, unsigned char val)
{
      unsigned char pos = scancode/8;
      unsigned char offset = scancode%8;
      
      if (val) {
            KeyArray[pos] |= 1<<offset;
            KeyboardNewData = scancode;
            }
      else KeyArray[pos] &= 0xFF - (1<<offset);
}

unsigned char KeyIsPressed(unsigned char scancode)
{
      unsigned char pos = scancode/8;
      unsigned char offset = scancode%8;
      return (KeyArray[pos]&(1<<offset));
}


void i86_KeyboardHandler(ISR_stack_regs *r) {
      unsigned char scancode = inportb(0x60);

      switch (scancode) {
            case 0x00:                                      // Error 0x00
            case 0xFC:                                      // Diagnostics failed (MF kb)
            case 0xFD:                                      // Diagnostics failed (AT kb)
            case 0xFF: KeyboardWaitInput(); outportb(0x60, 0xF4);   // Error 0xFF
                        break;
            case 0xAA:                                      // BAT test successful.
            case 0xFA:                                      // ACKnowledge
            case 0xFE:                                      // Last command invalid or parity error
            case 0xEE: break;                              // Echo response
        // Gray or break
            case 0xE0: KeyScancodePrefix |= 1; break;
            case 0xE1: KeyScancodePrefix |= 4; break;
            case 0xF0: KeyScancodePrefix |= 2; break;

        // Alt, ctrl...
            case 0x11: if ((KeyScancodePrefix&1) == 0) {              // Left alt
                           if ((KeyScancodePrefix&2) == 0) KeyModifierStatus |= 1<<2;
                           else KeyModifierStatus &= 0xFF - (1<<2);
                        }
                        else {                              // Right alt
                           if ((KeyScancodePrefix&2) == 0) KeyModifierStatus |= 1<<3;
                           else KeyModifierStatus &= 0xFF - (1<<3);
                        }
                        KeyScancodePrefix = 0; break;
                        
            case 0x12: if ((KeyScancodePrefix&1) == 0) {              // Left shift
                           if ((KeyScancodePrefix&2) == 0) KeyModifierStatus |= 1<<0;
                           else KeyModifierStatus &= 0xFF - (1<<0);
                        }
                        else {                              // Fake shift
                           if ((KeyScancodePrefix&2) == 0) KeyModifierStatus |= 1<<6;
                           else KeyModifierStatus &= 0xFF - (1<<6);
                        }
                        KeyScancodePrefix = 0; break;
                        
            case 0x14: if (KeyScancodePrefix&4) {           // Pause/break
                              if ((KeyScancodePrefix&2) == 0) KeyboardSetKey (0, 1);
                              else KeyboardSetKey (0, 0);
                              KeyScancodePrefix |= 8; break;
                              }
            
                        else if ((KeyScancodePrefix&1) == 0) {              // Left ctrl
                           if ((KeyScancodePrefix&2) == 0) KeyModifierStatus |= 1<<4;
                           else KeyModifierStatus &= 0xFF - (1<<4);
                        }
                        else {                              // Right ctrl
                           if ((KeyScancodePrefix&2) == 0) KeyModifierStatus |= 1<<5;
                           else KeyModifierStatus &= 0xFF - (1<<5);
                        }
                        KeyScancodePrefix = 0; break;
                        
            case 0x59:                                      // Right shift
                        if ((KeyScancodePrefix&2) == 0) KeyModifierStatus |= 1<<1;
                        else KeyModifierStatus &= 0xFF - (1<<1);

                        KeyScancodePrefix = 0; break;
                        
        // LEDs
            case 0x58: if ((KeyScancodePrefix&2) == 0) {
                  KeyLightsStatus ^= 4; KeyboardSetLEDs(KeyLightsStatus);
                  } KeyScancodePrefix = 0; break; // Caps
                  
            case 0x77:
                  if ((KeyScancodePrefix&4) && (KeyScancodePrefix&8)) KeyScancodePrefix=0;
                  else if ((KeyScancodePrefix&2) == 0) {
                  KeyLightsStatus ^= 2; KeyboardSetLEDs(KeyLightsStatus);
                  } KeyScancodePrefix = 0; break; // Num
            case 0x7E: if ((KeyScancodePrefix&2) == 0) {
                  KeyLightsStatus ^= 1; KeyboardSetLEDs(KeyLightsStatus);
                  } KeyScancodePrefix = 0; break; // Scroll
                        
            case 0x83: scancode = 0x02; // Put F7 under the 0x80 (128bit) barrier
                        
            default: 
                  // Remap gray keys
                  if (KeyScancodePrefix&1) switch (scancode) {
                        case 0x7C: scancode=0x08; break;    // PrintScreen
                        case 0x4A: scancode=0x6A; break;    // Numpad /
                        case 0x5A: scancode=0x59; break;    // Numpad Enter
                        case 0x69: scancode=0x5E; break;    // End
                        case 0x6B: scancode=0x5F; break;    // Left
                        case 0x6C: scancode=0x60; break;    // Home
                        case 0x70: scancode=0x61; break;    // Insert
                        case 0x71: scancode=0x62; break;    // Delete
                        case 0x72: scancode=0x63; break;    // Down
                        case 0x74: scancode=0x64; break;    // Right
                        case 0x75: scancode=0x65; break;    // Up
                        case 0x7A: scancode=0x67; break;    // PageDown
                        case 0x7D: scancode=0x68; break;    // PageUp
                        }
                  if ((KeyScancodePrefix&2) == 0) KeyboardSetKey(scancode, 1);
                  else KeyboardSetKey(scancode, 0);
                  
                  KeyScancodePrefix = 0; break;
      }
            
}



KeyboardKey GetKey()
{
      KeyboardKey ret;
      
      KeyboardNewData = 0xFF;
      while (KeyboardNewData==0xFF);              // wait for keypress
      
      ret.Scancode = KeyboardNewData;          // Send scancode for non-chars
      ret.ModifierStatus = KeyModifierStatus;    // Shift, ctrl... state
      ret.Lights = KeyLightsStatus;      // Num, caps.... state
      
      if ((ret.ModifierStatus & 1) || (ret.ModifierStatus & 2)) // Shift is on
            ret.Character = KeyMapShift[ret.Scancode];
      else ret.Character = KeyMap[ret.Scancode]; // Shift is off
      
      return ret;                         // And send it.
}




/***************************************
 *       Set repeat rate/delay         *
 ***************************************
 Values for inter-character delay (bits 4-0)
 (characters per second; default is 10.9)
     |  0 |  1 |  2 |  3 |  4 |  5 |  6 |  7 
 ----+----+----+----+----+----+----+----+----
  0  |30.0|26.7|24.0|21.8|20.0|18.5|17.1|16.0
  8  |15.0|13.3|12.0|10.9|10.0|9.2 |8.6 |8.0
  16 |7.5 |6.7 |6.0 |5.5 |5.0 |4.6 |4.3 |4.0
  24 |3.7 |3.3 |3.0 |2.7 |2.5 |2.3 |2.1 |2.0
  
 Values for delay:
 (miliseconds; default is 500)
    0 |   1 |   2 |   3
 -----+-----+-----+-----
  250 | 500 | 750 | 1000
  
 ***************************************/ 
void KeyboardSetRepeatRate(unsigned char rate, unsigned char delay)
{
      if (rate>3 || delay>31) return;

      unsigned char out = rate<<5 | delay;
      while ((inportb(0x64)&2) != 0);
      outportb(0x60, 0xF3);
      while ((inportb(0x64)&2) != 0);
      outportb(0x60, out);
}


/***************************************
 *       Set keyboard LEDs             *
 ***************************************
 +-----------+-------+-------+--------+
 | Bits 7-3  | Bit 2 | Bit 1 | Bit 0  |
 |   0       | Caps  | Num   | Scroll |
 |(reserved) |  lock |  lock |  lock  |
 +-----------+-------+-------+--------+
 ***************************************/ 
void KeyboardSetLEDs(unsigned char status)
{
    while ((inportb (0x64)&2)!=0);
    outportb (0x60, 0xED);
  
    while ((inportb (0x64)&2)!=0);
    outportb (0x60, status);
}

/***************************************
 *       Set scancode set              *
 ***************************************
      0  Get current scancode set
      1  Set to scancode set 1
      2  Set to scancode set 2
      3  Set to scancode set 3
 ***************************************/ 
void KeyboardSetScancodeSet(unsigned char set)
{
    if (set>3) return;
    
    while ((inportb (0x64)&2)!=0);
    outportb (0x60, 0xF0);
  
    while ((inportb (0x64)&2)!=0);
    outportb (0x60, set);
    
    KeyboardScancodeSet = set;
}

/*unsigned char i86_kb_get_scancodeset() {
      return KeyboardScancodeSet;
}*/

void KeyboardWaitInput()
{
      int fail_safe=200000;
      while ((inportb(0x64)&2)!=0 && fail_safe>0) fail_safe--;
}

void KeyboardWaitOutput()
{
      int fail_safe=200000;
      while ((inportb(0x64)&1)==0 && fail_safe>0) fail_safe--;
}


void KeyboardInstallA()
{
      KeyboardWaitInput(); outportb(0x60, 0xFF);         // Reset kb
      
      // Initialize variables
      KeyboardNewData = 0;
      KeyModifierStatus = 0;
      KeyScancodePrefix = 0;
      KeyLightsStatus = 0;
      KeyboardScancodeSet = 0;

      memset((void*)KeyArray, 0, 16);
}


void KeyboardInstallB()
{
      // Wait for BAT test results
      unsigned char temp;
      do temp = inportb(0x60);
      while (temp!=0xAA && temp!=0xFC);

      // Error
      if (temp == 0xFC) return;
      
      // Set new repeat rate
      KeyboardSetRepeatRate(1, 11);

      // Set scancode set 2
      KeyboardSetScancodeSet(2);  // Set new scancode set

      KeyboardWaitInput();
      outportb(0x64, 0x20); // Get "Command unsigned char"
      
      do {  temp = inportb(0x60);
      } while (temp==0xFA || temp==0xAA);
      
      temp &= 0xFF - (1<<6); // Set bit6 to 0: disable conversion
      KeyboardWaitInput(); outportb(0x64, 0x60); // Function to write cmd unsigned char
      KeyboardWaitInput(); outportb(0x60, temp); // Send it
}