#include <genesis.h>
#include "sinTable.h"
#include "cosTable.h"
#define TABLE_LENGTH 720
#define SPEED 5
#define FRAC_BITS 8

Line l;
s16 angle = 0;
s16 x, y;


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;
  s16 rx,ry,ra,xo,yo;
  s16 dy = sinTable[angle << 1];
  s16 dx = cosTable[angle << 1];
  s16 tan = 0;
  if(dx != 0) tan = (dy << FRAC_BITS)/dx;
  s16 aTan = 0;
  if(tan != 0) aTan = -(1 << (FRAC_BITS*2))/tan;
  dof = 0;
  s16 distH = 500;
  s16 hx = l.pt1.x << FRAC_BITS;
  s16 hy = l.pt1.y << FRAC_BITS;
  if(dy == 0){
    rx = l.pt1.x << FRAC_BITS;
    ry = l.pt1.y << FRAC_BITS;
    dof = 8;
  }else {
    if(angle > 180) {
      ry = ((l.pt1.y / 10 * 10)-1) << FRAC_BITS;
      rx = (((l.pt1.y - (ry >> FRAC_BITS))*aTan)) + (l.pt1.x << FRAC_BITS);
      yo = -(10 << FRAC_BITS);
      xo = ((-yo >> FRAC_BITS)*aTan);
    }
    if(angle < 180) {
      ry = ((l.pt1.y / 10 * 10)+10) << FRAC_BITS;
      rx = (((l.pt1.y - (ry >> FRAC_BITS))*aTan)) + (l.pt1.x << FRAC_BITS);
      yo = 10 << FRAC_BITS;
      xo = (-(yo >> FRAC_BITS)*aTan);
    }
  }
  while(dof<8){
    mx = (rx >> FRAC_BITS) / 10;
    my = (ry >> FRAC_BITS) / 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, rx >> FRAC_BITS, ry >> FRAC_BITS);
    }else{
      rx = rx + xo;
      ry = ry + yo;
      dof += 1;
    }
  }

  dof = 0;
  s16 distV = 500;
  s16 vx = l.pt1.x << FRAC_BITS;
  s16 vy = l.pt1.y << FRAC_BITS;
  s16 nTan = -tan;
  //s16 threshold = (51 << FRAC_BITS);
  //if(nTan > threshold){
   // nTan = threshold;
  //}
  //if(nTan < -threshold){
  //  nTan = -threshold;
  //}
  if(angle > 90 && r < 270) {
    rx = ((l.pt1.x / 10 * 10)-1) << FRAC_BITS;
    ry = ((l.pt1.x) - (rx >> FRAC_BITS)*nTan) + (l.pt1.y << FRAC_BITS);
    xo = -(10 << FRAC_BITS);
    yo = ((-xo >> FRAC_BITS)*nTan);
  }
  if(angle < 90 || angle > 270) {
    rx = ((l.pt1.x / 10 * 10)+10) << FRAC_BITS;
    ry = ((l.pt1.x) - (rx >> FRAC_BITS)*nTan) + (l.pt1.y << FRAC_BITS);
    xo = 10 << FRAC_BITS;
    yo = -(xo >> FRAC_BITS)*nTan;
  }
  if(dx == 0){
    rx = l.pt1.x << FRAC_BITS;
    ry = l.pt1.y << FRAC_BITS;
    xo = 0;
    yo = 0;
    dof = 8;
  }
  while(dof<8){
    mx = (rx >> FRAC_BITS) / 10;
    my = (ry >> FRAC_BITS) / 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, rx >> FRAC_BITS, ry >> FRAC_BITS);
    }else{
      rx = rx + xo;
      ry = ry + yo;
      dof += 1;
    }
  }
  //if(distV < distH){
    l.pt2.x = vx >> FRAC_BITS;
    l.pt2.y = vy >> FRAC_BITS;
  //}else {
    //l.pt2.x = hx >> FRAC_BITS;
    //l.pt2.y = hy >> FRAC_BITS;
  //}

  
}

void mapscan(){

	u8 mapcolor = 12;
	mapcolor |= mapcolor << 4;
	int mapscale = 10;

	for(u8 x = 0; x < 10; x++){
		for(u8 y = 0; y < 10; y++){
			if(map[y][x] == 1){
				Vect2D_s16 mapverts[4] = {{x*mapscale,y*mapscale},{(x+1)*mapscale,y*mapscale},{(x+1)*mapscale,(y+1)*mapscale},{x*mapscale,(y+1)*mapscale}};
				BMP_drawPolygon(mapverts, 4, mapcolor);

			}		
		}

	}
}


void render(){
  l.pt1.x = x / 10;
  l.pt1.y = y / 10;
	//Clear the bitmap
	BMP_clear();

	//Draw the line defined above (in the background, hidden)

	mapscan();
  if(angle >= 360) angle = angle - 360;
  if(angle < 0) angle = 360 + angle;
  castRay(angle-20);
    BMP_drawLine(&l);
  castRay(angle-10);
    BMP_drawLine(&l);
  for (int i = 0; i < 16; i++){
  castRay(angle);
    //BMP_drawLine(&l);
  }
  castRay(angle+10);
    BMP_drawLine(&l);
  castRay(angle+20);
  BMP_drawLine(&l);
  BMP_showFPS(0);

	//Flip the data to the screen - i.e. actually draw the complete image on screen
	BMP_flip(1);

	//Increment the destination y coordinate
	//l.pt2.y = l.pt2.y + 2;

	//Reset the destination y coordinate if it hits 160
	//if (l.pt2.y == 160)
	//{
	//l.pt2.y = 0;
	//}

  SYS_doVBlankProcess();

}

void update(){
  u16 joy = JOY_readJoypad(JOY_1);
  if(joy & BUTTON_LEFT) {
    //l.pt1.x -= SPEED;
    angle -= 1;
  }
  if(joy & BUTTON_RIGHT) {
    //l.pt1.x += SPEED;
    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()
{
  x = 150;
  y = 150;
  initVDP();

  //Initialise the bitmap engine
  BMP_init(FALSE, BG_A, PAL0, FALSE);

  //Set the colour of the line. We are using pallete 0 for the bitmap, so we have 0->15. 15 is used for white text, so we set an unused pallet colour, 14 to blue - RGB 0000FF.
  u16 colour_blue = RGB24_TO_VDPCOLOR(0x0000ff);
  u16 colour_red = RGB24_TO_VDPCOLOR(0x756a4a);
  PAL_setColor(14, colour_blue);
  PAL_setColor(13, colour_red);
  PAL_setColor(12, RGB24_TO_VDPCOLOR(0x00ff00));
  PAL_setColor(15, RGB24_TO_VDPCOLOR(0xff0000));
  VDP_setBackgroundColor(13);

  //A line needs a source coordinate x,y and a destination coordinate x,y along with a pallete colour.
  l.pt1.x = 15;
  l.pt1.y = 15;
  l.pt2.x = 255;
  l.pt2.y = 0;
  l.col = 14;
  l.col |= l.col << 4; // if we do not left shift the colour, we get gaps in the line

  while(TRUE)
  {
   	render();
    update();
  }
  return 0;
}