#include "Object3D.h"

#include "GeoAlgs.h"

Object3D::Object3D(Vector3DW& center, Vector3DW& xe, Vector3DW& ye, Vector3DW& ze)
{
  center_ = center;
  xe_ = xe;
  ye_ = ye;
  ze_ = ze;
  generateMatrices();
}

void 
Object3D::generateMatrices()
{
  T = Matrix4x4(xe_.getX(), ye_.getX(), ze_.getX(), 0,
				xe_.getY(), ye_.getY(), ze_.getY(), 0,
				xe_.getZ(), ye_.getZ(), ze_.getZ(), 0,
				0, 0, 0, 1);
}

void 
Object3D::setPosition(Vector3DW& pos)
{
  center_ = pos;
}

void 
Object3D::translate(real dx, real dy, real dz)
{
  center_ = center_ + Vector3DW(dx, dy, dz);
}


ObjectComposite::ObjectComposite(Vector3DW& center, Vector3DW& xe, Vector3DW& ye, Vector3DW& ze) 
	  :Object3D(center, xe, ye, ze)
{

}

ObjectComposite::~ObjectComposite() 
{
  for(int i=0; i<components_.size(); i++) delete(components_[i]);
}

bool 
ObjectComposite::rayIntersect(
	  Vector3DW& rOrigin, 
	  Vector3DW& rDirection, 
	  Vector3DW& intersectionPoint, 
	  Vector3DW& intersectionNormal, 
	  real& intersectionDistance, 
	  Color& color,
	  real& ka,
	  real& kd,
	  real& ks)
{
  bool intersect = false;
  bool baux = false;
  real minDist = (real)999999999;

  Vector3DW intersectionPoint1; 
  Vector3DW intersectionNormal1; 
  real intersectionDistance1; 
  Color color1;
  real ka1;
  real kd1;
  real ks1;

  for(int i=0; i<components_.size(); i++)
  {
	baux = components_[i]->rayIntersect(rOrigin, rDirection, intersectionPoint1, intersectionNormal1, intersectionDistance1, color1, ka1, kd1, ks1);
	if(baux && (intersectionDistance1 < minDist))
	{
	  minDist = intersectionDistance1;
	  intersect = true;

	  intersectionPoint = intersectionPoint1; 
	  intersectionNormal = intersectionNormal1; 
	  intersectionDistance = intersectionDistance1; 
	  color = color1;
	  ka = ka1;
	  kd = kd1;
	  ks = ks1;
	}
  }

  return intersect;
}

void 
ObjectComposite::drawOpenGL()
{
  for(int i=0; i<components_.size(); i++)
  {
	components_[i]->drawOpenGL();
  }
}

void 
ObjectComposite::translate(real dx, real dy, real dz)
{
  center_ = center_ + Vector3DW(dx, dy, dz);

  for(int i=0; i<components_.size(); i++)
  {
	components_[i]->translate(dx, dy, dz);
  }
}


Sphere::Sphere(Vector3DW& center, Vector3DW& xe, Vector3DW& ye, Vector3DW& ze, real r, Material* material)
	  :Object3D(center, xe, ye, ze)
{
  r_ = r;
  material_ = material;
}

bool 
Sphere::rayIntersect(
	  Vector3DW& rOrigin, 
	  Vector3DW& rDirection, 
	  Vector3DW& intersectionPoint, 
	  Vector3DW& intersectionNormal, 
	  real& intersectionDistance, 
	  Color& color,
	  real& ka,
	  real& kd,
	  real& ks)
{
  Vector3DW OC = center_ - rOrigin;
  bool inside = (OC.getNorm() < r_);
  real L = rDirection * OC;
  if(L < 0) return false;
  real D = (real) sqrt(OC.getNorm()*OC.getNorm() - L*L);
  if(D > r_) return false;
  real HC = (real) sqrt(r_*r_ - D*D);
  real t;
  if(inside) t = L + HC;
  else t = L - HC;
  Vector3DW i = rOrigin + rDirection*t;
  Vector3DW n = (i - center_)*(1/r_);

  intersectionPoint.setX(i.getX());
  intersectionPoint.setY(i.getY());
  intersectionPoint.setZ(i.getZ());
  intersectionNormal.setX(n.getX());
  intersectionNormal.setY(n.getY());
  intersectionNormal.setZ(n.getZ());
  intersectionDistance = t;

  Vector3DW CP = intersectionPoint - center_;
  real textX = (real)0.5 + (CP * xe_) / ((real)2 * r_);
  real textUp = (real)0.5 + (CP * ye_) / ((real)2 * r_);

  if(material_ != NULL) 
  {
	material_->getColorInfo(textX, textUp, color, ka, kd, ks);
  }

  return true;
}

