#include "Camera.h"
#include "Math.h"


//
// This is cheap way to make the clip planes from the mvp without needing to
// do expensive matrix inversions and other maths. This quite directly does the job.
//
// References:
//      http://gamedevs.org/uploads/fast-extraction-viewing-frustum-planes-from-world-view-projection-matrix.pdf
//      http://www.lighthouse3d.com/tutorials/view-frustum-culling/clip-space-approach-implementation-details/
//
void extractClipPlanes(float clipPlanes[6][4], const float* m)
{
  // Left clipping plane
  clipPlanes[0][0] = m[4+1*4-5] + m[1+1*4-5];
  clipPlanes[0][1] = m[4+2*4-5] + m[1+2*4-5];
  clipPlanes[0][2] = m[4+3*4-5] + m[1+3*4-5];
  clipPlanes[0][3] = m[4+4*4-5] + m[1+4*4-5];
  // Right clipping plane
  clipPlanes[1][0] = m[4+1*4-5] - m[1+1*4-5];
  clipPlanes[1][1] = m[4+2*4-5] - m[1+2*4-5];
  clipPlanes[1][2] = m[4+3*4-5] - m[1+3*4-5];
  clipPlanes[1][3] = m[4+4*4-5] - m[1+4*4-5];
  // Top clipping plane
  clipPlanes[2][0] = m[4+1*4-5] - m[2+1*4-5];
  clipPlanes[2][1] = m[4+2*4-5] - m[2+2*4-5];
  clipPlanes[2][2] = m[4+3*4-5] - m[2+3*4-5];
  clipPlanes[2][3] = m[4+4*4-5] - m[2+4*4-5];
  // Bottom clipping plane
  clipPlanes[3][0] = m[4+1*4-5] + m[2+1*4-5];
  clipPlanes[3][1] = m[4+2*4-5] + m[2+2*4-5];
  clipPlanes[3][2] = m[4+3*4-5] + m[2+3*4-5];
  clipPlanes[3][3] = m[4+4*4-5] + m[2+4*4-5];
  // Near clipping plane
  clipPlanes[4][0] = m[4+1*4-5] + m[3+1*4-5];
  clipPlanes[4][1] = m[4+2*4-5] + m[3+2*4-5];
  clipPlanes[4][2] = m[4+3*4-5] + m[3+3*4-5];
  clipPlanes[4][3] = m[4+4*4-5] + m[3+4*4-5];
  // Far clipping plane
  clipPlanes[5][0] = m[4+1*4-5] - m[3+1*4-5];
  clipPlanes[5][1] = m[4+2*4-5] - m[3+2*4-5];
  clipPlanes[5][2] = m[4+3*4-5] - m[3+3*4-5];
  clipPlanes[5][3] = m[4+4*4-5] - m[3+4*4-5];
  // Normalize the plane equations, if required
  for (int i = 0; i < 6; i++)
    Math::normalizeVector(clipPlanes[i]);
/*
  fprintf(stderr, "clipPlanesA[] = {\n");
  for (int i = 0; i < 6; i++)
    fprintf(stderr, "   %f  %f  %f  %f\n", clipPlanes[i][0],  clipPlanes[i][1],  clipPlanes[i][2],  clipPlanes[i][3]);
  fprintf(stderr, "};\n");
  */
}


// m_camera.build(cameraStyle[0], a_aspect,  0.01f*cameraStyle[4],  );
 

 // Probably want to do a few things
 //   - Calc once the rotations/scale/translation for the camera
 //   - Same for the object
 //   - then can combine them
 //   - alternative is work out how to combine the rotation/translation etc of camera with those of the object and do at once, but might not be so easy
 //   - currently it is fairly simple because the camera is relatively fixed and the objects don't rotate normally, just translate which is done in shader 

void Camera::build(float fov, float aspect, float rotate[3], float scale, float translateX, float translateY, float translateZ, bool ortho)
{
  const float deg2rad = 0.017453292f; // (3.14 / 180.0);
  if (ortho)
    Math::makeOrthographicMatrix4x4(m_projectionMatrix, -2.0f, 2.0f, -2.0f, 2.0f, 0.0f, 10.0f); // 0.1f, 100.0f);
  else
    Math::makePerspectiveMatrix4x4(m_projectionMatrix, fov * deg2rad, aspect, 0.1f, 100.0f);
  Math::makeIdentityMatrix4x4(m_cameraMatrix);
  Math::translateMatrix4x4(m_cameraMatrix, 0.0f, 0.0f, 1.5f);
  Math::rotateMatrix4x4(m_cameraMatrix, rotate[0] * deg2rad, 1.0f, 0.0f, 0.0f);
  Math::rotateMatrix4x4(m_cameraMatrix, rotate[1] * deg2rad, 0.0f, 1.0f, 0.0f);
  Math::rotateMatrix4x4(m_cameraMatrix, rotate[2] * deg2rad, 0.0f, 0.0f, 1.0f);
  Math::scaleMatrix4x4(m_cameraMatrix, scale, scale, scale);
  Math::translateMatrix4x4(m_cameraMatrix, translateX, translateY, translateZ);
  float baseModelViewMatrix[16];
  Math::makeIdentityMatrix4x4(baseModelViewMatrix);
  Math::translateMatrix4x4(baseModelViewMatrix, 0.0f, 0.0f, -4.0f);
  float modelViewMatrix[16];
  Math::multiplyMatrix4x4(modelViewMatrix, baseModelViewMatrix, m_cameraMatrix);
  Math::multiplyMatrix4x4(m_modelViewProjectionMatrix, m_projectionMatrix, modelViewMatrix);
  extractClipPlanes(m_clipPlanes, m_modelViewProjectionMatrix);
}


void Camera::calcFrustum(float a_frustumPts[8][4])
{
  // Kind of expensive with the matrix inverse, so better if just use for debugging
  float inv[16];
  Math::inverseMatrix4x4(inv, m_modelViewProjectionMatrix);
  for (int i = 0; i < 8; i++)
  {
    float w[4] = { (i&1)?1.0f:-1.0f, (i&2)?1.0f:-1.0f, (i&4)?0.95f:-0.94f, 1.0f };
    for (int j = 0; j < 3; j++)
      w[j] *= 1.01f;
    Math::transformVector(a_frustumPts[i], inv, w);
    for (int j = 0; j < 3; j++)
      a_frustumPts[i][j] /= a_frustumPts[i][3];
  }
}


bool Camera::isClipped(float x, float y, float z, float w, float h, float d)
{
  if ( x <= w  ||  x >= (1000-w) )
  {
    return true;
  }

  // Test specific
  float maxDist = (w < h) ? h : ((w < d) ? d : w); // could pre-compute this once
  
  maxDist *= 1.3; // Hack to fix things getting clipped too early on the edges of the screen
  
  float pnt[4] = { x, y, z, 1.0 }; // potential optimizations to save partial results if it is only x that is changing, I guess the clip planes are changing too
  /*
  // Original un-optimized code
  for (int i = 0; i < 6; i++) {
    if (-maxDist > Math::dotProduct(pnt, m_clipPlanes[i], 1, 1, 4))
    {
      return true;
    }
  }
  */
  
  // Optimization attempt which inlines everything and tries to remove branching
  for (int i = 0; i < 6; i++)
  {
    float dist = pnt[0] * m_clipPlanes[i][0] + pnt[1] * m_clipPlanes[i][1] + pnt[2] * m_clipPlanes[i][2] + pnt[3] * m_clipPlanes[i][3];
    if (-maxDist > dist)
    {
      return true;
    }
  }


  return false;
}