// Ray and Sphere Intersection---------------------------------------------------------------------
float raySphereIntersection(Ray ray, SPHERE sphere, Point3dPtr n1, Point3dPtr interp1, float* kd1)
{
	float t;
	float radius;
	Point3d center;
	Point3dPtr S;
	S = (Point3dPtr)malloc(sizeof(Point3d));
	*kd1 = sphere.kd;

	radius = sphere.radius;
	center.x = sphere.x;
	center.y = sphere.y;
	center.z = sphere.z;
	subVector(&center, ray.a, S);	//S = Sphere Center Translated into Coordinate Frame of Ray Origin

	//Intersection of Sphere and Line     =       Quadratic Function of Distance 
	// A ray is defined by: R(t) = R0 + t * Rd , t > 0 with R0 = [X0, Y0, Z0] and Rd = [Xd, Yd, Zd]
	float A = dotProduct(ray.b, ray.b);		//A = Xd^2 + Yd^2 + Zd^2
	float B = -2.0*dotProduct(S, ray.b);		//B = 2 * (Xd * (X0 - Xc) + Yd * (Y0 - Yc) + Zd * (Z0 - Zc))
	float C = dotProduct(S, S) - radius*radius;	//C = (X0 - Xc)^2 + (Y0 - Yc)^2 + (Z0 - Zc)^2 - Sr^2
	float D = B*B - 4 * A*C;					//Precompute Discriminant

	if (D >= 0.0)
	{
		// if there is a shorter one just use this one, if not use another(longer one).
		int sign = (C < 0.0) ? 1 : -1;
		t = (-B + sign*sqrt(D)) / 2.f; // A should be equal to 1

		// The surface normal
		n1->x = (ray.a->x + ray.b->x*t - center.x) / radius;
		n1->y = (ray.a->y + ray.b->y*t - center.y) / radius;
		n1->z = (ray.a->z + ray.b->z*t - center.z) / radius;

		// The intersection point
		interp1->x = ray.a->x + ray.b->x*t;
		interp1->y = ray.a->y + ray.b->y*t;
		interp1->z = ray.a->z + ray.b->z*t;

		return t;	//The distance
	}
	return 0.0;
	free(S);
}

bool collideLineRectangle(rectangle_struct* rec, vect3D o, vect3D v, int32 d, int32* kk, vect3D* ip)
{
	if(!rec)return false;
	vect3D n=vect(abs(rec->normal.x),abs(rec->normal.y),abs(rec->normal.z));
	int32 p1=dotProduct(v,n);
	if(!equals(p1,0))
	{
		vect3D p=convertVect(rec->position); //CHECK lightmap generation ?
		vect3D s=vect(rec->size.x*TILESIZE*2,rec->size.y*HEIGHTUNIT,rec->size.z*TILESIZE*2);
		
		int32 p2=dotProduct(vectDifference(p,o),n);

		int32 k=divf32(p2,p1);
		s8 sign=((s.x>0)^(s.y<0)^(s.z>0)^(p1<0))?(-1):(1);
		if(kk)
		{
			*kk=k+sign;
		}
		if(k<0 || k>d){return false;}
		vect3D i=addVect(o,vectMult(v,k));
		if(ip)*ip=i;
		i=vectDifference(i,p);
		
		bool r=true;
		if(s.x)
		{
			if(s.x>0)r=r&&i.x<s.x&&i.x>=0;
			else r=r&&i.x>s.x&&i.x<=0;
		}
		if(s.y)
		{
			if(s.y>0)r=r&&i.y<s.y&&i.y>=0;
			else r=r&&i.y>s.y&&i.y<=0;
		}
		if(s.z)
		{
			if(s.z>0)r=r&&i.z<s.z&&i.z>=0;
			else r=r&&i.z>s.z&&i.z<=0;
		}
		return r;
	}
	return false;
}

SATProjection getProjection(sf::Vector2f normal, Entity* const entity)
{
	double min = dotProduct(normal, entity->getVertexGlobalPosition(0));
	double max = min;

	for (int i(1); i < entity->getVertexCount(); ++i)
	{//project each vertex of the shape onto the normal
		//then calculate the maximum and minimum values in the array and return those
		//as a projection
		double dp = dotProduct(normal, entity->getVertexGlobalPosition(i));

		if (dp < min)
			min = dp;
		else if (dp > max)
			max = dp;

	}
	return(SATProjection(min, max));
}

    void cosine(vector< double >* v1, vector< double >* v2, double m1, double m2, float & result)
    {
	if ((int)v1->size() != (int)v2->size())
	{
	    result = -1;
	    return;
	}
	float dp=dotProduct(v1,v2);
	result = (dp/(m1*m2));
    }