void 
Sphere::drawOpenGL()
{
  glLoadIdentity();
  glTranslatef(center_.getX(), center_.getY(), center_.getZ());
  Color color;
  real raux;
  material_->getColorInfo(0, 0, color, raux, raux, raux);
  real r = (real)color.getR() / (real)255;
  real g = (real)color.getG() / (real)255;
  real b = (real)color.getB() / (real)255;
  glColor3d(r, g, b);
  glutSolidSphere(r_, 32, 32);
}

Circle::Circle(Vector3DW& center, Vector3DW& xe, Vector3DW& ye, Vector3DW& ze, real r, Material* material)
	  :Object3D(center, xe, ye, ze)
{
  r_ = r;
  material_ = material;
}

bool 
Circle::rayIntersect(
	  Vector3DW& rOrigin, 
	  Vector3DW& rDirection, 
	  Vector3DW& intersectionPoint, 
	  Vector3DW& intersectionNormal, 
	  real& intersectionDistance, 
	  Color& color,
	  real& ka,
	  real& kd,
	  real& ks)
{
  Vector3DW rOrigin1 = T * (rOrigin - center_);
  Vector3DW rDirection1 = T * rDirection;
  real taux = 0;
  real xTex = 0;
  real yTex = 0;
  real xCross = 0;
  real yCross = 0;

  if(rDirection1.getZ() != 0)
  {
	taux = (- rOrigin1.getZ()) / rDirection1.getZ();
	xCross = rOrigin1.getX() + taux*rDirection1.getX();
	yCross = rOrigin1.getY() + taux*rDirection1.getY();
	if(taux > 0)
	if((real)(xCross*xCross + yCross*yCross) <= r_*r_)
	{
	  if(rOrigin1.getZ() < 0) intersectionNormal = ze_*(real)-1;
	  else intersectionNormal = ze_;

	  intersectionPoint = rOrigin + rDirection*taux;
	  intersectionDistance = taux;

	  xTex = xCross / ((real)2*r_) + (real)0.5;
	  yTex = yCross / ((real)2*r_) + (real)0.5;

	  if(material_ != NULL) 
	  {
		material_->getColorInfo(xTex, yTex, color, ka, kd, ks);
	  }

	  return true;
	}
  }

  return false;
}


Cylinder::Cylinder(Vector3DW& center, Vector3DW& xe, Vector3DW& ye, Vector3DW& ze, real r, real height, Material* material)
	  :Object3D(center, xe, ye, ze)
{
  r_ = r;
  height_ = height;
  material_ = material;
}

bool 
Cylinder::rayIntersect(
	  Vector3DW& rOrigin, 
	  Vector3DW& rDirection, 
	  Vector3DW& intersectionPoint, 
	  Vector3DW& intersectionNormal, 
	  real& intersectionDistance, 
	  Color& color,
	  real& ka,
	  real& kd,
	  real& ks)
{
  Vector3DW rOrigin1 = T * (rOrigin - center_);
  Vector3DW rDirection1 = T * rDirection;

  rOrigin1.setZ(0);
  rDirection1.setZ(0);
  real normD = rDirection1.getNorm();
  rDirection1.normalize();

  real L = (rOrigin1 * rDirection1) * (real)-1;
  Vector3DW uv = Vector3DW(0, 0, 1) % rDirection1;
  real distance = (real)fabs((rOrigin1 * uv) / (uv.getNorm()));
  if(distance > r_) return false;
  real HC = (real)sqrt(r_*r_ - distance*distance);
  real t1 = (L - HC) / normD;
  real t2 = (L + HC) / normD;
  Vector3DW pt1 = rOrigin + (rDirection * t1);
  Vector3DW pt2 = rOrigin + (rDirection * t2);
  real d1 = (pt1 - center_) * ze_;
  real d2 = (pt2 - center_) * ze_;

  if((t1 > 0)&&(d1 >= 0)&&(d1 <= height_))
  {
	if((t2 > 0)&&(d2 >= 0)&&(d2 <= height_)) 
	{ 
	  if(t1 < t2) 
	  {
		intersectionPoint = pt1;
		intersectionDistance = t1;
	  } else 
	  {
		intersectionPoint = pt2; 
		intersectionDistance = t2;
	  }
	} else 
	{
	  intersectionPoint = pt1;
	  intersectionDistance = t1;
	}
  } else
  {
	if((t2 > 0)&&(d2 >= 0)&&(d2 <= height_)) 
	{
	  intersectionPoint = pt2;
	  intersectionDistance = t2;
	} else return false;
  }

  intersectionNormal = (intersectionPoint - center_);
  intersectionNormal = intersectionNormal - (ze_ * (intersectionNormal * ze_));
  intersectionNormal.normalize();

  if(material_ != NULL) 
  {
	Vector3DW ptaux = T * (intersectionPoint - center_);
	real cosY = ptaux.getY() / r_;
	real xTex = (real)acos(cosY) / M_PI;
	real yTex = ptaux.getZ() / height_;
	material_->getColorInfo(xTex, yTex, color, ka, kd, ks);
  }

  return true;
}


