#include "iostream"
#include "stdio.h"
#include "TCanvas.h"
#include "TView.h"
#include "TBRIK.h"
#include "TTUBE.h"
#include "TNode.h"
#include "TPolyMarker3D.h"
#include "Riostream.h"

#define ABS(x) ((x>0)?(x):(-x))

void rotation_matrix(Double_t *rot,double a, double b, double c);
void line_3D(double x1,double y1,double z1,double x2,double y2,double z2,int N,int color);
void point_3D(double x,double y,double z,int color);
bool intersection(
	double xs,double ys,double zs,//source position
	double a,double b,double c,//ray direction
	double xc,double yc,double zc,//cylinder position: center coordinates
	double rc,double hc,//cylinder dimensions: radius rc and half-height hc
	double &xi1,double &yi1,double &zi1,//intersection point 1
	double &xi2,double &yi2,double &zi2);//intersection point 2
int assign_points(double k1,double x1,double y1,double z1,
	double k2,double x2,double y2,double z2,
	double &xi1,double &yi1,double &zi1,
	double &xi2,double &yi2,double &zi2);

void display_3D(
	//char* filename, //nom du fichier de sauvegarde des données sur les rayons
	int Nppl=200, //nb de points/ligne pour les lignes 3D
	bool disp_cyl=true, //afficher le cylindre
	bool disp_main_base=true, //afficher la base principale (ici telle que z(cylindre)=0)
	bool disp_source_base=true, //afficher la base de la source
	int disp_rays=3, //afficher les rayons (=0,1,2,3) (jaune=intersecte, vert=n'intersecte pas)
	bool disp_angle_limits=true,//afficher les angles limites
	bool disp_in_pts=true, //afficher les points d'entrées
	bool disp_out_pts=true, //afficher les points de sortie
	bool disp_src_pts=true) //afficher les points sources
{
  //parameters
  int N=200000; // Nombre de tirages
  int M=0; // Nombre de faisceaux ayant touches le détecteur
  double sigma=0.1;//ecart-type
  double xd=5;//x du cylindre
  double yd=0;//y du cylindre
  double zd=10;//hauteur de la source: z(source)=-zd
  double dl=8;//hauteur du cylindre
  double r0=2;//rayon du cylindre
  double omega=0; // Angle solide à renvoyer
  double err=0; // Erreur associée à l'angle solide calculé

  //input data variables
  double *x=new double[N];
  double *y=new double[N];
  double *theta=new double[N];
  double *alpha=new double[N];

  //read data & parameters
  FILE *in=fopen("rays.txt","r");//filename,"r");
  fscanf(in,"%d %d %lf %lf %lf %lf %lf %lf %lf %lf\n\n",
	&N,&M,&sigma,&xd,&yd,&zd,&dl,&r0,&omega,&err);
  for (int i=0;i<M;i++)
  {
    fscanf(in,"%lf %lf %lf %lf\n",&x[i],&y[i],&theta[i],&alpha[i]);
  }
  fclose(in);

  //write out to check
  FILE *out=fopen("output2.txt","w");
  fprintf(out,"%d %d %lf %lf %lf %lf %lf %lf %lf %lf\n\n",N,M,sigma,xd,yd,zd,dl,r0,omega,err);
  for (int i=0;i<M;i++)
  {
    fprintf(out,"%lf %lf %lf %lf\n",x[i],y[i],theta[i],alpha[i]);
  }
  fclose(out);

  //define PI
  Double_t PI=TMath::Pi();

  bool inter;
  double xs,ys,zs;
  double a,b,c;
  double xc,yc,zc;
  double rc,hc;
  double xi1,yi1,zi1;
  double xi2,yi2,zi2;

  xc=xd;
  yc=yd;
  zc=dl/2.0;
  rc=r0;
  hc=dl/2.0;

  // create and open a canvas
  TCanvas *display = new TCanvas( "canvas_name", "canvas_title",0,0,500,500);

  // creating view
  TView *view = new TView(1);
  float range = 10;
  view->SetRange( -range, -range, -range, range, range, range );

  //define reference node
  TBRIK *ref_pointer = new TBRIK("ref_name","ref_title","void",0,0,0);
  TNode *ref_node_pointer = new TNode("ref_node_name","ref_node_title","ref_name");
  ref_node_pointer->cd();

  Double_t rot[9];
  rotation_matrix(rot,0,0,0);
  TRotMatrix *rot_pointer=new TRotMatrix("rot_name","rot_title",rot);

  TTUBE *tube_pointer=
	new TTUBE("tube_name","tube_title","void",0,rc,hc);
  TNode *node_pointer=
	new TNode("node_name","node_title",tube_pointer,xc,yc,zc,rot_pointer);

  //draw all 3D solids
  if(disp_cyl) ref_node_pointer->Draw();

  //number of points/line
  //int Nppl=2000;
  
  //draw main 3D base
  if(disp_main_base)
  {
	line_3D(0,0,0,range,0,0,Nppl,2);
	line_3D(0,0,0,0,range,0,Nppl,2);
	line_3D(0,0,0,0,0,range,Nppl,2);
  }
  //draw source 3D base
  if(disp_source_base)
  {
	line_3D(0,0,-zd,range,0,-zd,Nppl,2);
	line_3D(0,0,-zd,0,range,-zd,Nppl,2);
	line_3D(0,0,-zd,0,0,-zd+range,Nppl,2);
  }

  //draw rays and calculate intersections
  FILE *inter_file=fopen("intersections.txt","w");
  for(int i=0;i<M;i++)
  {
	xs=x[i];
	ys=y[i];
	zs=-zd;
	a=sin(theta[i])*cos(alpha[i]);
	b=sin(theta[i])*sin(alpha[i]);
	c=cos(theta[i]);

	//calculate intersections
	inter=intersection(xs,ys,zs,a,b,c,xc,yc,zc,rc,hc,xi1,yi1,zi1,xi2,yi2,zi2);
	fprintf(inter_file,
		"ray %d\tinter=%d\t(xi1,yi1,zi1)=(%f,%f,%f)\t(xi2,yi2,zi2)=(%f,%f,%f)\n",
		i,inter,xi1,yi1,zi1,xi2,yi2,zi2);

	//print errors (non-intersecting rays)
	if(!inter) printf("ray %d\tinter=%d\n",i,inter);

	//draw lines and points
	//draw angle limits
	if(i<4 && disp_angle_limits)
		line_3D(xs,ys,zs,xs+2*range*a,ys+2*range*b,-zd+2*range*c,Nppl,9);
   	//draw full rays
	if(i>=4 && inter && disp_rays==3)
		line_3D(xs,ys,zs,xs+2*range*a,ys+2*range*b,-zd+2*range*c,Nppl,5);
	if(i>=4 && !inter && disp_rays==3)
		line_3D(xs,ys,zs,xs+2*range*a,ys+2*range*b,-zd+2*range*c,Nppl,8);
	//draw stopped rays 1
	if(i>=4 && inter && disp_rays==1) line_3D(xs,ys,zs,xi1,yi1,zi1,Nppl,5);
	if(i>=4 && !inter && disp_rays==1) line_3D(xs,ys,zs,xi1,yi1,zi1,Nppl,8);
	//draw stopped rays 2
	if(i>=4 && inter && disp_rays==2) line_3D(xs,ys,zs,xi2,yi2,zi2,Nppl,5);
	if(i>=4 && !inter && disp_rays==2) line_3D(xs,ys,zs,xi2,yi2,zi2,Nppl,8);
	//draw points
printf("ray %i:(%lf,%lf,%lf)->(%lf,%lf,%lf)\n",i,xi1,yi1,zi1,xi2,yi2,zi2);
	if(disp_in_pts) point_3D(xi1,yi1,zi1,5);
	if(disp_out_pts) point_3D(xi2,yi2,zi2,7);
	if(disp_src_pts) point_3D(xs,ys,zs,3);
  }
  fclose(inter_file);
}

