

#include <stdio.h>
#include <io.h>
#include <system.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "poolcue.h"
#include "poolball.h"
#include "keyboard.h"

#define VC_MAX 32
#define ACC 100
#define pi 3.1415926
#define angle_trans 0.05236   //!WXC
#define zero 0.01
#define PRODUCT 0.32    // PRODUCT = dx * vc , when dx = min(zero), vc = vc_max
#define edge_acc 0.95


//Write to vga ram
#define IOWR_VGA_STRENGTHBAR(data) \
	IOWR_16DIRECT(DE2_VGA_RASTER_0_BASE, 128, data)
#define SERVELINE_EN(flag)\
	IOWR_16DIRECT(DE2_VGA_RASTER_0_BASE, 130, flag)

struct balltype
{
	int pos_x,pos_y;
	int vc_x, vc_y;
	int count_x, count_y;
	short int dir_x, dir_y;
	short int flag;
	float dx, dy;
	int force_move;
	int bias_x, bias_y;
}ball[16];


int hole[6][2]={{24,158},{320,154},{615,158},
				{24,461},{320,465},{615,461}};

void decide_direction(int ball_num)
	{
		if(ball[ball_num].dx >= zero)
			{
				ball[ball_num].vc_x = (int)(PRODUCT / ball[ball_num].dx);
				ball[ball_num].dir_x = 1;
			}
			else if (ball[ball_num].dx <= -1 * zero)
			{
				ball[ball_num].vc_x = - (int)(PRODUCT / ball[ball_num].dx);
				ball[ball_num].dir_x = -1;
			}
			else
			{
				ball[ball_num].vc_x = VC_MAX + 1;
				ball[ball_num].dir_x = 0;
				ball[ball_num].dx = 0;
			}

			if(ball[ball_num].dy >= zero)
			{
				ball[ball_num].vc_y = (int)(PRODUCT / ball[ball_num].dy);
				ball[ball_num].dir_y = 1;
			}
			else if (ball[ball_num].dy <= -1 * zero)
			{
				ball[ball_num].vc_y = - (int)(PRODUCT / ball[ball_num].dy);
				ball[ball_num].dir_y = -1;
			}
			else
			{
				ball[ball_num].vc_y = VC_MAX + 1;
				ball[ball_num].dir_y = 0;
				ball[ball_num].dy = 0;
			}
	}

void dir_change(int i, char axle, int change_to)
{
	if(axle == 'x')
	{
		//ball[i].dir_x = change_to;
		if((change_to == 1 && ball[i].dx < 0) || (change_to == -1 && ball[i].dx > 0))
		{
			ball[i].dx = -ball[i].dx;
		}

	}
	else if(axle == 'y')
	{
		//ball[i].dir_y = change_to;
		if((change_to == 1 && ball[i].dy < 0) || (change_to == -1 && ball[i].dy > 0))
		{
			ball[i].dy = -ball[i].dy;
		}
	}
}