Paralelepipedo::Paralelepipedo(Vector3DW& center, Vector3DW& xe, Vector3DW& ye, Vector3DW& ze, real dx, real dy, real dz, Material* material)
	  :Object3D(center, xe, ye, ze)
{
  dx_ = dx;
  dy_ = dy;
  dz_ = dz;
  material_ = material;
}

bool 
Paralelepipedo::rayIntersect(
	  Vector3DW& rOrigin, 
	  Vector3DW& rDirection, 
	  Vector3DW& intersectionPoint, 
	  Vector3DW& intersectionNormal, 
	  real& intersectionDistance, 
	  Color& color,
	  real& ka,
	  real& kd,
	  real& ks)
{
  Vector3DW rOrigin1 = T * (rOrigin - center_);
  Vector3DW rDirection1 = T * rDirection;

  real mint = (real)999999999;
  Vector3DW normal;
  real taux;
  real xCross;
  real yCross;
  real zCross;
  bool intersect = false;
  real xTex;
  real yTex;

  if(rDirection1.getX() != 0)
  {
	taux = (- rOrigin1.getX() + dx_) / rDirection1.getX();
	yCross = rOrigin1.getY() + taux*rDirection1.getY();
	zCross = rOrigin1.getZ() + taux*rDirection1.getZ();
	if(taux > 0)
	if(fabs(yCross) <= dy_)
	if(fabs(zCross) <= dz_)
	if(taux < mint)
	{
	  intersect = true;
	  mint = taux;
	  normal = xe_;
	  xTex = yCross / ((real)2*dy_) + (real)0.5;
	  yTex = zCross / ((real)2*dz_) + (real)0.5;
	}

	taux = (- rOrigin1.getX() - dx_) / rDirection1.getX();
	yCross = rOrigin1.getY() + taux*rDirection1.getY();
	zCross = rOrigin1.getZ() + taux*rDirection1.getZ();
	if(taux > 0)
	if(fabs(yCross) <= dy_)
	if(fabs(zCross) <= dz_)
	if(taux < mint)
	{
	  intersect = true;
	  mint = taux;
	  normal = xe_*(real)-1;
	  xTex = yCross / ((real)2*dy_) + (real)0.5;
	  yTex = zCross / ((real)2*dz_) + (real)0.5;
	}
  }

  if(rDirection1.getY() != 0)
  {
	taux = (- rOrigin1.getY() + dy_) / rDirection1.getY();
	xCross = rOrigin1.getX() + taux*rDirection1.getX();
	zCross = rOrigin1.getZ() + taux*rDirection1.getZ();
	if(taux > 0)
	if(fabs(xCross) <= dx_)
	if(fabs(zCross) <= dz_)
	if(taux < mint)
	{
	  intersect = true;
	  mint = taux;
	  normal = ye_;
	  xTex = xCross / ((real)2*dx_) + (real)0.5;
	  yTex = zCross / ((real)2*dz_) + (real)0.5;
	}

	taux = (- rOrigin1.getY() - dy_) / rDirection1.getY();
	xCross = rOrigin1.getX() + taux*rDirection1.getX();
	zCross = rOrigin1.getZ() + taux*rDirection1.getZ();
	if(taux > 0)
	if(fabs(xCross) <= dx_)
	if(fabs(zCross) <= dz_)
	if(taux < mint)
	{
	  intersect = true;
	  mint = taux;
	  normal = ye_*(real)-1;
	  xTex = xCross / ((real)2*dx_) + (real)0.5;
	  yTex = zCross / ((real)2*dz_) + (real)0.5;
	}
  }

  if(rDirection1.getZ() != 0)
  {
	taux = (- rOrigin1.getZ() + dz_) / rDirection1.getZ();
	xCross = rOrigin1.getX() + taux*rDirection1.getX();
	yCross = rOrigin1.getY() + taux*rDirection1.getY();
	if(taux > 0)
	if((real)fabs(xCross) <= dx_)
	if((real)fabs(yCross) <= dy_)
	if(taux < mint)
	{
	  intersect = true;
	  mint = taux;
	  normal = ze_;
	  xTex = xCross / ((real)2*dx_) + (real)0.5;
	  yTex = yCross / ((real)2*dy_) + (real)0.5;
	}

	taux = (- rOrigin1.getZ() - dz_) / rDirection1.getZ();
	xCross = rOrigin1.getX() + taux*rDirection1.getX();
	yCross = rOrigin1.getY() + taux*rDirection1.getY();
	if(taux > 0)
	if(fabs(xCross) <= dx_)
	if(fabs(yCross) <= dy_)
	if(taux < mint)
	{
	  intersect = true;
	  mint = taux;
	  normal = ze_*(real)-1;
	  xTex = xCross / ((real)2*dx_) + (real)0.5;
	  yTex = yCross / ((real)2*dy_) + (real)0.5;
	}
  }

  if(intersect)
  {
	intersectionPoint = rOrigin + rDirection*mint;
	intersectionNormal = normal;
	intersectionDistance = mint;

	if(material_ != NULL) 
	{
	  material_->getColorInfo(xTex, yTex, color, ka, kd, ks);
	}

	return true;
  }

  return intersect;
}