void rotation_matrix(Double_t *rot,double a, double b, double c)
{
  //first column
  rot[3*0+0]=cos(c)*cos(b)*cos(a)-sin(c)*sin(a);
  rot[3*1+0]=cos(c)*cos(b)*sin(a)+sin(c)*cos(a);
  rot[3*2+0]=-cos(c)*sin(b);
  //second column
  rot[3*0+1]=-sin(c)*cos(b)*cos(a)-cos(c)*sin(a);
  rot[3*1+1]=-sin(c)*cos(b)*sin(a)+cos(c)*cos(a);
  rot[3*2+1]=sin(c)*sin(b);
  //third column
  rot[3*0+2]=sin(b)*cos(a);
  rot[3*1+2]=sin(b)*sin(a);
  rot[3*2+2]=cos(b);
}

void line_3D(double x1,double y1,double z1,double x2,double y2,double z2,int N,int color)
{
  TPolyMarker3D *pm3d = new TPolyMarker3D(N);

  pm3d->SetMarkerColor(color);

  double x,y,z;

  // set points
  for(int i=0;i<N;i++)
  {
    x = x1+i*(x2-x1)/(N-1);
    y = y1+i*(y2-y1)/(N-1);
    z = z1+i*(z2-z1)/(N-1);
    pm3d->SetPoint(i,x,y,z);
  }

  pm3d->Draw();
}

