#include <genesis.h>
#include "sinTable.h"
#include "cosTable.h"
#define TABLE_LENGTH 720
#define SPEED 5
#define FRAC_BITS 8
#define TILE_WIDTH 8
#define TILE_HEIGHT 8
#define TILE_SIZE (TILE_WIDTH * TILE_HEIGHT / 2)
#define SCREEN_WIDTH 320
#define SCREEN_HEIGHT 224
#define SCREEN_TILES_X 40
#define SCREEN_TILES_Y 28
#define TILE_EMPTY 0
#define TILE_FILLED 1
#define MAP_WIDTH 10
#define MAP_HEIGHT 10

Line l;
s16 angle = 0;
s16 x, y;
u8 tileBuffer[TILE_SIZE];
u16 tileIndex = 0;


const u8 map[10][10] = {{1,1,1,1,1,1,1,1,1,1},
                	{1,0,0,0,0,0,0,0,0,1},
                	{1,1,1,1,0,0,1,1,1,1},
                	{1,0,0,1,0,0,1,0,0,1},
                	{1,0,0,0,0,0,1,0,0,1},
                	{1,0,0,1,0,0,1,1,0,1},
                	{1,1,1,1,0,0,1,0,0,1},
                	{1,0,0,0,0,0,0,0,0,1},
                	{1,0,0,0,0,0,1,0,0,1},
                	{1,1,1,1,1,1,1,1,1,1}};

int dist(int ax, int ay, int bx, int by){
  return (ax-bx)*(ax-bx) + (ay-by)*(ay-by);
}

void castRay(s16 angle){
  if(angle >= 360) angle = angle - 360;
  if(angle < 0) angle = 360 + angle;
  u16 r,mx,my,mp,dof;
  fix16 rx,ry,ra,xo,yo;
  s16 ind = (int)((float)angle/360.0f*1024.0f);
  fix16 dy = sinFix16(ind);
  fix16 dx = cosFix16(ind);
  fix16 tan = FIX16(0);
  if(dx != 0) tan = fix16Div(dy, dx);
  fix16 aTan = FIX16(0);
  if(tan != 0) aTan = fix16Div(FIX16(-1), tan);
  s16 tanInt = fix16ToInt(fix16Mul(aTan, FIX16(100)));
  dof = 0;
  fix16 distH = FIX16(500);
  fix16 hx = FIX16(l.pt1.x);
  fix16 hy = FIX16(l.pt1.y);
  if(dy == 0){
    rx = FIX16(l.pt1.x);
    ry = FIX16(l.pt1.y);
    dof = 8;
  }else {
    if(angle > 180) {
      ry = FIX16(l.pt1.y / 10 * 10)-FIX16(1);
      rx = fix16Mul(FIX16(l.pt1.y) - ry,aTan) + FIX16(l.pt1.x);
      yo = FIX16(-10);
      xo = fix16Mul(0-yo,aTan);
    }
    if(angle < 180) {
      ry = FIX16((l.pt1.y / 10 * 10)+10);
      rx = fix16Mul(FIX16(l.pt1.y) - ry,aTan) + FIX16(l.pt1.x);
      yo = FIX16(10);
      xo = fix16Mul(0-yo,aTan);
    }
  }
  while(dof<8){
    mx = fix16ToInt(rx) / 10;
    my = fix16ToInt(ry) / 10;
    if(rx < 0 || ry < 0){
      dof = 8;
      continue;
    }
    if(mx < 10 && my < 10 && map[my][mx] == 1){
      dof = 8;
      hx = rx;
      hy = ry;
      distH = dist(l.pt1.x, l.pt1.y, fix16ToInt(rx), fix16ToInt(ry));
    }else{
      rx = rx + xo;
      ry = ry + yo;
      dof += 1;
    }
  }

  dof = 0;
  fix16 distV = FIX16(500);
  fix16 vx = FIX16(l.pt1.x);
  fix16 vy = FIX16(l.pt1.y);
  fix16 nTan = 0-tan;
  if(fix16ToInt(nTan) > 51){
    nTan = FIX16(51);
  }
  if(fix16ToInt(nTan) < -51){
    nTan = FIX16(-51);
  }
  if(angle > 90 && r < 270) {
    rx = FIX16(l.pt1.x / 10 * 10)-FIX16(1);
    ry = fix16Mul(FIX16(l.pt1.x) - rx,nTan) + FIX16(l.pt1.y);
    xo = FIX16(-10);
    yo = fix16Mul(0-xo,nTan);
  }
  if(angle < 90 || angle > 270) {
    rx = FIX16((l.pt1.x / 10 * 10)+10);
    ry = fix16Mul(FIX16(l.pt1.x) - rx,nTan) + FIX16(l.pt1.y);
    xo = FIX16(10);
    yo = fix16Mul(0-xo,nTan);
  }
  if(dx == 0){
    rx = FIX16(l.pt1.x);
    ry = FIX16(l.pt1.y);
    xo = FIX16(0);
    yo = FIX16(0);
    dof = 8;
  }
  while(dof<8){
    mx = fix16ToInt(rx) / 10;
    my = fix16ToInt(ry) / 10;
    if(rx < 0 || ry < 0){
      dof = 8;
      continue;
    }
    if(mx < 10 && my < 10 && map[my][mx] == 1){
      dof = 8;
      vx = rx;
      vy = ry;
      distV = dist(l.pt1.x, l.pt1.y, fix16ToInt(rx), fix16ToInt(ry));
    }else{
      rx = rx + xo;
      ry = ry + yo;
      dof += 1;
    }
  }
  if(distV < distH){
    l.pt2.x = fix16ToInt(vx);
    l.pt2.y = fix16ToInt(vy);
  }else {
    l.pt2.x = fix16ToInt(hx);
    l.pt2.y = fix16ToInt(hy);
  }

  
}


