#include "camera.h"
#include "macros.h"

template <class __type> inline
void normalize (Vector3<__type>& V) {
  __type len = 1.0/sqrt(V.x*V.x + V.y*V.y + V.z*V.z);
  V.x *= len;
  V.y *= len;
  V.z *= len;
}

void CameraTargetGL::Origin (GLfloat x, GLfloat y, GLfloat z) {
  origin(Vector3f(x, y, z));
}
void CameraTargetGL::Origin (const Vector3f& V) {
  origin(V);
}
void CameraTargetGL::Origin (Dynamic<Vector3f, GLfloat> *ptr, bool strong = false) {
  origin(ptr, strong);
}
void CameraTargetGL::Target (GLfloat x, GLfloat y, GLfloat z) {
  target(Vector3f(x, y, z));
}
void CameraTargetGL::Target (const Vector3f& V) {
  target(V);
}
void CameraTargetGL::Target (Dynamic<Vector3f, GLfloat> *ptr, bool strong = false) {
  target(ptr, strong);
}
void CameraTargetGL::Roll (GLfloat r) {
  roll(r);
}
void CameraTargetGL::Roll (Dynamic<GLfloat, GLfloat> *ptr, bool strong = false) {
  roll(ptr, strong);
}
Vector3f CameraTargetGL::Origin (double time = 0.0) {
  return origin.Val(time);
}
Vector3f CameraTargetGL::Target (double time = 0.0) {
  return target.Val(time);
}

void CameraTargetGL::GL (double time = 0.0)
{
  Vector3f U, V, N, A, B;
  GLfloat sr, cr, h, v;

  Vector3f O = origin.Val(time);
  Vector3f T = target.Val(time);
  N = T - O;

  U.x = N.z;
  U.y = 0;
  U.z = -N.x;
  V.x = -N.x*N.y;
  V.y = N.z*N.z + N.x*N.x;
  V.z = -N.y*N.z;
  normalize(N);
  normalize(U);
  normalize(V);
  sr = sin(roll.Val(time));
  cr = cos(roll.Val(time));
  A.x = cr*U.x + sr*V.x;
  A.y = cr*U.y + sr*V.y;
  A.z = cr*U.z + sr*V.z;
  B.x = -sr*U.x + cr*V.x;
  B.y = -sr*U.y + cr*V.y;
  B.z = -sr*U.z + cr*V.z;
  U = A;
  V = B;

  h = sin(horizontal_fov/2) / cos(horizontal_fov/2); // tan(hfov/2)
  v = sin(vertical_fov/2) / cos(vertical_fov/2); // tan(vfov/2)

  glMatrixMode(GL_PROJECTION);

  glLoadIdentity();
  glFrustum(-near_clipplane*h, near_clipplane*h,
            -near_clipplane*v, near_clipplane*v,
             near_clipplane, far_clipplane);

  glMatrixMode(GL_MODELVIEW);

  M[0] = U.x; M[4] = U.y; M[8] =  U.z; M[12] = -(O.x*U.x + O.y*U.y + O.z*U.z);
  M[1] = V.x; M[5] = V.y; M[9] =  V.z; M[13] = -(O.x*V.x + O.y*V.y + O.z*V.z);
  M[2] = -N.x; M[6] = -N.y; M[10] = -N.z; M[14] = (O.x*N.x + O.y*N.y + O.z*N.z);
  M[3] = 0.0; M[7] = 0.0; M[11] = 0.0; M[15] = 1.0;

  glLoadMatrixf(M);
}

void CameraTarget3DS::Origin (GLfloat x, GLfloat y, GLfloat z) {
  origin(Vector3f(x, y, z));
}
void CameraTarget3DS::Origin (const Vector3f& V) {
  origin(V);
}
void CameraTarget3DS::Origin (Dynamic<Vector3f, GLfloat> *ptr, bool strong = false) {
  origin(ptr, strong);
}
void CameraTarget3DS::Target (GLfloat x, GLfloat y, GLfloat z) {
  target(Vector3f(x, y, z));
}
void CameraTarget3DS::Target (const Vector3f& V) {
  target(V);
}
void CameraTarget3DS::Target (Dynamic<Vector3f, GLfloat> *ptr, bool strong = false) {
  target(ptr, strong);
}
void CameraTarget3DS::Roll (GLfloat r) {
  roll(r);
}
void CameraTarget3DS::Roll (Dynamic<GLfloat, GLfloat> *ptr, bool strong = false) {
  roll(ptr, strong);
}
Vector3f CameraTarget3DS::Origin (double time = 0.0) {
  return origin.Val(time);
}
Vector3f CameraTarget3DS::Target (double time = 0.0) {
  return target.Val(time);
}