void point_3D(double x,double y,double z,int color)
{
  TPolyMarker3D *pm3d = new TPolyMarker3D(1);

  pm3d->SetMarkerColor(color);
  pm3d->SetPoint(0,x,y,z);
  pm3d->Draw();
}

bool intersection(
	double xs,double ys,double zs,//source position
	double a,double b,double c,//ray direction
	double xc,double yc,double zc,//cylinder position: center coordinates
	double rc,double hc,//cylinder dimensions: radius rc and half-height hc
	double &xi1,double &yi1,double &zi1,//intersection point 1
	double &xi2,double &yi2,double &zi2)//intersection point 2
{
	//return value
	bool ret=false;

	//arbitrary initialisation
	xi1=0;yi1=0;zi1=0;
	xi2=0;yi2=0;zi2=0;

	//"tri-boolean" intersection variables: 0=nothing,1=intersection,2=tangent
	int top_inter=0;
	int tube_inter=0;
	int bottom_inter=0;

	//special boolean variables when tube_inter=1
	//In that case, there are 2 intersections, which can be on the cylinder or not.
	bool tube_inter_1=false;
	bool tube_inter_2=false;

	//We change our referential so that the cylinder is at the center.
	double xr=xs-xc;
	double yr=ys-yc;
	double zr=zs-zc;
	//For now, we'll consider that the cylinder isn't rotated=>ar=a,br=b,cr=c:
	double ar=a;
	double br=b;
	double cr=c;
	
	//moving point M(x,y,z)
	//x=xr+k*a;
	//y=yr+k*b;
	//z=zr+k*c;
	//must verify x^2+y^2=rc^2 and -hc<z<hc (tube intersection)
	//or x^2+y^2<rc^2 and ABS(z)=hc (disk intersection)

	//"_t"=top disk,"_b"=bottom disk,"_1"&"_2"=tube
	double k_t=0,xi_t=0,yi_t=0,zi_t=0;
	double k_b=0,xi_b=0,yi_b=0,zi_b=0;
	double k_1=0,xi_1=0,yi_1=0,zi_1=0;
	double k_2=0,xi_2=0,yi_2=0,zi_2=0;

	double A,B,C,delta;

	//disk intersections
	if(c==0)
	{
		if(zr==hc) top_inter=2;
		if(zr==-hc) bottom_inter=2;
	}
	else
	{
		//case 1: top disk intersection:
		k_t=(hc-zr)/c;
		xi_t=xr+k_t*ar;
		yi_t=yr+k_t*br;
		zi_t=zr+k_t*cr;
		if(xi_t*xi_t+yi_t*yi_t<rc*rc)
		{
			top_inter=1;
		}
		//case 2: bottom disk intersection:
		k_b=(-hc-zr)/c;
		xi_b=xr+k_b*ar;
		yi_b=yr+k_b*br;
		zi_b=zr+k_b*cr;
		if(xi_b*xi_b+yi_b*yi_b<rc*rc)
		{
			bottom_inter=1;
		}
	}

	//tube intersections
	//resolution of the second order equation x^2+y^2+z^2=rc^2=>A*k^2+B*k+C=0:
	A=ar*ar+br*br;
	B=2*(ar*xr+br*yr);
	C=(xr*xr+yr*yr-rc*rc);
	delta=B*B-4*A*C;
	if(A==0)
	{
		if(C==0) tube_inter=2;
		else tube_inter=0;
	}
	else
	{
		if(delta<0)
		{
			tube_inter=0;
		}
		else
		{
			k_1=(-B+sqrt(delta))/(2*A);
			k_2=(-B-sqrt(delta))/(2*A);
			xi_1=xr+k_1*a;
			yi_1=yr+k_1*b;
			zi_1=zr+k_1*c;
			xi_2=xr+k_2*a;
			yi_2=yr+k_2*b;
			zi_2=zr+k_2*c;
			if(ABS(zi_1)<hc)
			{
				tube_inter=1;
				tube_inter_1=true;				
			}
			if(ABS(zi_2)<hc)
			{
				tube_inter=1;
				tube_inter_2=true;
			}
		}
	}

//	if(top_inter==2) printf("warning: top_inter==2\n");
//	if(tube_inter==2) printf("warning: tube_inter==2\n");
//	if(bottom_inter==2) printf("warning: bottom_inter==2\n");

	//return values

	//We consider that a tangent ray does not "intersect":
	top_inter=top_inter%2;
	tube_inter=tube_inter%2;
	bottom_inter=bottom_inter%2;
	//There are then 6 possible intersection cases:
	if(top_inter*bottom_inter)
	{
		assign_points(k_t,xi_t,yi_t,zi_t,
			k_b,xi_b,yi_b,zi_b,
			xi1,yi1,zi1,
			xi2,yi2,zi2);
		printf("top_inter*bottom_inter:");
		if(ret) printf("warning: multiple cases\n");
		ret=true;
	}
	if(top_inter*tube_inter_1)
	{
		assign_points(k_t,xi_t,yi_t,zi_t,
			k_1,xi_1,yi_1,zi_1,
			xi1,yi1,zi1,
			xi2,yi2,zi2);
		printf("top_inter*tube_inter_1:");
		if(ret) printf("warning: multiple cases\n");
		ret=true;
	}
	if(top_inter*tube_inter_2)
	{
		assign_points(k_t,xi_t,yi_t,zi_t,
			k_2,xi_2,yi_2,zi_2,
			xi1,yi1,zi1,
			xi2,yi2,zi2);
		printf("top_inter*tube_inter_2:");
		if(ret) printf("warning: multiple cases\n");
		ret=true;
	}
	if(bottom_inter*tube_inter_1)
	{
		assign_points(k_b,xi_b,yi_b,zi_b,
			k_1,xi_1,yi_1,zi_1,
			xi1,yi1,zi1,
			xi2,yi2,zi2);
		printf("bottom_inter*tube_inter_1:");
		if(ret) printf("warning: multiple cases\n");
		ret=true;
	}
	if(bottom_inter*tube_inter_2)
	{
		assign_points(k_b,xi_b,yi_b,zi_b,
			k_2,xi_2,yi_2,zi_2,
			xi1,yi1,zi1,
			xi2,yi2,zi2);
		printf("bottom_inter*tube_inter_2:");
		if(ret) printf("warning: multiple cases\n");
		ret=true;
	}
	if(tube_inter_1*tube_inter_2)
	{
		assign_points(k_1,xi_1,yi_1,zi_1,
			k_2,xi_2,yi_2,zi_2,
			xi1,yi1,zi1,
			xi2,yi2,zi2);
		printf("tube_inter_1*tube_inter_2:");
		if(ret) printf("warning: multiple cases\n");
		ret=true;
	}
/*
if(top_inter)
{
printf("top_inter=%d,tube_inter=%d,bottom_inter=%d,tube_inter_1=%d,tube_inter_2=%d\n",top_inter,tube_inter,bottom_inter,tube_inter_1,tube_inter_2);
printf("(xi1,yi1,zi1)=(%lf,%lf,%lf) (xi2,yi2,zi2)=(%lf,%lf,%lf)\n",xi1,yi1,zi1,xi2,yi2,zi2);
}
*/
	if(ret)
	{
		//We change back to the original referential.
		xi1=xi1+xc;
		yi1=yi1+yc;
		zi1=zi1+zc;
		xi2=xi2+xc;
		yi2=yi2+yc;
		zi2=zi2+zc;
		printf("(%lf,%lf,%lf)->(%lf,%lf,%lf)\n",xi1,yi1,zi1,xi2,yi2,zi2);
		return(true);
	}
	else return(false);
}

int assign_points(double k1,double x1,double y1,double z1,
	double k2,double x2,double y2,double z2,
	double &xi1,double &yi1,double &zi1,
	double &xi2,double &yi2,double &zi2)
{
	if(k1<k2)
	{
		xi1=x1;yi1=y1;zi1=z1;
		xi2=x2;yi2=y2;zi2=z2;
	}
	else
	{
		xi1=x2;yi1=y2;zi1=z2;
		xi2=x1;yi2=y1;zi2=z1;
	}				
	return(0);
}