#include "vga.h"
#include "pc_stuff.h"
#include <malloc.h>
#include <conio.h>
#include <string.h>
#include <stdio.h>
#include <time.h>

#define VGA_SEQUENCE_CONTROLLER_INDEX (0x03C4)
#define VGA_SEQUENCE_CONTROLLER_DATA  (0x03C5)

#define VGA_SEQUENCE_CONTROLLER_MEMORY_MODE (0x04)
#define VGA_SEQUENCE_CONTROLLER_ODD_EVEN_SEQUENTIAL_ACCESS (0x04)
#define VGA_SEQUENCE_CONTROLLER_EXTENDED_MEMORY (0x02)

#define VGA_SEQUENCE_CONTROLLER_MAP_MASK_MODE (0x02)
#define VGA_SEQUENCE_CONTROLLER_ALL_PLANES (0xff)

#define VGA_GRAPHICS_REGISTERS_INDEX (0x03CE)
#define VGA_GRAPHICS_REGISTERS_DATA (0x03CF)

#define VGA_GRAPHICS_DATA_ROTATE (0x03)

#define VGA_CRT_CONTROLLER_INDEX (0x03d4)
#define VGA_CRT_CONTROLLER_DATA (0x03d5)

#define VGA_CRT_CONTROLLER_UNDERLINE_MODE (0x14)
#define VGA_CRT_CONTROLLER_MODE_CONTROL (0x17)

#define VGA_CRT_CONTROLLER_DISPLAY_HIGH_ADDRESS (0x0c)
#define VGA_CRT_CONTROLLER_DISPLAY_LOW_ADDRESS (0x0d)

#define VGA_CRT_CONTROLLER_VERTICAL_RETRACE_START (0x10)
#define VGA_CRT_CONTROLLER_VERTICAL_RETRACE_END (0x11)
#define VGA_CRT_CONTROLLER_VERTICAL_DISPLAY_END (0x12)
#define VGA_CRT_CONTROLLER_VERTICAL_BLANK_START (0x15)
#define VGA_CRT_CONTROLLER_VERTICAL_BLANK_END (0x16)
#define VGA_CRT_CONTROLLER_VERTICAL_TOTAL (0x06)
#define VGA_CRT_CONTROLLER_OVERFLOW (0x07)

int activePage = 0;
byte *drawBuffer, *displayMirrorBuffer;

void initializeDrawBuffer() {
  int i;

  drawBuffer = (byte *)malloc(VGA_DISPLAY_WIDTH * VGA_DISPLAY_HEIGHT);
  displayMirrorBuffer = (byte *)malloc(VGA_DISPLAY_WIDTH * VGA_DISPLAY_HEIGHT);

  for (i = 0; i < VGA_DISPLAY_WIDTH * VGA_DISPLAY_HEIGHT; ++i) {
    drawBuffer[i] = 0;
    displayMirrorBuffer[i] = 0;
  }
}

void freeDrawBuffer() {
  free(drawBuffer);
  free(displayMirrorBuffer);
}

void setActiveVGAMemoryPlanes(int planes) {
  outpw_to_register(
    VGA_SEQUENCE_CONTROLLER_INDEX,
    VGA_SEQUENCE_CONTROLLER_MAP_MASK_MODE,
    planes
  );
}

byte *getDrawBuffer() {
  return drawBuffer;
}

clock_t startTime, endTime;

