#include "HKED_Util.h"
#include <iostream>
#include <cmath>
using std::cout;
using std::endl;

float HKED_Util::UnrollViewAngle{0};
// Convert x,y,z of phototubes to position in Unrolled view
void       HKED_Util::UnrollView(double position[3],int location,float maxY)
{
	double pmtX=position[0];
	double pmtY=position[1];
	double pmtZ=position[2];
	HKED_Util::UnrollView(&pmtX ,&pmtY,&pmtZ,location,maxY);
	position[0]=pmtX;
	position[1]=pmtY;
	position[2]=pmtZ;
}

void       HKED_Util::rotateTank(double position[3],float radius, double angle)
{
	double pmtX=position[0];
	double pmtY=position[1];
	double pmtZ=position[2];
	HKED_Util::rotateTank(&pmtX ,&pmtY,&pmtZ,radius, angle);
	position[0]=pmtX;
	position[1]=pmtY;
	position[2]=pmtZ;
}
void       HKED_Util::rotateTank(double* pmtX, double* pmtY, double* pmtZ,float radius,double angle)
{
	float pangle=atan2(*pmtY,*pmtX);
	angle=pangle+angle;
	float R=sqrt(*pmtX*(*pmtX)+*pmtY*(*pmtY));
	*pmtX=R*cos(angle);
	*pmtY=R*sin(angle);
}
void       HKED_Util::UnrollView(double* pmtX ,double* pmtY,double* pmtZ,int location,float maxY)
{
	float newY;
	rotateTank(pmtX,pmtY,pmtZ,maxY,HKED_Util::UnrollViewAngle);

	if(location==0)
	{// to[] cap
		*pmtY=(*pmtZ+maxY)-*pmtY;

	}
	if(location==2)
	{// bottom
		
		*pmtY=(*pmtZ-maxY)+*pmtY;


	}	
	if(location==1)
	{ // top cap
		//float angle=atan2(*pmtY,*pmtX)+(3.1415927/2.0);
		float angle;
		if(*pmtX>0)
			angle=atan2(*pmtX,*pmtY);
		else
			angle=-atan2(-1.0*(*pmtX),*pmtY);
		float rho=maxY*angle;
		*pmtX=rho;
		*pmtY=*pmtZ;		
	}
	*pmtZ=0.0;
	float xshift=maxY*(3.1415927);
	float x=*pmtX;
	if(x>xshift)x=x-(xshift*2);
	*pmtX=x;

	return;
}