void initTileBuffer(u8 color) {
    u8 colorValue = (color << 4) | color;  // Color for both nibbles
    for (int i = 0; i < TILE_SIZE; i++) {
        tileBuffer[i] = colorValue;
    }
}

void uploadTileDataToVRAM(u16 vramTileIndex) {
    VDP_loadTileData(tileBuffer, vramTileIndex, 1, DMA);  // Load 1 tile to VRAM
}

void mapscan() {
    u8 mapcolor = 15;  // map color
    mapcolor |= mapcolor << 4;

    for (u8 x = 0; x < 10; x++) {
        for (u8 y = 0; y < 10; y++) {
            if (map[y][x] == 1) {
                // Draw a filled tile for the map block
                for (u8 px = 0; px < TILE_WIDTH; px++) {
                    for (u8 py = 0; py < TILE_HEIGHT; py++) {
                        setPixelInBuffer(px, py, mapcolor);
                    }
                }
                // Upload the filled tile to VRAM
                uploadTileDataToVRAM(tileIndex);
                VDP_setTileMapXY(BG_A, tileIndex, x, y);
            }
        }
    }
}

void clearScreenWithTile(u16 tileIndex) {
    // Fill the entire screen with the background tile
    VDP_fillTileMapRect(BG_A, TILE_ATTR_FULL(PAL1, 0, FALSE, FALSE, tileIndex), 0, 0, SCREEN_TILES_X, SCREEN_TILES_Y);
}

void drawMap() {
    for (u16 y = 0; y < MAP_HEIGHT; y++) {
        for (u16 x = 0; x < MAP_WIDTH; x++) {
            u8 tileType = map[y][x] ? TILE_FILLED : TILE_EMPTY;
            VDP_setTileMapXY(BG_A, TILE_ATTR_FULL(PAL1, 0, FALSE, FALSE, tileType), x, y);
        }
    }
}

void render() {
    clearScreenWithTile(TILE_EMPTY);
    drawMap();
    VDP_setTextPlane(BG_B);
    VDP_setTextPalette(2);
    VDP_showFPS(1);
    s16 castStart = angle - 20;
    for(int i = 0; i < 40; i++){
      castRay(castStart + i);
    }
    SYS_doVBlankProcess();
}

void update() {
    u16 joy = JOY_readJoypad(JOY_1);
    if (joy & BUTTON_LEFT) {
        angle -= 1;
    }
    if (joy & BUTTON_RIGHT) {
        angle += 1;
    }
    if (joy & BUTTON_UP) {
        s16 ind = (int)((float)angle / 360.0f * 1024.0f);
        fix16 dy = sinFix16(ind);
        fix16 dx = cosFix16(ind);
        y += fix16ToInt(fix16Mul(dy, FIX16(SPEED)));
        x += fix16ToInt(fix16Mul(dx, FIX16(SPEED)));
    }
    if (joy & BUTTON_DOWN) {
        s16 ind = (int)((float)angle / 360.0f * 1024.0f);
        fix16 dy = sinFix16(ind);
        fix16 dx = cosFix16(ind);
        y -= fix16ToInt(fix16Mul(dy, FIX16(SPEED)));
        x -= fix16ToInt(fix16Mul(dx, FIX16(SPEED)));
    }
}

void initVDP() {
    SYS_disableInts();
    VDP_setPlaneSize(64, 32, TRUE);
    SYS_enableInts();
}

int main() {
    SYS_disableInts();
    VDP_setPlaneSize(64, 32, TRUE);
    SYS_enableInts();
    PAL_setColor(14, RGB24_TO_VDPCOLOR(0x1c1c14));
    PAL_setColor(15, RGB24_TO_VDPCOLOR(0x17ffe8));
    VDP_setBackgroundColor(14);

    // Initialize and upload tile data for TILE_EMPTY and TILE_FILLED
    initTileBuffer(15); // Color for TILE_FILLED
    VDP_loadTileData(tileBuffer, TILE_FILLED, 1, DMA);
    
    initTileBuffer(0); // Color for TILE_EMPTY
    VDP_loadTileData(tileBuffer, TILE_EMPTY, 1, DMA);

    while (TRUE) {
        render();
    }

    return 0;
}