#include "../System.h"
#include <memory.h>
#include <stdio.h>

uint8_t gbPrinterStatus = 0;
int gbPrinterState = 0;
uint8_t gbPrinterData[0x280 * 9];
uint8_t gbPrinterPacket[0x400];
int gbPrinterCount = 0;
int gbPrinterDataCount = 0;
int gbPrinterDataSize = 0;
int gbPrinterResult = 0;

bool gbPrinterCheckCRC()
{
    uint16_t crc = 0;

    for (int i = 2; i < (6 + gbPrinterDataSize); i++) {
        crc += gbPrinterPacket[i];
    }

    int msgCrc = gbPrinterPacket[6 + gbPrinterDataSize] + (gbPrinterPacket[7 + gbPrinterDataSize] << 8);

    return msgCrc == crc;
}

void gbPrinterReset()
{
    gbPrinterState = 0;
    gbPrinterDataSize = 0;
    gbPrinterDataCount = 0;
    gbPrinterCount = 0;
    gbPrinterStatus = 0;
    gbPrinterResult = 0;
}

void gbPrinterShowData()
{
    systemGbPrint(gbPrinterData,
        gbPrinterDataCount,
        gbPrinterPacket[6],
        gbPrinterPacket[7],
        gbPrinterPacket[8],
        gbPrinterPacket[9]);
    /*
  allegro_init();
  install_keyboard();
  set_gfx_mode(GFX_AUTODETECT, 160, 144, 0, 0);
  PALETTE pal;
  pal[0].r = 255;
  pal[0].g = 255;
  pal[0].b = 255;
  pal[1].r = 168;
  pal[1].g = 168;
  pal[1].b = 168;
  pal[2].r = 96;
  pal[2].g = 96;
  pal[2].b = 96;
  pal[3].r = 0;
  pal[3].g = 0;
  pal[3].b = 0;
  set_palette(pal);
  acquire_screen();
  uint8_t *data = gbPrinterData;
  for(int y = 0; y < 0x12; y++) {
    for(int x = 0; x < 0x14; x++) {
      for(int k = 0; k < 8; k++) {
        int a = *data++;
        int b = *data++;
        for(int j = 0; j < 8; j++) {
          int mask = 1 << (7-j);
          int c = 0;
          if(a & mask)
            c++;
          if(b & mask)
            c+=2;
          putpixel(screen, x*8+j, y*8+k, c);
        }
      }
    }
  }
  release_screen();
  while(!keypressed()) {
  }
  */
}

void gbPrinterReceiveData()
{
    int i = gbPrinterDataCount;
    if (gbPrinterPacket[3]) { // compressed
        uint8_t* data = &gbPrinterPacket[6];
        uint8_t* dest = &gbPrinterData[gbPrinterDataCount];
        int len = 0;
        while (len < gbPrinterDataSize) {
            uint8_t control = *data++;
            if (control & 0x80) { // repeated data
                control &= 0x7f;
                control += 2;
                memset(dest, *data++, control);
                len += 2;
                dest += control;
            } else { // raw data
                control++;
                memcpy(dest, data, control);
                dest += control;
                data += control;
                len += control + 1;
            }
        }
        gbPrinterDataCount = (int)(dest - gbPrinterData);
    } else {
        memcpy(&gbPrinterData[gbPrinterDataCount],
            &gbPrinterPacket[6],
            gbPrinterDataSize);
        gbPrinterDataCount += gbPrinterDataSize;
    }
}

void gbPrinterCommand()
{
    switch (gbPrinterPacket[2]) {
    case 0x01:
        // reset/initialize packet
        gbPrinterDataCount = 0;
        gbPrinterStatus = 0;
        break;
    case 0x02:
        // print packet
        gbPrinterShowData();
        break;
    case 0x04:
        // data packet
        gbPrinterReceiveData();
        break;
    case 0x0f:
        // NUL packet
        break;
    }
}

uint8_t gbPrinterSend(uint8_t b)
{
    switch (gbPrinterState) {
    case 0:
        gbPrinterCount = 0;
        // receiving preamble
        if (b == 0x88) {
            gbPrinterPacket[gbPrinterCount++] = b;
            gbPrinterState++;
        } else {
            // todo: handle failure
            gbPrinterReset();
        }
        break;
    case 1:
        // receiving preamble
        if (b == 0x33) {
            gbPrinterPacket[gbPrinterCount++] = b;
            gbPrinterState++;
        } else {
            // todo: handle failure
            gbPrinterReset();
        }
        break;
    case 2:
        // receiving header
        gbPrinterPacket[gbPrinterCount++] = b;
        if (gbPrinterCount == 6) {
            gbPrinterState++;
            gbPrinterDataSize = gbPrinterPacket[4] + (gbPrinterPacket[5] << 8);
        }
        break;
    case 3:
        // receiving data
        if (gbPrinterDataSize) {
            gbPrinterPacket[gbPrinterCount++] = b;
            if (gbPrinterCount == (6 + gbPrinterDataSize)) {
                gbPrinterState++;
            }
            break;
        }
        gbPrinterState++;
    // intentionally move to next if no data to receive
    case 4:
        // receiving CRC
        gbPrinterPacket[gbPrinterCount++] = b;
        gbPrinterState++;
        break;
    case 5:
        // receiving CRC-2
        gbPrinterPacket[gbPrinterCount++] = b;
        if (gbPrinterCheckCRC()) {
            gbPrinterCommand();
        }
        gbPrinterState++;
        break;
    case 6:
        // receiving dummy 1
        gbPrinterPacket[gbPrinterCount++] = b;
        gbPrinterResult = 0x81;
        gbPrinterState++;
        break;
    case 7:
        // receiving dummy 2
        gbPrinterPacket[gbPrinterCount++] = b;
        gbPrinterResult = gbPrinterStatus;
        gbPrinterState = 0;
        gbPrinterCount = 0;
        break;
    }
    return gbPrinterResult;
}