void detect_bound_edge(int i)
{
	  if(ball[i].pos_x < 45 && ball[i].pos_y < 179)  //left_up pocket area
	  {
		  if(39 - ball[i].pos_x > 179 - ball[i].pos_y)
		  {
			  dir_change(i, 'x', 1);
			  dir_change(i, 'y', -1);

			  ball[i].dx *= edge_acc;
			  ball[i].dy *= edge_acc;
			  decide_direction (i);

		  }
		  else if(173 - ball[i].pos_y > 45 - ball[i].pos_x)
		  {
			  dir_change(i, 'x', -1);
			  dir_change(i, 'y', 1);

			  ball[i].dx *= edge_acc;
			  ball[i].dy *= edge_acc;
			  decide_direction (i);

		  }

	  }
	  else if(ball[i].pos_x > 594 && ball[i].pos_y < 179)  //right_up pocket area
	  {
		  if(ball[i].pos_x - 600 > 179 - ball[i].pos_y)
		  {
			  dir_change(i, 'x', -1);
			  dir_change(i, 'y', -1);

			  ball[i].dx *= edge_acc;
			  ball[i].dy *= edge_acc;
			  decide_direction (i);

		  }
		  else if(173 - ball[i].pos_y > ball[i].pos_x - 594)
		  {
			  dir_change(i, 'x', 1);
			  dir_change(i, 'y', 1);

			  ball[i].dx *= edge_acc;
			  ball[i].dy *= edge_acc;
			  decide_direction (i);

		  }

	  }
	  else if(ball[i].pos_x < 45 && ball[i].pos_y > 440)  //left_down pocket area
	  {
		  if(39 - ball[i].pos_x > ball[i].pos_y - 440)
		  {
			  dir_change(i, 'x', 1);
			  dir_change(i, 'y', 1);

			  ball[i].dx *= edge_acc;
			  ball[i].dy *= edge_acc;
			  decide_direction (i);

		  }
		  else if(ball[i].pos_y - 446 > 45 - ball[i].pos_x)
		  {
			  dir_change(i, 'x', -1);
			  dir_change(i, 'y', -1);

			  ball[i].dx *= edge_acc;
			  ball[i].dy *= edge_acc;
			  decide_direction (i);

		  }

	  }
	  else if(ball[i].pos_x > 594 && ball[i].pos_y > 440)  //right_down pocket area
	  {
		  if(ball[i].pos_x - 600 > ball[i].pos_y - 440)
		  {
			  dir_change(i, 'x', -1);
			  dir_change(i, 'y', 1);

			  ball[i].dx *= edge_acc;
			  ball[i].dy *= edge_acc;
			  decide_direction (i);

		  }
		  else if(ball[i].pos_y - 446 > ball[i].pos_x - 594)
		  {
			  dir_change(i, 'x', 1);
			  dir_change(i, 'y', -1);

			  ball[i].dx *= edge_acc;
			  ball[i].dy *= edge_acc;
			  decide_direction (i);

		  }

	  }
	  else
	  {

		  if(ball[i].pos_x <= 39)
		  {
			  ball[i].pos_x = 39;
			  dir_change(i, 'x', 1);

			  ball[i].dx *= edge_acc;
			  ball[i].dy *= edge_acc;
			  decide_direction (i);

		  }
		  if(ball[i].pos_x >= 600)
		  {
			  ball[i].pos_x = 600;
			  dir_change(i, 'x', -1);

			  ball[i].dx *= edge_acc;
			  ball[i].dy *= edge_acc;
			  decide_direction (i);

		  }
		  if(ball[i].pos_y <= 173)
		  {
			  if(ball[i].pos_x > 328 || ball[i].pos_x < 312)  //up_mid pocket area
			  {
				  ball[i].pos_y = 173;
				  dir_change(i, 'y', 1);

				  ball[i].dx *= edge_acc;
				  ball[i].dy *= edge_acc;
				  decide_direction (i);

			  }

		  }
		  if(ball[i].pos_y >= 446)
		  {


			  if(ball[i].pos_x > 328 || ball[i].pos_x < 312)   //down_mid pocket area
			  {
				  ball[i].pos_y = 446;
				  dir_change(i, 'y', -1);

				  ball[i].dx *= edge_acc;
				  ball[i].dy *= edge_acc;
				  decide_direction (i);

			  }

		  }

	  }
}