void drawBufferToUnchainedMemory() {
  int x, y, plane, pageOffset, yOffset, yDisplayOffset;
  byte *vgaStart;
  unsigned long data;

  int rowsToCopy[VGA_DISPLAY_HEIGHT], minRow = -1, maxRow = 0;

  unsigned long *drawBufferLong = (unsigned long *)drawBuffer,
                *displayMirrorBufferLong = (unsigned long *)displayMirrorBuffer;

  byte copyRow[VGA_DISPLAY_WIDTH];

  startTime = clock();

  pageOffset = activePage * PAGE_SIZE;
  vgaStart = (byte *)(VGA + pageOffset);

  for (y = 0; y < VGA_DISPLAY_HEIGHT; ++y) {
    rowsToCopy[y] = 0;
    yOffset = y * VGA_UNCHAINED_LINE_WIDTH;

    for (x = 0; x < VGA_UNCHAINED_LINE_WIDTH; x++) {
      data = *(drawBufferLong++);

      if (displayMirrorBufferLong[yOffset] != data) {
        rowsToCopy[y] = 1;

        break;
      }

      yOffset++;
    }
  }

  memcpy(displayMirrorBuffer, drawBuffer, VGA_DISPLAY_WIDTH * VGA_DISPLAY_HEIGHT);

  for (y = 0; y < VGA_DISPLAY_HEIGHT; ++y) {
    if (rowsToCopy[y]) {
      if (minRow == -1) minRow = y;
      maxRow = y;
    }
  }


  if (minRow == -1) return;

  drawBufferLong = (unsigned long *)drawBuffer;

  for (y = minRow; y <= maxRow; ++y) {
    if (!rowsToCopy[y]) continue;

    yOffset = y * VGA_UNCHAINED_LINE_WIDTH;

    for (x = 0; x < VGA_UNCHAINED_LINE_WIDTH; ++x) {
      data = drawBufferLong[yOffset + x];

      for (plane = 0; plane < 4; plane++) {
        copyRow[x + (plane * VGA_UNCHAINED_LINE_WIDTH)] = data & 0xff;
        data >>= 8;
      }
    }

    for (plane = 0; plane < 4; ++plane) {
      setActiveVGAMemoryPlanes(1 << plane);

      memcpy(vgaStart + yOffset, copyRow + (plane * VGA_UNCHAINED_LINE_WIDTH), VGA_UNCHAINED_LINE_WIDTH);
    }
  }

  endTime = clock();

}

void enableUnchainedVGAMode() {
  word clearOffset;

  // convert VGA pointer into a word sized 
  ulong *ptr = (ulong *)VGA;

  // The VGA registers are (mostly) byte sized, and are grouped
  // logically behind a pair of index/data ports.
  // To access them, you prep the index port on the VGA card
  // for the register you want to access, then do that access
  // via the data port.
  //
  // Visual: VGA has pouches which contain organization boxes of
  // registers.

  // target the Sequence Memory Mode Register for Memory Mode
  // enable sequential access and extended memory
  outpw_to_register(
    VGA_SEQUENCE_CONTROLLER_INDEX,
    VGA_SEQUENCE_CONTROLLER_MEMORY_MODE,
    VGA_SEQUENCE_CONTROLLER_EXTENDED_MEMORY | VGA_SEQUENCE_CONTROLLER_ODD_EVEN_SEQUENTIAL_ACCESS
  );

  // target the Sequence Memory Mode Register for map mask
  outpw_to_register(
    VGA_SEQUENCE_CONTROLLER_INDEX,
    VGA_SEQUENCE_CONTROLLER_MAP_MASK_MODE,
    VGA_SEQUENCE_CONTROLLER_ALL_PLANES
  );

  for (clearOffset = 0; clearOffset < PAGE_SIZE; ++clearOffset) {
    *ptr++ = 0;
  }

  // there's a way to address memory as words or longs, but we're
  // going to set it to byte addressing because it makes mroe sense.
  outpw_to_register(
    VGA_CRT_CONTROLLER_INDEX,
    VGA_CRT_CONTROLLER_UNDERLINE_MODE,
    0x00
  );

  // * enable hsync and vsync
  // * activate byte Mode
  // * ...something with address wrapping that I don't quite understand yet...
  // * ...even more with address funkiness that I don't understand
  outpw_to_register(
    VGA_CRT_CONTROLLER_INDEX,
    VGA_CRT_CONTROLLER_MODE_CONTROL,
    0xe3
  );

  outp(0x3c2, 0xe3);
}