void Ball::handleBallCollision(vector<float> targetPosition , vector<float> targetVelocity , float targetMass , float targetRadius) {			//Resolves Ball Collisions
	//this->setVelocity()
	vector<float> newPos  	= addVectors(this->getPosition() , ScalarMult(this->getVelocity() , DELTA_T)); 					// Position in next iteration
	vector<float> deltaPos 	= addVectors(newPos , ScalarMult(targetPosition , -1.0)); 										//R1 - R2
	float speedAlongNormal 	= dotProduct(deltaPos , addVectors(targetVelocity , ScalarMult(this->getVelocity() , -1.0))); 	// (V1-V2).N
	float distSquare 		= dotProduct(deltaPos , deltaPos);									
	
	if( (distSquare <= pow( this->getRadius() + targetRadius , 2)) &&( speedAlongNormal >= 0.0) ) {
		this->setVelocity(solveBallCollision(this->getVelocity(), targetVelocity, newPos, targetPosition, this->getMass(), targetMass , coefficientRestitution).first); /// checks and updates the balls velocity if it collides with some other ball
		this-> setTimeSinceCollision(BLINK_TIME); //Display changes according to timeSinceCollision
	}

	#if defined(DEBUG) || defined(BALL_DEBUG)
		float x 			= this->getMass();
		float y 			= this->getMass() * pow(this->getVelocity(),2) * 0.5;
		string temp_output	= "Mass =" + to_string(x) + " Energy= " + to_string(y);
		cout<<temp_output<<"\n";
	#endif
}

float Vector::projectedLength(Vector B)
{//compute the length of A on B, return -1 means ERROR
  float len=B.length();
  if (fabs(len-0)<ZERO){
	printf("\nERROR: Vector::projectedLength()");
	return -1;
  }else{
	return (dotProduct(B)/len);
  }
}

bool checkTeminationConditions(Plan& P, ADDvector& x, ADDvector& lambda, ADDvector& nu, bool phase1) {
	double epsilon_feas = .01;
	double epsilon = .01;
	ADDvector r_t = R_T(P, x, lambda, nu, phase1);
	ADD dist_r_pri = CalcDist(r_t, 0, x.count());
	ADD dist_r_dual = CalcDist(r_t, x.count() + lambda.count(), r_t.count());
	ADD lambdaTest = dotProduct(-P.F(x, phase1), lambda);
	return maximum(dist_r_pri) < epsilon_feas && 
		   maximum(dist_r_dual) < epsilon_feas && 
		   maximum(lambdaTest) < epsilon;
}

bool intersectSphere(ray R, sphere S)
{
    vect deltaP = {S.x - R.x,S.y-R.y, S.z-R.z};

    double part1 = pow(dotProduct(R.u, deltaP),2.0); 
    double part2 = pow(vectLength(deltaP),2.0);
    double part3 = pow(S.r,2.0);
       double result = part1 - part2 + part3;
    if(result<=0) return false;
    return true;
}

	void Plane::moveAtBorder(Vector3D& v,bool inside) const
	{
		float dist = dotProduct(tNormal,v - getTransformedPosition());

		if ((dist <= 0.0f) == inside)
			inside ? dist += APPROXIMATION_VALUE : dist -= APPROXIMATION_VALUE;
		else
			inside ? dist -= APPROXIMATION_VALUE : dist += APPROXIMATION_VALUE;

		v += tNormal * -dist;
	}

bool CSphere::Intersects(GzRay ray, float &tValue)
{
	GzCoord origVec;
	origVec[X] =  m_centre[X] - ray.origin[X];
	origVec[Y] =  m_centre[Y] - ray.origin[Y];
	origVec[Z] =  m_centre[Z] - ray.origin[Z];

	float a = dotProduct(origVec,ray.direction); 

	float b = dotProduct(origVec, origVec) - (a*a); //bsquare value using pythagoras theorem

	float f = (m_radius * m_radius) - b;

	if(f < 0) //means square root wil definitely be negative
		return false;
	
	tValue = a - sqrt(f);
	return true;

}

    Vector3D Cylinder::computeNormal(const Vector3D& point) const
    {
        float dist = dotProduct(tDirection,point - getTransformedPosition());
        if(dist >= length*0.5f) return tDirection;
		if(dist <= -length*0.5f) return -tDirection;

		Vector3D ext = point - (tDirection*dist + getTransformedPosition());
		float r = ext.getNorm(); ext = ext / r;

		return ext;
    }

double VectorType::angleBetweenVectors(VectorType secondVector)
{
	//cos(theta) = (u . v) / (|u|*|v|
	double theta = acos(dotProduct(secondVector)
		/
		(vectorLength() * secondVector.vectorLength())

		);
	
	return theta;
}