void bound_balls(int b1,int b2)
{
	float s1=100,s2=100,s,d1,d2,x,y,dx1,dx2,dy1,dy2;
	int dis1_x, dis2_x;
	int dis1_y, dis2_y;

	if(b2==18)
	{
		ball[b1].dx=-ball[b1].dx;
	}
	else if(b2==17)
	{
		ball[b1].dy=-ball[b1].dy;
	}
	else
	{

		x = ball[b2].pos_x - ball[b1].pos_x;
		y = ball[b2].pos_y - ball[b1].pos_y;

		//if(!(abs(ball[b1].vc_x) >= VC_MAX && abs(ball[b1].vc_y) >= VC_MAX))
		if(!(fabs(ball[b1].dx) < zero && fabs(ball[b1].dy) < zero))
		{
			s1 = atan2(ball[b1].dy,ball[b1].dx);

		}
		//if(!(abs(ball[b2].vc_x) >= VC_MAX && abs(ball[b2].vc_y) >= VC_MAX))
		if(!(fabs(ball[b2].dx) < zero && fabs(ball[b2].dy) < zero))
		{
			s2 = atan2(ball[b2].dy,ball[b2].dx);

		}
		s = atan2(y,x);


		if(s1!=100 && fabs(s-s1) < pi/2)
		{
			d1=sqrt(ball[b1].dx * ball[b1].dx + ball[b1].dy * ball[b1].dy)*cos(s-s1);
			dx1= d1*cos(s);
			dy1= d1*sin(s);
			ball[b1].dx -= dx1;
			ball[b1].dy -= dy1;
			ball[b2].dx += dx1;
			ball[b2].dy += dy1;
		}
		if(s2 != 100 && fabs(s-s2) > pi/2)
		{
			d2=sqrt(ball[b2].dx * ball[b2].dx + ball[b2].dy * ball[b2].dy)*cos(pi-(s-s2));
			dx2= d2*cos(pi-s);
			dy2= d2*sin(pi-s);
			ball[b1].dx += dx2;
			ball[b1].dy += dy2;
			ball[b2].dx -= dx2;
			ball[b2].dy -= dy2;
		}
	}

	ball[b1].dx *= 0.90;
	ball[b1].dy *= 0.90;
	ball[b2].dx *= 0.90;
	ball[b2].dy *= 0.90;


	decide_direction (b1);
	decide_direction (b2);

	if(x*x + y*y < 196)
	{
		if(x > 0)
		{
			dis2_x = 1;
			dis1_x = -1;
		}
		else
		{
			dis2_x = -1;
			dis1_x = 1;
		}

		if(y > 0)
		{
			dis2_y = 1;
			dis1_y = -1;
		}
		else
		{
			dis2_y = -1;
			dis1_y = 1;
		}

		moveball(b1, &ball[b1].pos_x, &ball[b1].pos_y, dis1_x, dis1_y, &ball[b1].bias_x, &ball[b1].bias_y);
		moveball(b2, &ball[b2].pos_x, &ball[b2].pos_y, dis2_x, dis2_y, &ball[b2].bias_x, &ball[b2].bias_y);

		ball[b2].force_move = 1;
	}
}