void enable320x256VGAMode() {
  outpw_to_register(
    VGA_CRT_CONTROLLER_INDEX,
    VGA_CRT_CONTROLLER_VERTICAL_RETRACE_END,
    0x2c
  );
  outpw_to_register(
    VGA_CRT_CONTROLLER_INDEX,
    VGA_CRT_CONTROLLER_VERTICAL_TOTAL,
    0x0d
  );
  outpw_to_register(
    VGA_CRT_CONTROLLER_INDEX,
    VGA_CRT_CONTROLLER_OVERFLOW,
    0x3e
  );
  outpw_to_register(
    VGA_CRT_CONTROLLER_INDEX,
    VGA_CRT_CONTROLLER_VERTICAL_RETRACE_START,
    0xea
  );
  outpw_to_register(
    VGA_CRT_CONTROLLER_INDEX,
    VGA_CRT_CONTROLLER_VERTICAL_RETRACE_END,
    0xac
  );
  outpw_to_register(
    VGA_CRT_CONTROLLER_INDEX,
    VGA_CRT_CONTROLLER_VERTICAL_DISPLAY_END,
    0xdf
  );
  outpw_to_register(
    VGA_CRT_CONTROLLER_INDEX,
    VGA_CRT_CONTROLLER_VERTICAL_BLANK_START,
    0xe7
  );
  outpw_to_register(
    VGA_CRT_CONTROLLER_INDEX,
    VGA_CRT_CONTROLLER_VERTICAL_BLANK_END,
    0x06
  );
}

void clearWithColor(byte color) {
  int x, y;
  int pageOffset, drawY;
  long *start;
  long newColor = (color << 24) + (color << 16) + (color << 8) + (color);

  // write to all planes
  outpw_to_register(
    VGA_SEQUENCE_CONTROLLER_INDEX,
    VGA_SEQUENCE_CONTROLLER_MAP_MASK_MODE,
    0xff
  );

  pageOffset = activePage * PAGE_SIZE;
  start = (long *)(VGA + pageOffset);

  for (y = 0; y < VGA_DISPLAY_HEIGHT; ++y) {
    for (x = 0; x < VGA_UNCHAINED_LINE_WIDTH / 4; ++x) {
      *(start++) = newColor;
    }
  }
}

/**
 * this is very temporary
 */
void copyUnchainedMemoryToActive(byte *src) {
  int y, plane, _testSize, _testOffset;

  _testSize = 80 * 240;
  _testOffset = 80 * 256;

  for (plane = 0; plane < 4; ++plane) {
    // copying to/from VGA memory requires setting the plane to write to...
    outpw_to_register(
      VGA_SEQUENCE_CONTROLLER_INDEX,
      VGA_SEQUENCE_CONTROLLER_MAP_MASK_MODE,
      1 << plane
    );

    memcpy(VGA + activePage * PAGE_SIZE, src + plane * _testOffset, _testSize);
  }
}

void copyScratchPlanesToActive() {
  int y, plane;

  for (plane = 0; plane < 4; ++plane) {
    // copying to/from VGA memory requires setting the plane to write to...
    outpw_to_register(
      VGA_SEQUENCE_CONTROLLER_INDEX,
      VGA_SEQUENCE_CONTROLLER_MAP_MASK_MODE,
      1 << plane
    );

    // and to read from
    outpw_to_register(
      0x3ce,
      0x04,
      plane
    );

    memcpy(VGA + activePage * PAGE_SIZE, VGA + SCRATCH_PAGE, PAGE_SIZE);
  }
}

void copyScratchPlanesToActiveViaLatches() {
  int pixel;
  byte *src, *dest;
  
  // TODO: VGA latches
  outpw_to_register(
    VGA_SEQUENCE_CONTROLLER_INDEX,
    VGA_SEQUENCE_CONTROLLER_MAP_MASK_MODE,
    0xff
  );

  outpw_to_register(
    VGA_GRAPHICS_REGISTERS_INDEX,
    VGA_GRAPHICS_DATA_ROTATE,
    0x10
  );

  src = VGA + SCRATCH_PAGE;
  dest = VGA + activePage * PAGE_SIZE;

  for (pixel = 0; pixel < PAGE_SIZE; ++pixel) {
    volatile char _temp = *(src++);
    *(dest++) = 0;
  }

  outpw_to_register(
    VGA_GRAPHICS_REGISTERS_INDEX,
    VGA_GRAPHICS_DATA_ROTATE,
    0x00
  );
}

