#include <genesis.h>

#define SPEED 2

Line l;
s16 angle = 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){

  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(1);
  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(angle == 0 || angle == 180){
    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(mx >=0 && my >= 0 && 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);
  float nTan = 0-tan;
  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(angle == 90 || angle == 270){
    rx = FIX16(l.pt1.x);
    ry = FIX16(l.pt1.y);
    dof = 8;
  }
  while(dof<8){
    mx = fix16ToInt(rx) / 10;
    my = fix16ToInt(ry) / 10;
    if(mx >=0 && my >= 0 && 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);
  }
  char* str[5];
  sprintf(str, "%d", distV);
  BMP_drawText(str, 20, 0);
  char* str2[5];
  sprintf(str2, "%d", distH);
  BMP_drawText(str2, 20, 1);
}

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(){
	//Clear the bitmap
	BMP_clear();

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

	mapscan();
  if(angle > 360) angle = 0;
  if(angle < 0) angle = 359;
  castRay(angle);
  BMP_drawLine(&l);

	//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--;
  }
  if(joy & BUTTON_RIGHT) {
    //l.pt1.x += SPEED;
    angle++;
  }
  if(joy & BUTTON_UP) {
    l.pt1.y -= SPEED;
  }
  if(joy & BUTTON_DOWN) {
    l.pt1.y += SPEED;
  }
}

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

int main()
{
  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;
}