import lll.wrj4P5.*;
import lll.Loc.*;
Wrj4P5 wii;


int correctX, correctY;
float a1,b1,c1,a3,b3,a2,b2,c2;

PVector [] cal = new PVector[4];
PVector [] dots = new PVector[4];


int calPoints = 0;
boolean calibrated = false;

PFont fontA;

void setup() 
{
	size(600,400,P2D);
	wii=new Wrj4P5(this);
	background( 0 );
	fontA = loadFont("Ziggurat-HTF-Black-32.vlw");
	textFont(fontA, 32);
	wii.connect();
}

int wiiX;
int wiiY;

void draw()
{
	Loc p=wii.rimokon.irLights[0];
	if (p.x>-1) 
	{
		wiiX = (int)( ( 1 - p.x) * width );
		wiiY = (int)( ( 1 - p.y) * height );
	}

	background( 0 );
	fill( 255 );

	if( calibrated == false )
	{
		if(p.x>-1) getCalibrationPoint();

		drawCalibrationImage();  
		fill( 255 );
		text(wiiX, width / 2, height / 2);
		text(wiiY, width / 2, height / 2 + 40);
	}
	else
	{
		translateCoords( wiiX, wiiY );
		ellipse( correctX, correctY, 10, 10 );
		//ellipse( (1 - p.x)*width, (1.-p.y)*height, 10, 10);
	}
}

void drawCalibrationImage()
{
	//dot is the original calibration image
	dots[0] = new PVector( 4,4 ); //top left
	dots[1] = new PVector( width -4, 4 ); //top right
	dots[2] = new PVector( 4, height -4 ); //bot left
	dots[3] = new PVector( width -4, height -4); //bot right

	stroke( 255 );
	fill( 0 );
	for( int i = 0; i < 4 ; i ++ )
	{
		ellipse( dots[i].x,dots[i].y, 10, 10 );
	}
}

void translateCoords(int X, int Y )
{
	float corrX = (a1 * X + b1 * Y + c1 ) / (a3 * X + b3 * Y + 1 );
	float corrY = (a2 * X + b2 * Y + c2 ) / (a3 * X + b3 * Y + 1 );

	correctX = (int)corrX;
	correctY = (int)corrY;
	//println( (int)corrX + ":" + (int)corrY );
}


void getCalibrationPoint()
{
	println( "got event" );
//	if( calibrated == false )
	{
		cal[calPoints ++] = new PVector( wiiX, wiiY );
		//println( cal[calPoints].x + " " + cal[calPoints].y );

		if( calPoints == 4 )
		{
			calibrated = true; 
			calibrate();
		}
	}
	delay( 500 );

}