int main()
{
  printf("Hello from Nios II!\n");

  int i;
  int j;
  int ff;
  float temp_angle;   //!WXC

  int acc_count = 0;

  short int move_flag;
  short int bias_x;
  short int bias_y;
  int dis_x;
  int dis_y;
  int begin_flag = 0;
  int angle = 60;
  int strength = 0;
  int release = 0;

  int hole_dis_x;
  int hole_dis_y;
  int pocketarea_flag = 0;

  int ballxy_triangle[15][2]={{3,4},{1,-3},{-1,2},{-3,-1},
		  	  	  	  	  	  {3,-4},{3,-2},{1,-1},{-1,0},{-5,0},{1,1},{3,0},
		  	  	  	  	  	  {-3,1},{-1,-2},{1,3},{3,2},};

  // initialize the position of the balls
  for(i = 0; i < 15; i++)			// initialize the position of the 15 balls
  {
	  ball[i].pos_x = 500 + ballxy_triangle[i][0]*8;
	  ball[i].pos_y = 310 + ballxy_triangle[i][1]*8;
	  placeball(i, ball[i].pos_x, ball[i].pos_y, 0, 0, &ball[i].bias_x, &ball[i].bias_y);
	  ball[i].flag = 1;
	  ball[i].force_move = 0;
  }
  ball[15].pos_x = 104;
  ball[15].pos_y = 300;
  //ball[15].pos_x = 328;
  //ball[15].pos_y = 433;
  placeball(15, ball[15].pos_x, ball[15].pos_y, 0, 0, &ball[15].bias_x, &ball[15].bias_y);
  ball[15].flag = 1;

  // initialize the speed of the balls
  for(i = 0; i < 16; i++)
  {
	  ball[i].vc_x = VC_MAX + 1;
	  ball[i].vc_y = VC_MAX + 1;
	  ball[i].count_x = 0;
	  ball[i].count_y = 0;
	  ball[i].dir_x = 1;
	  ball[i].dir_y = -1;
	  ball[i].dx = 0;
	  ball[i].dy = 0;
  }

  strength = 16;
  IOWR_VGA_STRENGTHBAR(16);

  //ball[15].dx = 20;
  //ball[15].vc_x = 15;

/*
  for(i = 0; i <16; i++)
  {
	  ball[i].flag = 0;
	  if( (i!= 9) && (i!=15))
	  {
	  	  ball[i].pos_x = 20*i + 10;
	  	  ball[i].pos_y = 20;
	  	  placeball(i, ball[i].pos_x, ball[i].pos_y, 0, 0, &ball[i].bias_x, &ball[i].bias_y);
	  }
  }
  ball[9].flag = 1;
  ball[15].flag = 1;
  ball[15].pos_y = ball[9].pos_y;
*/

  while(1)
  {
	  while(begin_flag == 0)
	  {
		  cx = ball[15].pos_x;
		  cy = ball[15].pos_y;
		  //printf("release1 = %d\n",release);
		  get_key(&strength, &angle, &release);
		  //printf("release2 = %d\n",release);
		  print_poolcue(cx, cy, angle);
		  IOWR_VGA_STRENGTHBAR(strength);

		  if (release == 1)
		  {
			  //printf("angle = %d; power = %d\n", angle, strength*2);
			  begin_flag = 1;
			  //!WXC
			  temp_angle = angle_trans * angle;
			  ball[15].dx= - (strength + 1) * zero * cos(temp_angle);
			  ball[15].dy= - (strength + 1) * zero * sin(temp_angle);
			  //!WXC
			  decide_direction (15);

			  ball[15].count_x = ball[15].vc_x;
			  ball[15].count_y = ball[15].vc_y;
			  //print_poolcue(0, 0, 120);

		  }
	  }
	  //printf("game begin!\n");
	  while(begin_flag == 1)
	  {
		  release = 0;
		  //printf("white x = %d; white x = %d;\n", ball_pos_x[15], ball_pos_y[15]);

		  //!WXC
		  for(i = 15; i >= 0; i--)
		  {

			  if(ball[i].flag != 0)
			  {

				  //continue;


				  move_flag = 0;
				  //Update (Position update) counter
				  if(ball[i].count_x > 0)
				  {
					  ball[i].count_x --;
					  //ball[i].count_x = ball[i].count_x - 10;
					  bias_x = 0;
				  }
				  else if(ball[i].vc_x <= VC_MAX)
					  //else if((ball[i].vc_x <= ball[i].max_vc_x) && (ball[i].vc_x <= VC_MAX))
				  {
					  ball[i].count_x = ball[i].vc_x;
					  move_flag = 1;
					  if(ball[i].dir_x == 1)
					  {
						  bias_x = 1;
					  }
					  else if(ball[i].dir_x == -1)
					  {
						  bias_x = -1;
					  }
					  else
					  {
						  bias_x = 0;
					  }
				  }
				  else
				  {
					  bias_x = 0;
				  }

				  if(ball[i].count_y > 0)
				  {
					  ball[i].count_y --;
					  //ball[i].count_y = ball[i].count_y -10;
					  bias_y = 0;
				  }
				  else if(ball[i].vc_y <= VC_MAX)
					  //else if((ball[i].vc_y <= ball[i].max_vc_y) && (ball[i].vc_y <= VC_MAX))
				  {
					  ball[i].count_y = ball[i].vc_y;
					  move_flag = 1;
					  if(ball[i].dir_y == 1)
					  {
						  bias_y = 1;
					  }
					  else if(ball[i].dir_y == -1)
					  {
						  bias_y = -1;
					  }
					  else
					  {
						  bias_y = 0;
					  }
				  }
				  else
				  {
					  bias_y = 0;
				  }


				  //Update positions
				  if(move_flag || ball[i].force_move)
				  {

					  if(move_flag)
					  {

						  moveball(i, &ball[i].pos_x, &ball[i].pos_y, bias_x, bias_y, &ball[i].bias_x, &ball[i].bias_y);
						  //ball[i].pos_x = ball[i].pos_x + bias_x;
						  //ball[i].pos_y = ball[i].pos_y + bias_y;
					  }

					  ball[i].force_move = 0;

					  //detect and deal the condition that ball hits edge
					  detect_bound_edge(i);


					  for(j = 15; j >= 0; j--)
					  {
						  if(j != i)
						  {
							  dis_x = abs(ball[i].pos_x - ball[j].pos_x);
							  dis_y = abs(ball[i].pos_y - ball[j].pos_y);
							  if(dis_x < 14 || dis_y < 14)
							  {
								  if(dis_x * dis_x + dis_y * dis_y <= 196)
								  {
									  bound_balls(i, j);
									  //printf("Oops!\n");
								  }
							  }
						  }
					  }
					  for(j = 0; j < 6; j++)
					  {
						  hole_dis_x = ball[i].pos_x - hole[j][0];
						  hole_dis_y = ball[i].pos_y - hole[j][1];

						  if(hole_dis_x * hole_dis_x + hole_dis_y * hole_dis_y <= 144)
							  //if((pow(fabs(ball[i].pos_x-hole[j][0]),2)+pow(fabs(ball[i].pos_y-hole[j][1]),2))<150)
						  {
							  printf("pocketed!\n");
							  ball[i].flag = 0;
							  if(i == 15)
							  {
								  ball[15].pos_x = 104;
								  ball[15].pos_y = 310;
								  placeball(15, ball[15].pos_x, ball[15].pos_y, 0, 0, &ball[15].bias_x, &ball[15].bias_y);
								  //begin_flag = 0;   //!WXC need to wait other balls to stop
								  ball[15].flag = 1;

							  }
							  else
							  {
								  ball[i].pos_x = 20*i + 10;
								  ball[i].pos_y = 20;
								  placeball(i, ball[i].pos_x, ball[i].pos_y, 0, 0, &ball[i].bias_x, &ball[i].bias_y);
							  }

							  break;
						  }
					  }
				  }

			  }

			  while(IORD_16DIRECT(IRTIMER_0_BASE, 0) != 0)
			  {
				  //printf("waiting...\n");
				  //for(i = 0; i < 100; i++);
			  }

			  IOWR_16DIRECT(IRTIMER_0_BASE, 0, 3000);
		  }

		  //Update Speed (dx,dy)
		  if(acc_count >= ACC)
		  {
			  acc_count = 0;
			  for(i = 16; i >= 0; i--)
			  {
				  ball[i].dx = ball[i].dx * 0.95;
				  ball[i].dy = ball[i].dy * 0.95;
				  decide_direction(i);
			  }
		  }
		  else
		  {
			  acc_count ++;
		  }

		  //!WXC: improved
		  ff=1;
		  for(j = 15; j >= 0; j--)
		  {
			  //if(!(fabs(ball[j].dx) < 0.1 && fabs(ball[j].dy) < 0.1) && ball[j].flag == 1)	//if there're still balls moving
			  if(!(ball[j].dx == 0 && ball[j].dy ==0) && ball[j].flag == 1)	//if there're still balls moving
			  {
				  ff=0;
				  break;
			  }
		  }
		  if(ff)		//if all the balls have stopped
		  {
			  begin_flag = 0;
			  release = 0;
			  //if(chang)  player=!player;
			  //chang=0;
		  }

		  //!WXC: improved
	  }
  }


 return 0;
}