void CameraTarget3DS::GL (double time = 0.0)
{
  Vector3f U, V, N, A, B;
  GLfloat sr, cr, h, v;

  Vector3f O = origin.Val(time);
  Vector3f T = target.Val(time);
  N = T - O;

  U.x = N.y;
  U.y = -N.x;
  U.z = 0;
  V.x = -N.x*N.z;
  V.y = -N.y*N.z;
  V.z = N.x*N.x+N.y*N.y;

  normalize(N);
  normalize(U);
  normalize(V);

  GLfloat __roll = -PI*(roll.Val(time))/180.0F;
  sr = sin(__roll);
  cr = cos(__roll);
  A.x = cr*U.x + sr*V.x;
  A.y = cr*U.y + sr*V.y;
  A.z = cr*U.z + sr*V.z;
  B.x = -sr*U.x + cr*V.x;
  B.y = -sr*U.y + cr*V.y;
  B.z = -sr*U.z + cr*V.z;
  U = A;
  V = B;

  h = sin(horizontal_fov/2) / cos(horizontal_fov/2); // tan(hfov/2)
  v = sin(vertical_fov/2) / cos(vertical_fov/2); // tan(vfov/2)

  glMatrixMode(GL_PROJECTION);

  glLoadIdentity();
  glFrustum(-near_clipplane*h, near_clipplane*h,
            -near_clipplane*v, near_clipplane*v,
             near_clipplane, far_clipplane);

  glMatrixMode(GL_MODELVIEW);

  M[0] = U.x; M[4] = U.y; M[8] =  U.z; M[12] = -(O.x*U.x + O.y*U.y + O.z*U.z);
  M[1] = V.x; M[5] = V.y; M[9] =  V.z; M[13] = -(O.x*V.x + O.y*V.y + O.z*V.z);
  M[2] = -N.x; M[6] = -N.y; M[10] = -N.z; M[14] = (O.x*N.x + O.y*N.y + O.z*N.z);
  M[3] = 0.0; M[7] = 0.0; M[11] = 0.0; M[15] = 1.0;

  glLoadMatrixf(M);
}

void CameraDescent3DS::rotate(Vector3f& U,GLfloat Nroll,GLfloat Vroll,GLfloat Uroll)
{
  GLfloat sp = sin(Nroll);
  GLfloat cp = cos(Nroll);

  GLfloat sf = sin(Vroll);
  GLfloat cf = cos(Vroll);

  GLfloat sg = sin(Uroll);
  GLfloat cg = cos(Uroll);

  GLfloat x,y,z;

  x=U.x;
  y=U.y;
  z=U.z;
  U.x=x;
  U.y=cp*y+sp*z;
  U.z=-sp*y+cp*z;

  x=U.x;
  y=U.y;
  z=U.z;
  U.x=cf*x-sf*z;
  U.y=y;
  U.z=sf*x+cf*z;

  x=U.x;
  y=U.y;
  z=U.z;
  U.x=cg*x+sg*y;
  U.y=-sg*x+cg*y;
  U.z=z;
}

void CameraDescent3DS::uprav(Vector3f& U,Vector3f& I,Vector3f& J,Vector3f& K)
{
  GLfloat x=U.x;
  GLfloat y=U.y;
  GLfloat z=U.z;

  U.x= x*I.x+ y*J.x+ z*K.x;
  U.y= x*I.y+ y*J.y+ z*K.y;
  U.z= x*I.z+ y*J.z+ z*K.z;
}

void CameraDescent3DS::GL(double time = 0.0)
{
  Vector3f U, V, N;
  GLfloat h, v;

  normalize(I);
  normalize(J);
  normalize(K);

  U.x=1;U.y=0;U.z=0;
  V.x=0;V.y=1;V.z=0;
  N.x=0;N.y=0;N.z=1;

  rotate(U,Uroll,Vroll,Nroll);
  rotate(V,Uroll,Vroll,Nroll);
  rotate(N,Uroll,Vroll,Nroll);

  uprav(U,I,J,K);
  uprav(V,I,J,K);
  uprav(N,I,J,K);

  I=U;
  J=V;
  K=N;

  h = sin(horizontal_fov/2) / cos(horizontal_fov/2); // tan(hfov/2)
  v = sin(vertical_fov/2) / cos(vertical_fov/2); // tan(vfov/2)

  glMatrixMode(GL_PROJECTION);

  glLoadIdentity();
  glFrustum(-near_clipplane*h, near_clipplane*h,
            -near_clipplane*v, near_clipplane*v,
             near_clipplane, far_clipplane);

  glMatrixMode(GL_MODELVIEW);

  M[0] = U.x; M[4] = U.y; M[8] =  U.z; M[12] = -(O.x*U.x + O.y*U.y + O.z*U.z);
  M[1] = V.x; M[5] = V.y; M[9] =  V.z; M[13] = -(O.x*V.x + O.y*V.y + O.z*V.z);
  M[2] = -N.x; M[6] = -N.y; M[10] = -N.z; M[14] = (O.x*N.x + O.y*N.y + O.z*N.z);
  M[3] = 0.0; M[7] = 0.0; M[11] = 0.0; M[15] = 1.0;
  glLoadMatrixf(M);
}