void drawSprite(struct SpriteRender* sprite) {
  int x, y;
  byte pixel;

  byte* spriteData = sprite->data;
  byte* drawBufferPos = drawBuffer + sprite->x + (sprite->y * VGA_DISPLAY_WIDTH);

  for (y = 0; y < sprite->height; ++y) {
    for (x = 0; x < sprite->width; ++x) {
      pixel = *(spriteData++);
      if (pixel != sprite->transparentColor) {
        *(drawBufferPos) = pixel;
      }

      drawBufferPos++;
    }

    drawBufferPos += (VGA_DISPLAY_WIDTH - sprite->width);
    spriteData += sprite->modulo;
  }
}

void drawUnchainedSprite(struct SpriteRender* sprite) {
  int dataX, dataY, currentPlane;
  int drawX, drawY, vgaWriteYStart;
  int startY, endY;
  int nextStartX = 1;
  int planeSampleX = 0, readPositionOffset;
  int vgaWritePosition, readPosition, spriteAdvance;
  int activePageOffset, alwaysVGAWriteStart, startVGAWritePosition;
  byte readResult;

  // precalculate sprite sheet handling and VGA plane
  currentPlane = sprite->x & 3;
  spriteAdvance = sprite->width + sprite->modulo;

  outpw_to_register(
    VGA_SEQUENCE_CONTROLLER_INDEX,
    VGA_SEQUENCE_CONTROLLER_MAP_MASK_MODE,
    1 << currentPlane
  );

  // precalculate x pixel drawing
  activePageOffset = activePage * PAGE_SIZE;

  alwaysVGAWriteStart = sprite->y;
  if (alwaysVGAWriteStart < 0) alwaysVGAWriteStart = 0;
  alwaysVGAWriteStart *= VGA_DISPLAY_WIDTH;

  // precalculate y pixel drawing
  startY = sprite->y;
  endY = sprite->y + sprite->height;
  readPositionOffset = 0;

  if (startY < 0) {
    readPositionOffset = (spriteAdvance * (-startY));
    startY = 0;
  }
  if (endY > VGA_DISPLAY_HEIGHT) { endY = VGA_DISPLAY_HEIGHT; }

  startVGAWritePosition = activePageOffset + (sprite->x + alwaysVGAWriteStart) / PLANE_PIXEL_DISTANCE;

  for (dataX = 0; dataX < sprite->width; ++dataX) {
    if (planeSampleX >= sprite->width) {
      planeSampleX = nextStartX;
      nextStartX++;
      currentPlane = (currentPlane + 1) & 3;

      startVGAWritePosition = activePageOffset + (planeSampleX + sprite->x + alwaysVGAWriteStart) / PLANE_PIXEL_DISTANCE;

      // save a second out
      outp(
        VGA_SEQUENCE_CONTROLLER_DATA,
        1 << currentPlane
      );
    }

    drawX = planeSampleX + sprite->x;
    if (drawX >= 0 && drawX < VGA_DISPLAY_WIDTH) {
      vgaWritePosition = startVGAWritePosition;
      readPosition = planeSampleX + readPositionOffset;

      for (drawY = startY; drawY < endY; ++drawY) {
        readResult = sprite->data[readPosition];
        if (readResult != sprite->transparentColor) {
          VGA[vgaWritePosition] = readResult;
        }
        vgaWritePosition += VGA_UNCHAINED_LINE_WIDTH;
        readPosition += spriteAdvance;
      }
    }

    startVGAWritePosition++;
    planeSampleX += 4;
  }
}
void swapPlanes() {
  outpw(VGA_CRT_CONTROLLER_INDEX, VGA_CRT_CONTROLLER_DISPLAY_HIGH_ADDRESS | ((PAGE_SIZE * activePage) & 0xff00));
  outpw(VGA_CRT_CONTROLLER_INDEX, VGA_CRT_CONTROLLER_DISPLAY_LOW_ADDRESS | ((PAGE_SIZE * activePage) << 8));

  activePage = 1 - activePage;
}

void setVGAColors(struct VGAColor colors[], int totalColors) {
  int i;

  outp(0x3c8,0);
  for (i = 0; i < totalColors; ++i) {
    outp(0x3c9, colors[i].red);
    outp(0x3c9, colors[i].green);
    outp(0x3c9, colors[i].blue);
  }
}