#include <cmath>
#include "Maths.h"


struct vec4f
{
  union {
    float v[4];
    struct { float x, y, z, w; };
  };
};


struct mat4f
{
  union {
    vec4f m[4];
    struct { vec4f t, b, n, w; };  // tangent, bi-normal, normal
  };
};


vec4f addVector(const vec4f& a, const vec4f& b)
{
  vec4f res;
  res.x = a.x + b.x;
  res.y = a.y + b.y;
  res.z = a.z + b.z;
  res.w = a.w + b.w;
  return res;
}


vec4f subtractVector(const vec4f& a, const vec4f& b)
{
  vec4f res;
  res.x = a.x - b.x;
  res.y = a.y - b.y;
  res.z = a.z - b.z;
  res.w = a.w - b.w;
  return res;
}


void rotateVector(vec4f& vec, const mat4f& mat)
{
  vec4f res;
  Math::transformVector(&res.x, &mat.t.x, &vec.x);
  vec = res;
}


// An area - like a quad, but modelled as a 2D circular area floating in 3D space
struct Patch
{
  vec4f  normal; // <- instead a reference to some group it belongs to with shared TBN
  vec4f  position; // w of position is the radius
  float  intensity;
};


bool inline isNearZero(float val)
{
  const float epsilon = 0.0001f;
  return (val < epsilon && val > -epsilon);
}


vec4f orthogonalVector(const vec4f& normal)
{
  // A vector orthogonal to (a, b, c) is (-b, a, 0), or (-c, 0, a) or (0, -c, b)
  // derivation:
  //   plane through origin
  //     ax + by + cz = 0
  //   orthogonal vector is any point on this plane, eg satisfies the equation
  // so let x be 0
  // by = -cz
  // y = -c/b z
  // let y be c
  // c = -c/b z
  // c/ (-c/b) = z
  // z = -b
  // x = 0, y = c, z = -b
  vec4f v;
  if (!isNearZero(normal.z) || !isNearZero(normal.y)) {
    v.x = 0.0f; v.y = normal.z; v.z = -normal.y;
  } else if (!isNearZero(normal.x) || !isNearZero(normal.y)) {
    v.x = normal.y; v.y = -normal.x; v.z = 0.0f;
  } else if (!isNearZero(normal.x) || !isNearZero(normal.z)) {
    v.x = normal.z; v.y = 0.0f; v.z = -normal.x;
  //} else if (isNearZero(normal.x) && isNearZero(normal.z) && isNearZero(normal.y)) {
  } else {
    printf("vector is zero, so can't make orthogonal vector\n");
    //printf("last case\n");
    //v.x = normal.z; v.y = 0.0f; v.z = -normal.x;
  }
  v.w = 0.0f;
  return v;
}


// This info is same across a polygon which is broken in to patches
// Ideally it should be calculated per polygon and then each patch references this
mat4f patchAxisVectors(const Patch& patch)
{
  // Calc tangent and bi-normals for patch
  mat4f res;
  res.t = orthogonalVector(patch.normal); // orthogonal to normal
  Math::normalizeVector(&res.t.x);
  Math::crossProduct(&res.b.x, &patch.normal.x, &res.t.x);
  res.b.w = 0.0f;
  Math::normalizeVector(&res.b.x);
  res.n = patch.normal;
  res.n.w = 0.0f;
  res.w.x = 0.0f;
  res.w.y = 0.0f;
  res.w.z = 0.0f;
  res.w.w = 1.0f;
  return res;
}