GLmatrix CameraDescent3DS::infoGL(double time = 0.0)
{
  Vector3f U, V, N;
  GLfloat h, v;

  normalize(I);
  normalize(J);
  normalize(K);

  U.x=1;U.y=0;U.z=0;
  V.x=0;V.y=1;V.z=0;
  N.x=0;N.y=0;N.z=1;

  rotate(U,Uroll,Vroll,Nroll);
  rotate(V,Uroll,Vroll,Nroll);
  rotate(N,Uroll,Vroll,Nroll);

  uprav(U,I,J,K);
  uprav(V,I,J,K);
  uprav(N,I,J,K);

  I=U;
  J=V;
  K=N;

  h = sin(horizontal_fov/2) / cos(horizontal_fov/2); // tan(hfov/2)
  v = sin(vertical_fov/2) / cos(vertical_fov/2); // tan(vfov/2)

  glMatrixMode(GL_PROJECTION);

  glLoadIdentity();
  glFrustum(-near_clipplane*h, near_clipplane*h,
            -near_clipplane*v, near_clipplane*v,
             near_clipplane, far_clipplane);

  glMatrixMode(GL_MODELVIEW);

  M[0] = U.x; M[4] = U.y; M[8] =  U.z; M[12] = -(O.x*U.x + O.y*U.y + O.z*U.z);
  M[1] = V.x; M[5] = V.y; M[9] =  V.z; M[13] = -(O.x*V.x + O.y*V.y + O.z*V.z);
  M[2] = -N.x; M[6] = -N.y; M[10] = -N.z; M[14] = (O.x*N.x + O.y*N.y + O.z*N.z);
  M[3] = 0.0; M[7] = 0.0; M[11] = 0.0; M[15] = 1.0;

  return M;
}



void CameraDescent3DS::GLmirror (double a, double b, double c, double d, double time = 0.0)
{
  Vector3f U, V, N;
  GLfloat h, v;

  normalize(I);
  normalize(J);
  normalize(K);

  U.x=1;U.y=0;U.z=0;
  V.x=0;V.y=1;V.z=0;
  N.x=0;N.y=0;N.z=1;

  rotate(U,Uroll,Vroll,Nroll);
  rotate(V,Uroll,Vroll,Nroll);
  rotate(N,Uroll,Vroll,Nroll);

  uprav(U,I,J,K);
  uprav(V,I,J,K);
  uprav(N,I,J,K);

  I=U;
  J=V;
  K=N;

  h = sin(horizontal_fov/2) / cos(horizontal_fov/2); // tan(hfov/2)
  v = sin(vertical_fov/2) / cos(vertical_fov/2); // tan(vfov/2)

  glMatrixMode(GL_PROJECTION);

  glLoadIdentity();
  glFrustum(-near_clipplane*h, near_clipplane*h,
            -near_clipplane*v, near_clipplane*v,
             near_clipplane, far_clipplane);

  glMatrixMode(GL_MODELVIEW);


  Vector3f nrm(a,b,c);
  float l;
  Vector3f OB=O;

  V=O+V;
  l = V.x*a + V.y*b + V.z*c + d;
  V-=2*l*nrm;

  U=O+U;
  l = U.x*a + U.y*b + U.z*c + d;
  U-=2*l*nrm;

  N=O+N;
  l = N.x*a + N.y*b + N.z*c + d;
  N-=2*l*nrm;

  l = O.x*a + O.y*b + O.z*c + d;
  O-=2*l*nrm;

  V=V-O;
  U=U-O;
  N=N-O;

  M[0] = U.x; M[4] = U.y; M[8] =  U.z; M[12] = -(O.x*U.x + O.y*U.y + O.z*U.z);
  M[1] = V.x; M[5] = V.y; M[9] =  V.z; M[13] = -(O.x*V.x + O.y*V.y + O.z*V.z);
  M[2] = -N.x; M[6] = -N.y; M[10] = -N.z; M[14] = (O.x*N.x + O.y*N.y + O.z*N.z);
  M[3] = 0.0; M[7] = 0.0; M[11] = 0.0; M[15] = 1.0;

  O=OB;

  glLoadMatrixf(M);
}