FunctionRevolution::FunctionRevolution(Vector3DW& center, Vector3DW& xe, Vector3DW& ye, Vector3DW& ze, Material* material)
	  :Object3D(center, xe, ye, ze)
{
  material_ = material;
  ti_ = 0; 
  tf_ = 1;
  maxRadius_ = 2;
  num_intervals_ = 7;
  tolerance_ = (real)0.001;

  generateMatrices();
}

bool 
FunctionRevolution::rayIntersect(
	  Vector3DW& rOrigin, 
	  Vector3DW& rDirection, 
	  Vector3DW& intersectionPoint, 
	  Vector3DW& intersectionNormal, 
	  real& intersectionDistance, 
	  Color& color,
	  real& ka,
	  real& kd,
	  real& ks)
{
  Vector3DW rOrigin1 = R * (T * rOrigin);
  Vector3DW rDirection1 = R * rDirection;
  if(!rayIntersectTransformed(rOrigin1, rDirection1, intersectionPoint, intersectionNormal, intersectionDistance, color, ka, kd, ks)) return false;
  intersectionPoint = T_1 * (R_1 * intersectionPoint);
  intersectionNormal = R_1 * intersectionNormal;
  return true;
}

bool 
FunctionRevolution::rayIntersectTransformed(
	  Vector3DW& rOrigin, 
	  Vector3DW& rDirection, 
	  Vector3DW& intersectionPoint, 
	  Vector3DW& intersectionNormal, 
	  real& intersectionDistance, 
	  Color& color,
	  real& ka,
	  real& kd,
	  real& ks)
{
  Vector3DW p1, p2;
  Vector3DW axisOrigin(0, 0, 0);
  Vector3DW axisDirection(0, 0, 1);
  if(!cylinderIntersection(axisOrigin, axisDirection, maxRadius_, rOrigin, rDirection, p1, p2)) 
	return false;

  real tmin, tmax, raux;
  tmin = (p1 - axisOrigin) * axisDirection;
  tmax = (p2 - axisOrigin) * axisDirection;
  if(tmin > tmax)
  {
	raux = tmin;
	tmin = tmax;
	tmax = raux;
  }
  if(tmin > tf_) return false;
  if(tmax < ti_) return false;
  if(tmin < ti_) tmin = ti_;
  if(tmax > tf_) tmax = tf_;

  Oz_ = rOrigin.getZ();
  drz_ = rDirection.getZ();

  if(drz_ == 0) return false;
  if(Oz_ > tf_)
  {
	if(drz_ > 0) return false;
  } else
  if(Oz_ < ti_)
  {
	if(drz_ < 0) return false;
  } else
  {
	if(drz_ > 0) tmin = Oz_;
	else tmax = Oz_;
  }

  real Ax, Ay, Bx, By, t_intersect;
  Ax = rDirection.getX() / drz_;
  Ay = rDirection.getY() / drz_;
  Bx = rOrigin.getX() - Oz_ * Ax;
  By = rOrigin.getY() - Oz_ * Ay;
  a_ = Ax*Ax + Ay*Ay;
  b_ = 2*(Ax*Bx + Ay*By);
  c_ = Bx*Bx + By*By;

  if(!find_t(tmin, tmax, t_intersect)) return false;

  real H = (t_intersect - Oz_) / drz_;

  intersectionPoint.setX(rOrigin.getX() + H * rDirection.getX());
  intersectionPoint.setY(rOrigin.getY() + H * rDirection.getY());
  intersectionPoint.setZ(t_intersect);

  intersectionNormal.setX(intersectionPoint.getX());
  intersectionNormal.setY(intersectionPoint.getY());
  intersectionNormal.setZ(0);
  intersectionNormal.normalize();
  intersectionNormal.setZ(-1 * dfdt(t_intersect));
  intersectionNormal.normalize();

  intersectionDistance = H;

  if(material_ != NULL)
  {
	material_->getColorInfo(0, 0, color, ka, kd, ks);
  }

  return true;
}