// Calculates the influence of one patch on another patch
// represents the projection of the patch on to the hemi-sphere then projected down to the circle
// The calculated value is called the 'Nusselt analog'
// Alternative simplified calculation might be to do a dot-product between each normals and
// the vector between the patches and attenuate by the distance between them as an approximation
// doesn't take in to account obscurence by intervening patches - need some other logic to calculate
// possibly with use of BSP trees - probably a kind of ray-tracing from the patch out to test it
// reaches other patches
float nusseltFormFactor(const Patch& patch1, const Patch& patch2)
{
  // Could cache the tangents and bi-normals
  //mat4f p1 = scene.quads[patch1.quadIndex].basis;
  //mat4f p2 = scene.quads[patch2.quadIndex].basis;
  mat4f p1 = patchAxisVectors(patch1);
  mat4f p2 = patchAxisVectors(patch2);

  // get relative position of patch 2 to patch 1
  vec4f vecToPatch2 = subtractVector(patch2.position, patch1.position);

  // The corners in world space could be cached instead but at more storage cost
  vec4f corner1 = addVector(addVector(vecToPatch2, p2.t), p2.b);
  vec4f corner2 = addVector(subtractVector(vecToPatch2, p2.t), p2.b);
  vec4f corner3 = subtractVector(subtractVector(vecToPatch2, p2.t), p2.b);
  vec4f corner4 = subtractVector(addVector(vecToPatch2, p2.t), p2.b);

  // This is the projection on to a unit hemisphere
  Math::normalizeVector(&corner1.x);
  Math::normalizeVector(&corner2.x);
  Math::normalizeVector(&corner3.x);
  Math::normalizeVector(&corner4.x);

  // rotate in to coordinate space of patch 1
  rotateVector(corner1, p1);
  rotateVector(corner2, p1);
  rotateVector(corner3, p1);
  rotateVector(corner4, p1);

  // Now the x,y of the corners is their projection on to the circle
  vec4f v = subtractVector(corner1, corner2);
  vec4f w = subtractVector(corner3, corner2);
  float area = 0.5f * std::fabs(v.x * w.y - v.y * w.x);
  v = subtractVector(corner3, corner4);
  w = subtractVector(corner1, corner4);
  area += 0.5f * std::fabs(v.x * w.y - v.y * w.x);

  return area;
}


// Simplified form factor calculation - need to compare the results from this and test to see how good it is
// https://en.wikipedia.org/wiki/View_factor
float simpleFormFactor(const Patch& patch1, const Patch& patch2)
{
  // get direction of patch 1 to patch 2
  vec4f patch2patch = subtractVector(patch2.position, patch1.position);
  //printf("p2p: %f %f %f %f\n", patch2patch.x, patch2patch.y, patch2patch.z, patch2patch.w);
  float magSqr = Math::dotProduct(&patch2patch.x, &patch2patch.x, 1, 1, 3);
  float dot1 = patch1.normal.x * patch2patch.x + patch1.normal.y * patch2patch.y + patch1.normal.z * patch2patch.z;
  float dot2 = patch2.normal.x * patch2patch.x + patch2.normal.y * patch2patch.y + patch2.normal.z * patch2patch.z;
  //return (dot1 * dot2 * 11.5) / (M_PI * magSqr * magSqr); // the additional magSqr is a built-in normalization of patch2patch that is optimized away
  return (dot1 * dot2) / (M_PI * magSqr * magSqr); // the additional magSqr is a built-in normalization of patch2patch that is optimized away
}


void formFactorUnitTest()
{
  printf("testing form factors\n");

  Patch p1 = { {{{ 0.1f, 1.0f, 0.0f, 0.0f }}}, {{{ 0.0f, 0.0f, 0.0f, 0.0f }}} };
  Patch p2 = { {{{ 0.1f, 1.0f, 0.0f, 0.0f }}}, {{{ 20.0f, 20.0f, 20.0f, 0.0f }}} };
  Math::normalizeVector(&p1.normal.x);
  Math::normalizeVector(&p2.normal.x);

  for (int i = 0; i < 16; i++)
  {
    p2.position.x = 20.0f + i * 10.0f;
    float ff1 = nusseltFormFactor(p1, p2);
    float ff2 = simpleFormFactor(p1, p2);
    printf("x: %i  ff: %f  %f\n", i, ff1, ff2);
  }
  p2.position.x = 20.0f;
  for (int i = 0; i < 16; i++)
  {
    p2.position.y = 20.0f + i * 10.0f;
    float ff1 = nusseltFormFactor(p1, p2);
    float ff2 = simpleFormFactor(p1, p2);
    printf("x: %i  ff: %f  %f\n", i, ff1, ff2);
  }
  p2.position.y = 20.0f;
  for (int i = 0; i < 16; i++)
  {
    p2.position.z = 20.0f + i * 10.0f;
    float ff1 = nusseltFormFactor(p1, p2);
    float ff2 = simpleFormFactor(p1, p2);
    printf("x: %i  ff: %f  %f\n", i, ff1, ff2);
  }
}