void LatticeReduction::reduceFast(Tensor&t){
  Vector v[3];
  v[0]=t.getRow(0);
  v[1]=t.getRow(1);
  v[2]=t.getRow(2);
  while(true){
    sort(v);
    reduce(v[0],v[1]);
    double b11=modulo2(v[0]);
    double b22=modulo2(v[1]);
    double b12=dotProduct(v[0],v[1]);
    double b13=dotProduct(v[0],v[2]);
    double b23=dotProduct(v[1],v[2]);
    double z=b11*b22-b12*b12;
    double y2=-(b11*b23-b12*b13)/z;
    double y1=-(b22*b13-b12*b23)/z;
    int x1min=floor(y1);
    int x1max=x1min+1;
    int x2min=floor(y2);
    int x2max=x2min+1;
    bool first=true;
    double mbest,mtrial;
    Vector trial,best;
    for(int x1=x1min;x1<=x1max;x1++)
    for(int x2=x2min;x2<=x2max;x2++){
      trial=v[2]+x2*v[1]+x1*v[0];
      mtrial=modulo2(trial);
      if(first || mtrial<mbest){
        mbest=mtrial;
        best=trial;
        first=false;
      }
    }
    if(modulo2(best)+epsilon>=modulo2(v[2])) break;
    v[2]=best;
  }
  sort(v);
  t.setRow(0,v[0]);
  t.setRow(1,v[1]);
  t.setRow(2,v[2]);
}

void Screen::rayTrace(const P_FLT time) {
  clock_t startTimer, endTimer;

  startTimer = clock();

  P_FLT horizontal = sin(scene->camera->angle * 0.5f),
        vertical = horizontal / scene->camera->aspectRatio;

  Vector3D forward = scene->camera->forward,
           up = scene->camera->up,
           top = up - forward * dotProduct(up, forward),
           left = scene->camera->left;
  top.normalize();
  top *= vertical;
  left *= horizontal;

  Point3D center = scene->camera->viewpoint + forward,
          topLeft = center + top + left;

  Vector3D pixelHor = -left / static_cast<P_FLT>(width / 2),
           pixelVert = -top / static_cast<P_FLT>(height / 2);

  Vector3D topLeftPixel = topLeft - scene->camera->viewpoint +
                          (pixelHor * 0.5f) + (pixelVert * 0.5f);

  ScreenTracer * screenTracer = new ScreenTracer(scene);
  for (int i = 0; i < width; i++) {
    for (int j = 0; j < height; j++) {
      Vector3D rayDir = topLeftPixel +
                        pixelHor * static_cast<P_FLT>(i) +
                        pixelVert * static_cast<P_FLT>(j);
      rayDir.normalize();

      pixels[j * width + i] = Color();
      screenTracer->addTask(&pixels[j * width + i], rayDir);
    }
  }
  screenTracer->init(scene->camera->viewpoint, pixelHor, pixelVert);

  pthread_t * threads = new pthread_t[threadNum];
  for (int i = 0; i < threadNum; i++) {
    pthread_create(&threads[i], NULL, &runScreenTracer,
                   static_cast<void *>(screenTracer));
  }
  for (int i = 0; i < threadNum; i++) {
    pthread_join(threads[i], NULL);
  }
  endTimer = clock();
  printf("\rTracing image...100.0%% completed (%.3f seconds).\n",
         clockTimeMT(startTimer, endTimer));

  delete screenTracer;
}

ColorRGB DiffSpecMaterial::ambientResponse(const ONB& uvw, const Vector3D& v_in, const Vector3D& p, const Vector2D& uv)
{
	float cosine = dotProduct(v_in, uvw.w());
	if (cosine < 0.0f) cosine = -cosine;
	float temp1 = 1.0f - cosine;
	float R = R0 + (1.0f - R0) *temp1*temp1*temp1*temp1*temp1;
	float P = (R + 0.5f) / 2.0f;
	if(rng() <= P)
		return spec_mat->ambientResponse(uvw, v_in, p, uv);
	else
		return diff_mat->ambientResponse(uvw, v_in, p, uv);
}

bool Function::inTriangleUseBarycenter(Point p, Point end1, Point end2, Point end3) {
	Point v0 = subtract(end3, end1);
	Point v1 = subtract(end2, end1);
	Point v2 = subtract(p, end1);

	double dot00 = dotProduct(v0, v0);
	double dot01 = dotProduct(v0, v1);
	double dot02 = dotProduct(v0, v2);
	double dot11 = dotProduct(v1, v1);
	double dot12 = dotProduct(v1, v2);

	double inverDeno = 1 / (dot00 * dot11 - dot01 * dot01);
	double u = (dot11 * dot02 - dot01 * dot12) * inverDeno;
	if (u < 0 || u > 1)
		return false;
	double v = (dot00 * dot12 - dot01 * dot02) * inverDeno;
	if (v < 0 || v > 1)
		return false;

	return (u + v) <= 1;
}