void 
FunctionRevolution::translate(real dx, real dy, real dz)
{
  Object3D::translate(dx, dy, dz);
  generateMatrices();
}

void 
FunctionRevolution::generateMatrices()
{
  T = Matrix4x4(1, 0, 0, -1*center_.getX(),
				0, 1, 0, -1*center_.getY(),
				0, 0, 1, -1*center_.getZ(),
				0, 0, 0, 1);

  T_1 = Matrix4x4(1, 0, 0, center_.getX(),
				  0, 1, 0, center_.getY(),
				  0, 0, 1, center_.getZ(),
				  0, 0, 0, 1);

  Matrix4x4 Ry;
  Matrix4x4 Ry_1;
  Matrix4x4 Rx;
  Matrix4x4 Rx_1;

  if(ze_.getZ() == 0)
  {
	real dxy = 1;
	if(ze_.getX() < 0) dxy = -1;

	Ry = Matrix4x4(0, 0, dxy, 0,
				   0, 1, 0, 0,
				   dxy, 0, 0, 0,
				   0, 0, 0, 1);

	Ry_1 = Matrix4x4(0, 0, dxy, 0,
					 0, 1, 0, 0,
					 dxy, 0, 0, 0,
					 0, 0, 0, 1);
  } else
  {
	real dxdz = ze_.getX() / ze_.getZ();
	real hy = (real)1.0 / ((real)sqrt((real)1.0 + dxdz*dxdz));

	Ry = Matrix4x4(hy, 0, -hy*dxdz, 0,
				   0, 1, 0, 0,
				   1*hy*dxdz, 0, hy, 0,
				   0, 0, 0, 1);

	Ry_1 = Matrix4x4(hy, 0, 1*hy*dxdz, 0,
					 0, 1, 0, 0,
					 -hy*dxdz, 0, hy, 0,
					 0, 0, 0, 1);
  }

  real dy = ze_.getY();
  real hy = (real)sqrt((real)1.0 - dy*dy);

  Rx = Matrix4x4(1, 0, 0, 0,
				 0, hy, -1*dy, 0,
				 0, dy, hy, 0,
				 0, 0, 0, 1);

  Rx_1 = Matrix4x4(1, 0, 0, 0,
				   0, hy, dy, 0,
				   0, -1*dy, hy, 0,
				   0, 0, 0, 1);

  R = Rx * Ry;
  R_1 = Ry_1 * Rx_1;

  Vector3DW teste = R * ze_;
  teste = R_1 * teste;
}

bool 
FunctionRevolution::find_t(real ti, real tf, real& t_intersection)
{
  real d2i, d2f, ft2i, ft2f, dt, H, minH, t_found, diff_found, t_aux;
  if((tf<0)||(ti<0)||(tf<ti)||(num_intervals_<1)) return false; 
  dt = (tf - ti) / (real)num_intervals_;
  minH = (real)999999999;
  bool found = false;
  t_aux = ti;
  d2i = a_*ti*ti + b_*ti + c_;
  ft2i = f(ti);	ft2i = ft2i*ft2i;
  for(int i=0; i<num_intervals_; i++)
  {
	t_aux += dt;
	d2f = a_*t_aux*t_aux + b_*t_aux + c_;
	ft2f = f(t_aux);	ft2f = ft2f*ft2f;
	if(((ft2i > d2i)&&(ft2f < d2f))||((ft2i < d2i)&&(ft2f > d2f)))
	{
	  H = (t_aux - Oz_) / drz_;
	  if((H>0)&&(H<minH))
	  {
		found = true;
		minH = H;
		t_found = t_aux;
		diff_found = (real)fabs(ft2f - d2f);
	  }
	}
	d2i = d2f;
	ft2i = ft2f;
  }

  if(found)
  {
	if(diff_found < tolerance_) 
	{
	  t_intersection = t_found;
	  return true;
	} else 
	{
	  return find_t(t_found-dt, t_found, t_intersection);
	}
  }

  return false;
}