void calibrate()
{
	println( "running calibration" );


float [][] matrix = { 
  { -1, -1, -1, -1, 0, 0, 0, 0 },

 {   -cal[0].x, -cal[1].x, -cal[2].x, -cal[3].x, 0, 0, 0, 0 },
 { -cal[0].y, -cal[1].y, -cal[2].y, -cal[3].y, 0,0,0,0 },
 { 0,0,0,0,-1,-1,-1,-1 },
 { 0,0,0,0, -cal[0].x, -cal[1].x, -cal[2].x, -cal[3].x },
 { 0,0,0,0, -cal[0].y, -cal[1].y, -cal[2].y, -cal[3].y },
 { cal[0].x * dots[0].x, cal[1].x * dots[1].x, cal[2].x * dots[2].x, cal[3].x * dots[3].x, cal[0].x * dots[0].y, cal[1].x * dots[1].y, cal[2].x * dots[2].y, cal[3].x * dots[3].y },
 { cal[0].y * dots[0].x, cal[1].y * dots[1].x, cal[2].y * dots[2].x, cal[3].y * dots[3].x, cal[0].y * dots[0].y, cal[1].y * dots[1].y, cal[2].y * dots[2].y, cal[3].y * dots[3].y },
  };


float [] bb = { -dots[0].x, -dots[1].x, -dots[2].x, -dots[3].x, -dots[0].y, -dots[1].y, -dots[2].y, -dots[3].y };

// gauß-elimination

for( int j = 1; j < 4; j ++ )
{

   for( int i = 1; i < 8; i ++ )
   {
	matrix[i][j] = - matrix[i][j] + matrix[i][0];
   //	println (matrix[i][j]);
   }
   bb[j] = -bb[j] + bb[0];
   matrix[0][j] = 0;

}


  for( int i = 2; i < 8; i ++ )
  {
    matrix[i][2] = -matrix[i][2] / matrix[1][2] * matrix[1][1] + matrix[i][1];
  }
bb[2] = - bb[2] / matrix[1][2] * matrix[1][1] + bb[1];
matrix[1][2] = 0;


  for( int i = 2; i < 8; i ++ )
  {
    matrix[i][3] = -matrix[i][3] / matrix[1][3] * matrix[1][1] + matrix[i][1];
  }
bb[3] = - bb[3] / matrix[1][3] * matrix[1][1] + bb[1];
matrix[1][3] = 0;



  for( int i = 3; i < 8; i ++ )
  {
    matrix[i][3] = -matrix[i][3] / matrix[2][3] * matrix[2][2] + matrix[i][2];
  }
bb[3] = - bb[3] / matrix[2][3] * matrix[2][2] + bb[2];
matrix[2][3] = 0;

println( "var57, var56, var55");
println( matrix[4][6] + " " + matrix[4][5] + " " + matrix[4][4] );

for( int j = 5; j < 8; j ++ )
{
  for( int i = 4; i < 8; i ++ )
  {
     matrix[i][j] = -matrix[i][j] + matrix[i][4];
  }
  bb[j] = -bb[j] + bb[4];
  matrix[3][j] = 0;
}


for( int i = 5; i < 8; i ++ )
  {
    matrix[i][6] = -matrix[i][6] / matrix[4][6] * matrix[4][5] + matrix[i][5];
  }

bb[6] = - bb[6] / matrix[4][6] * matrix[4][5] + bb[5];
matrix[4][6] = 0;


for( int i = 5; i < 8; i ++ )
  {
    matrix[i][7] = -matrix[i][7] / matrix[4][7] * matrix[4][5] + matrix[i][5];
  }
bb[7] = - bb[7] / matrix[4][7] * matrix[4][5] + bb[5];
matrix[4][7] = 0;


for( int i = 6; i < 8; i ++ )
  {
    matrix[i][7] = -matrix[i][7] / matrix[5][7] * matrix[5][6] + matrix[i][6];
  }
bb[7] = - bb[7] / matrix[5][7] * matrix[5][6] + bb[6];
matrix[5][7] = 0;



matrix[7][7] = - matrix[7][7]/matrix[6][7]*matrix[6][3] + matrix[7][3];
bb[7] = -bb[7]/matrix[6][7]*matrix[6][3] + bb[3];
matrix[6][7] = 0;


println( "data dump" );
for( int i = 0; i < 8 ; i ++ )
{
   for( int j= 0; j < 8 ; j ++ )
   {
     print( matrix[i][j] + "," );
   }
   println("");
}

println( "bb" );
   for( int j= 0; j < 8 ; j ++ )
   {
     print( bb[j] + "," );
   }

println("");

b3 =  bb[7] /matrix[7][7];
b2 = (bb[6]-(matrix[7][6]*b3+matrix[6][6]*a3))/matrix[5][6];
a2 = (bb[5]-(matrix[7][5]*b3+matrix[6][5]*a3+matrix[5][5]*b2))/matrix[4][5];
c2 = (bb[4]-(matrix[7][4]*b3+matrix[6][5]*a3+matrix[5][4]*b2+matrix[4][4]*a2))/matrix[3][4];
a3 = (bb[3]-(matrix[7][3]*b3))/matrix[6][3];
b1 = (bb[2]-(matrix[7][2]*b3+matrix[6][2]*a3+matrix[5][2]*b2+matrix[4][2]*a2+matrix[3][2]*c2))/matrix[2][2];
a1 = (bb[1]-(matrix[7][1]*b3+matrix[6][1]*a3+matrix[5][1]*b2+matrix[4][1]*a2+matrix[3][1]*c2+matrix[2][1]*b1))/matrix[1][1];
c1 = (bb[0]-(matrix[7][0]*b3+matrix[6][0]*a3+matrix[5][0]*b2+matrix[4][0]*a2+matrix[3][0]*c2+matrix[2][0]*b1+matrix[1][0]*a1))/matrix[0][0];

println( a3 );
println( b3 );
println( b2 );
println( a2 );
println( c2 );
println( b1 );
println( a1 );
println( c1 );

}