void multiply (float *A, float *B, float *out, int size) {
    float *temp = new float[size];

    for (int col=0; col<size; col++) {
        for (int i=0; i<size; i++)
            temp[i] = B[i * size + col];
        for (int row=0; row<size; row++)
            out[row * size + col] = dotProduct(&A[row], temp, size);
    }

    delete[] temp;
}

int main()
{
	printf("\n a = "); printVector(a);
	printf("\n b = "); printVector(b);
	printf("\n c = "); printVector(c);
	printf("\n a . b = %f",dotProduct(a,b));
	printf("\n a x b = "); printVector(crossProduct(a,b));
	printf("\n a . (b x c) = %f",scalarTripleProduct(a,b,c));
	printf("\n a x (b x c) = "); printVector(vectorTripleProduct(a,b,c));
	
	return 0;
}

void Pbc::buildShifts(std::vector<Vector> shifts[2][2][2])const{
  const double small=1e-28;

// clear all shifts
  for(int i=0;i<2;i++) for(int j=0;j<2;j++) for(int k=0;k<2;k++) shifts[i][j][k].clear();

// enumerate all possible shifts
// since box is reduced, only 27 shifts have to be attempted
  for(int l=-1;l<=1;l++) for(int m=-1;m<=1;m++) for(int n=-1;n<=1;n++){

// int/double shift vectors
    int ishift[3]={l,m,n};
    Vector dshift(l,m,n);

// count how many components are != 0
    unsigned count=0;
    for(int s=0;s<3;s++) if(ishift[s]!=0) count++;

// skips trivial (0,0,0) and cases with three shifts
// only 18 shifts survive past this point
    if(count==0 || count==3) continue;

// check if that Wigner-Seitz face is perpendicular to the axis.
// this allows to eliminate shifts in symmetric cells.
// e.g., if one lactice vector is orthogonal to the plane spanned
// by the other two vectors, that shift should never be tried
    Vector cosdir=matmul(reduced,transpose(reduced),dshift);
    double dp=dotProduct(dshift,cosdir);
    double ref=modulo2(dshift)*modulo2(cosdir);
    if(std::fabs(ref-dp*dp)<small) continue;

// here we start pruning depending on the sign of the scaled coordinate
    for(int i=0;i<2;i++) for(int j=0;j<2;j++) for(int k=0;k<2;k++){

      int block[3]={2*i-1,2*j-1,2*k-1};

// skip cases where shift would bring too far from origin
      bool skip=false;
      for(int s=0;s<3;s++) if(ishift[s]*block[s]>0) skip=true;
      if(skip) continue;
      skip=true;
      for(int s=0;s<3;s++){
// check that the components of cosdir along the non-shifted directions
// have the proper sign
        if(((1-ishift[s]*ishift[s])*block[s])*cosdir[s]<-small) skip=false;
      }
      if(skip)continue;

// if we arrive to this point, shift is eligible and is added to the list
      shifts[i][j][k].push_back(matmul(transpose(reduced),dshift));
    }
  }
}

bool collideLineConvertedRectangle(vect3D n, vect3D p, vect3D s, vect3D o, vect3D v, int32 d, int32* kk, vect3D* ip)
{
	int32 p1=dotProduct(v,n);
	if(!equals(p1,0))
	{
		int32 p2=dotProduct(vectDifference(p,o),n);

		int32 k=divf32(p2,p1);
		s8 sign=((s.x>0)^(s.y<0)^(s.z>0)^(p1<0))?(-1):(1);
		if(kk)
		{
			*kk=k+sign;
		}
		if(k<0 || k>d){return false;}
		vect3D i=addVect(o,vectMult(v,k));
		if(ip)*ip=i;
		i=vectDifference(i,p);
		NOGBA("I %d %d %d",i.x,i.y,i.z);
		NOGBA("S %d %d %d",s.x,s.y,s.z);
		
		bool r=true;
		if(s.x)
		{
			if(s.x>0)r=r&&i.x<s.x&&i.x>=0;
			else r=r&&i.x>s.x&&i.x<=0;
		}
		if(s.y)
		{
			if(s.y>0)r=r&&i.y<s.y&&i.y>=0;
			else r=r&&i.y>s.y&&i.y<=0;
		}
		if(s.z)
		{
			if(s.z>0)r=r&&i.z<s.z&&i.z>=0;
			else r=r&&i.z>s.z&&i.z<=0;
		}
		return r;
	}
	return false;
}