#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 operator +(const vec4f& a, const vec4f& b)
{
  vec4f res = {{{ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }}};
  return res;
}


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


inline float dot3(const vec4f& a, const vec4f& b)
{
  //return Math::dotProduct(&a.x, &b.x, 1, 1, 3);
  return a.x * b.x + a.y * b.y + a.z * b.z;
}


vec4f operator *(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 scaleVector(const vec4f& a, float b)
{
  vec4f res;
  res.x = a.x * b;
  res.y = a.y * b;
  res.z = a.z * b;
  res.w = a.w * b;
  return res;
}
vec4f operator *(const vec4f& a, float b)
{
  return scaleVector(a, b);
}


struct rotation
{
  const vec4f& q()      // get the quaternion
  {
    return quaternion;
  }
  const mat4f& m()      // get as a matrix
  {
    if (dirty)
    {
      convert();
    }
    return cachedMatrix;
  }
  void set(vec4f quat)  // set
  {
    quaternion = quat;
    dirty = true;
  }
  // TODO: perhaps operator overload the get and sets
private:
  void convert()
  {
    Math::quatToMatrix4x4(&cachedMatrix.m[0].x, &quaternion.x);
    dirty = false;
  }
  vec4f quaternion;
  bool dirty = true;
  mat4f cachedMatrix; // only needs to be 3x3
};


vec4f rotateVector(const vec4f& vec, const mat4f& mat)
{
  vec4f res;
  Math::transformVector(&res.x, &mat.t.x, &vec.x);
  return res;
}
vec4f operator *(const vec4f& a, const mat4f& mat)
{
  return rotateVector(a, mat);
}


float vectorLength(vec4f& vec)
{
  return sqrt(Math::dotProduct(&vec.x, &vec.x, 1, 1, 3));
}


struct Ray
{
  vec4f start, end;
  vec4f invN;
};


// Top 8 looks better than top 32 !!!
#define TOPX  64 // 32


// 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
                 // Could represent as 2 values and quantize to 8 bits and have a lookup table or something like that
  vec4f  position; // w of position is the radius
  int    quadIndex;

  float  w, h;

  int    u, v;

  vec4f  color;
  vec4f  newColor;

  bool   inShadow; // TODO: need a bit for each light that cast shadows
  float  directLightIntensity;

  float worstFF;
  float topFF[TOPX];
  int   topIdx[TOPX];
  //float  intensity;
  //float  newIntensity;
  float  totalFormFactors;
};


enum ShapeType
{
  Box
};


struct Shape
{
  ShapeType m_type;
  float x,y,z, w,h,d;  // width,height,depth
};


struct WindowParameters
{
  float w,h;
};


struct CameraParameters
{
  vec4f position;
  vec4f rotation;
};


struct LightParameters
{
  vec4f position;
  vec4f color;
  float power;
  float shininess;
  bool  castShadows;
};


struct RadiosityParameters
{
  int   topXToUse;
  float patchSize;
  float timesPerFrame;
  float a, b, c, d;
};


struct Vertex
{
  vec4f position;
  vec4f uv;
};


struct Quad
{
  // Vertex verts[4];  TODO: change to a vertex spec and array of them
  vec4f a, b, c, d;
  vec4f uvs[4];
  vec4f color;
  mat4f basis;
};


struct Texture
{
  uint32_t* bits;
  int w, h;
  uint32_t state;
  uint32_t texId;
};


template <typename T>
inline constexpr T constPow(const T base, const T exponent)
{
  return (exponent == 0) ? 1 : (base * constPow(base, exponent - 1));
}


// N-Dimentional tree (generalization of binary tree, quadtree, octree etc)
// Next generalization would be to make this support multiple trees in one (perhaps one root but masks of properties in the nodes)
// To make normals as maskable properties, would need to quantize the normals to perhaps 64 possible normals, so could use a 64bit mask
// Perhaps a Deltoidal hexecontahedron with 60 faces - each face is same shape and area - so regular without bias
// Or perhaps a dodecahedron with each hexagon face split in to triangles (pentakis dodecahedron) - makes 60 faces, 32 vertexes
template <int D>
class DTree
{
public:
  struct pos
  {
    float v[D];
  };

  class ObjectInterface
  {
  public:
    virtual pos position() = 0;
  };


  // Moved to public for query interface
  struct DTreePoint
  {
    int v[D];
  };

  // Moved to public for query interface
  struct DTreeBounds
  {
    DTreePoint origin;
    int size;
  };
private:

  // For a quadtree, D = 2, maxChildren = 4
  // For a octree,   D = 3, maxChildren = 8
  static const int maxChildren = constPow(2, D);

  class DTreeNode
  {
  public:
    DTreeNode* nodes[maxChildren];
    DTreeNode()
    {
      memset(nodes, 0, sizeof(nodes));
    }
    void AddItem(ObjectInterface* item)
    {
      items.push_back(item);
    }
    void RemoveItem(ObjectInterface* item)
    {
      items.erase(std::remove(items.begin(), items.end(), item), items.end());
    }
    size_t ItemCount()
    {
      return items.size();
    }
  //private:
    std::vector<ObjectInterface*> items;
  };

public:
  DTree(float _resolution = 1.0f)
  {
    resolution = _resolution;
  }

  ~DTree()
  {
    // TODO: clean-up
  }

  // probably needs to be a shared_ptr or may be a way to remove it from the DTree when it is destroyed
  void AddObject(ObjectInterface* item)
  {
    auto addFunctor = [](DTreeNode& node, ObjectInterface* item) { node.AddItem(item); };
    DTreePoint pnt = QuantizePoint(item->position());
    Expand(pnt);
    Traverse(addFunctor, *rootNode, bounds, pnt, item);
  }

  void RemoveObject(ObjectInterface* item) // need to quickly test the item belongs to the quadtree
  {
    // TODO: this doesn't recursively remove nodes as it traverses back up the tree
    //auto removeFunctor = [](DTreeNode& node, DTreeItemInterface* item) { node.RemoveItem(item); };
    //Traverse(removeFunctor, *rootNode, bounds, QuantizePoint(item->DTreeX(), item->DTreeY()), item);
    
    RemoveTraverse(*rootNode, bounds, QuantizePoint(item->position), item);
  }

  void MoveObject(ObjectInterface* item, pos pnt)
  {
    DTreePoint oldPnt = QuantizePoint(item->position());
    DTreePoint newPnt = QuantizePoint(pnt);
    MoveTraverse(*rootNode, bounds, oldPnt, newPnt, item);
  }

  static const int maxTreeDepth = 32;

  struct QueryContext
  {
    QueryContext(std::function<bool(const DTreeBounds&)> f) : func(f) {}

    // Functor to be called to decide if matches or not
    std::function<bool(const DTreeBounds&)> func;

    // Basically an explicit stack 
    int recurseDepth;

    /*
    struct Stack
    {
      int         state; // where to jump to
      DTreeNode*  node;
      DTreeBounds bounds;
      int         index;
      int         octant;
    };
    Stack stack[maxTreeDepth];
    */

    int         state[maxTreeDepth];
    DTreeNode*  nodeStack[maxTreeDepth];
    DTreeBounds bounds[maxTreeDepth];
    int         index[maxTreeDepth];
    int         octant[maxTreeDepth];
  };

  QueryContext InitializeQuery(std::function<bool(const DTreeBounds&)> func)
  {
    QueryContext context(func);
    context.recurseDepth = 0;
    QueryInitStackHelper(context, rootNode, bounds);
    return context;
  }

  void QueryInitStackHelper(QueryContext& context, DTreeNode* node, const DTreeBounds& b)
  {
    context.nodeStack[context.recurseDepth] = node;
    context.bounds[context.recurseDepth] = b;
    context.index[context.recurseDepth] = 0;
    context.octant[context.recurseDepth] = 0;
    context.state[context.recurseDepth] = 0;
  }

  ObjectInterface* QueryNextHelper(QueryContext& context, DTreeNode* node, const DTreeBounds& b)
  {
    context.recurseDepth++;
    QueryInitStackHelper(context, node, b);
    return QueryNext(context);
  }

#define CoroutineBegin    switch(state) { case 0:
#define CoroutineEmit(x)  do { state=__LINE__; return x; case __LINE__:; } while (0)
#define CoroutineEnd      }

  ObjectInterface* QueryNext(QueryContext& context) // Gets the next object, or returns nullptr if no more
  {
    DTreeNode* node = context.nodeStack[context.recurseDepth];
    DTreeBounds& b = context.bounds[context.recurseDepth];
    int& index = context.index[context.recurseDepth];
    int& q = context.octant[context.recurseDepth];
    int& state = context.state[context.recurseDepth];
    DTreeNode* childNode;
    DTreeBounds childBounds;

    // Can't use local variables from here down
    CoroutineBegin

    if (b.size == 1)
    {
      while (index < node->ItemCount())
      {
        index++;
        return node->items[index - 1];
      }
      context.recurseDepth--;
      return QueryNext(context); // Calling self, so don't need emit. What happens if use emit?
    }

    while (q < maxChildren)
    {
      childNode = node->nodes[q];
      if (childNode)
      {
        childBounds = ChildBounds(b, q);
        if (context.func(childBounds))
        {
          CoroutineEmit(QueryNextHelper(context, childNode, childBounds));
        }
      }
      q++;
    }

    if (context.recurseDepth)
    {
      context.recurseDepth--;
      return QueryNext(context); // Calling self, so don't need emit
    }

    CoroutineEnd

    return nullptr;          // Emit?
  }


  // Recursive but non-iterative and blocking implementation of the query
  std::vector<ObjectInterface*> QueryExecute(std::function<bool(const DTreeBounds&)> func)
  {
    std::vector<ObjectInterface*> ret;
    QueryExecuteRecurse(ret, func, rootNode, bounds);
    return ret;
  }

  // helper to the above function
  void QueryExecuteRecurse(std::vector<ObjectInterface*>& ret, std::function<bool(const DTreeBounds&)> func,
          DTreeNode* node, const DTreeBounds& b)
  {
    DTreeNode* childNode;
    DTreeBounds childBounds;

    if (b.size == 1)
    {
      int index = 0;
      while (index < node->ItemCount())
      {
        index++;
        ret.push_back(node->items[index - 1]);
      }
      return;
    }

    int q = 0;
    while (q < maxChildren)
    {
      childNode = node->nodes[q];
      if (childNode)
      {
        childBounds = ChildBounds(b, q);
        if (func(childBounds))
        {
          QueryExecuteRecurse(ret, func, childNode, childBounds);
        }
      }
      q++;
    }
  }

  // public so can debug
  DTreeBounds bounds;

private:
  /*
  enum class Direction : int 
  {
    Middle = 0,
    Left = 1<<0,  // 1   0001
    Right = 1<<1, // 2   0010
    Down = 1<<2,  // 4   0100
    Up = 1<<3,    // 8   1000
    UpLeft = Up | Left,
    UpRight = Up | Right,
    DownLeft = Down | Left,
    DownRight = Down | Right
  };
  */

  float resolution;
  DTreeNode* rootNode = nullptr;

  int QuantizeValue(float value)
  {
    return (int)std::floor((value + resolution * 0.5f) / resolution);
  }

  DTreePoint QuantizePoint(pos pnt)
  {
    DTreePoint p;
    for (int i = 0; i < D; i++)
    {
      p.v[i] = QuantizeValue(pnt.v[i]);
    }
    return p;
  }

  void Expand(const DTreePoint& pnt)
  {
    if (!rootNode)
    {
      rootNode = new DTreeNode;
      bounds.origin = pnt;
      bounds.size = 1;
      return;
    }
    ExpandRecurse(pnt);
  }

  static inline DTreeBounds ChildBounds(const DTreeBounds& b, int quadrant)
  {
    DTreeBounds childBounds = b;
    childBounds.size = b.size >> 1;

    // TODO: need to generalize to higher dimensions - need to resolve why x and y are inverted to each other to do this
    //childBounds.origin.x += (quadrant & 1) ? childBounds.size : 0;
    //childBounds.origin.y += (quadrant & 2) ? 0 : childBounds.size;
    
    for (int i = 0; i < D; i++)
    {
      childBounds.origin.v[i] += (quadrant & (1<<i)) ? childBounds.size : 0;
    }
    
    return childBounds;
  }
  
  static inline bool PointInBounds(const DTreePoint& pnt, const DTreeBounds& b)
  {
    //return (pnt.x >= b.origin.x && pnt.y >= b.origin.y && pnt.x < (b.origin.x + b.size) && pnt.y < (b.origin.y + b.size));

    // generalized to higher dimensions
    for (int i = 0; i < D; i++)
    {
      if ((pnt.v[i] < b.origin.v[i]) || (pnt.v[i] >= (b.origin.v[i] + b.size)))
        return false;
    }
    return true;
  }

  bool RemoveTraverse(DTreeNode& node, const DTreeBounds& b, const DTreePoint& pnt, ObjectInterface* item)
  {
    if (b.size == 1)
    {
      node.RemoveItem(item);
      return node.ItemCount() == 0;
    }

    for (int q = 0; q < maxChildren; q++)
    {
      DTreeBounds childBounds = ChildBounds(b, q);
      if (PointInBounds(pnt, childBounds))
      {
        //assert(node.nodes[q]);
        if (RemoveTraverse(*node.nodes[q], childBounds, pnt, item))
        {
          delete node.nodes[q];
          node.nodes[q] = 0;
        }
        break;
      }
    }
    
    //return !node.nodes[0] && !node.nodes[1] && !node.nodes[2] && !node.nodes[3];

    // generalized to higher dimensions:
    for (int i = 0; i < maxChildren; i++)
    {
      if (node.nodes[i])
        return false;
    }
    return true;
  }


  void MoveTraverse(DTreeNode& node, const DTreeBounds& b, const DTreePoint& oldPnt, const DTreePoint& newPnt, ObjectInterface* item)
  {
    //assert(b.size != 1)
    for (int q = 0; q < maxChildren; q++)
    {
      DTreeBounds childBounds = ChildBounds(b, q);
      bool oldIn = PointInBounds(oldPnt, childBounds);
      bool newIn = PointInBounds(newPnt, childBounds);
      if (oldIn && newIn)
      {
        //assert(node.nodes[q]);
        MoveTraverse(*node.nodes[q], childBounds, oldPnt, newPnt, item);
      }
      else if (oldIn)
      {
        //assert(node.nodes[q]);
        RemoveTraverse(*node.nodes[q], childBounds, oldPnt, item);
      }
      else if (newIn)
      {
        auto addFunctor = [](DTreeNode& node, ObjectInterface* item) { node.AddItem(item); };
        Traverse(addFunctor, *node.nodes[q], childBounds, newPnt, item);
      }
    }
  }

  template <typename F>
  void Traverse(const F& func, DTreeNode& node, const DTreeBounds& b, const DTreePoint& pnt, ObjectInterface* item)
  {
    if (b.size == 1)
    {
      func(node, item);
      return;
    }
    for (int q = 0; q < maxChildren; q++)
    {
      DTreeBounds childBounds = ChildBounds(b, q);
      if (PointInBounds(pnt, childBounds))
      {
        if (!node.nodes[q])
          node.nodes[q] = new DTreeNode;
        Traverse(func, *node.nodes[q], childBounds, pnt, item);
        return;
      }
    }

    /*
    // Generic traverse idea - callbacks for each part of the traversal
    auto actionFunctor = [func, item](DTreeNode& node, const DTreeBounds& bounds) { func(node, item); };
    auto terminateFunctor = [](const DTreeNode& node, const DTreeBounds& bounds) { return bounds.size == 1; };
    auto testFunctor = [pnt](const DTreeNode& node, const DTreeBounds& bounds) { return PointInBounds(pnt, bounds); };
    auto nullNodeFunctor = [](DTreeNode& node, int quadrant) { node.nodes[quadrant] = new DTreeNode; };
    TraverseGeneric(actionFunctor, terminateFunctor, testFunctor, nullNodeFunctor, node, b, true);
    */
  }

  template <typename ActionF, typename TerminateF, typename TestF, typename NullNode>
  void TraverseGeneric(const ActionF& action, const TerminateF& term, const TestF& test, const NullNode& nullNode, DTreeNode& node, const DTreeBounds& b, bool earlyOut)
  {
    if (term(node, b))
    {
      action(node, b);
      return;
    }
    for (int q = 0; q < maxChildren; q++)
    {
      DTreeBounds childBounds = ChildBounds(b, q);
      if (test(node, childBounds))
      {
        if (!node.nodes[q])
          nullNode(node, q);
        if (!node.nodes[q])
          TraverseGeneric(action, term, test, nullNode, *node.nodes[q], childBounds, earlyOut);
        if (earlyOut)
          return;
      }
    }
  }

  void ExpandRecurse(const DTreePoint& pnt)
  {
    // assert(rootNode);
    
    // First check if pnt is inside the bounds or not
    bool middle = true;
    for (int axis = 0; axis < D; axis++)
    {
      bool left = pnt.v[axis] < bounds.origin.v[axis];
      bool right = pnt.v[axis] >= (bounds.origin.v[axis] + bounds.size);
      if (left || right)
      {
        middle = false;
        break;
      }
    }
    if (middle)
      return;
  
    // Test: generalized to higher dimensions might be something like this:
    int octant = 0;
    for (int axis = 0; axis < D; axis++)
    {
      bool left = pnt.v[axis] < bounds.origin.v[axis];
      octant |= (left) ? (1<<D) : 0;
      bounds.origin.v[axis] -= (left) ? bounds.size : 0;
    }
    bounds.size <<= 1;
    DTreeNode* newRootNode = new DTreeNode;
    newRootNode->nodes[octant] = rootNode;
    rootNode = newRootNode;
    ExpandRecurse(pnt);

#if 0
    // TODO: generalize to higher dimensions
    int growDirectionFlags = 0; // Middle
    growDirectionFlags |= (pnt.x < bounds.origin.x)                  ? (1<<0) : 0;// ? Direction::Left : Direction::Middle;
    growDirectionFlags |= (pnt.x >= (bounds.origin.x + bounds.size)) ? (1<<1) : 0;// ? Direction::Right : Direction::Middle;
    growDirectionFlags |= (pnt.y < bounds.origin.y)                  ? (1<<2) : 0;// ? Direction::Down : Direction::Middle;
    growDirectionFlags |= (pnt.y >= (bounds.origin.y + bounds.size)) ? (1<<3) : 0;// ? Direction::Up : Direction::Middle;

    int oldRootQuadrant = 0;
    switch (growDirectionFlags) {
      case /* middle */     0:                return; // Nothing more to do, the point is already inside the current bounds
      case /* up */         (1<<3): 
      case /* up-left */    (1<<3) | (1<<0):  oldRootQuadrant = 3; break;
      case /* right */      (1<<1): 
      case /* up-right */   (1<<1) | (1<<3):  oldRootQuadrant = 2; break;
      case /* left */       (1<<0): 
      case /* down-left */  (1<<0) | (1<<2):  oldRootQuadrant = 1; break;
      case /* down */       (1<<2): 
      case /* down-right */ (1<<2) | (1<<1):  oldRootQuadrant = 0; break;
    }

    int q = oldRootQuadrant;
    bounds.origin.x -= (q & 1) ? bounds.size : 0;
    bounds.origin.y -= (q & 2) ? 0 : bounds.size;
    bounds.size <<= 1;
    DTreeNode* newRootNode = new DTreeNode;
    newRootNode->nodes[q] = rootNode;
    rootNode = newRootNode;
    ExpandRecurse(pnt);
#endif
  }
};



/*
struct PatchOctreeNode;
union PatchOctreeElement
{
  Patch*           patch;
  PatchOctreeNode* child;
};
struct PatchOctreeNode
{
  uint8_t            childMask;
  PatchOctreeElement children[8];
};
struct PatchOctree
{
  float            resolution;
  vec4f            boundsOrigin;
  vec4f            boundsSize;
  PatchOctreeNode* rootNode;
};
void InitOctee(PatchOctree& octree, float resolution)
{
  octree.rootNode = nullptr;
  octree.resolution = resolution;
}
void AddPatch(PatchOctree& octree, Patch& patch)
{
  if (octree.rootNode == nullptr)
  {
    octree.rootNode = new PatchOctreeNode;
    octree.rootNode.childMask = 1;
    memset(octree.rootNode.children, 0, sizeof(octree.rootNode.children));
    octree.rootNode.children[0].patch = &patch;
    octree.boundsOrigin = patch.position;
    octree.boundsSize.x = octree.resolution;
    octree.boundsSize.y = octree.resolution;
    octree.boundsSize.z = octree.resolution;
    return;
  }

}
*/

struct Scene
{
  WindowParameters window;
  RadiosityParameters settings;
  CameraParameters camera;
  LightParameters light;
  // PatchOctree patchOctree;
  DTree<3> patchOctree;
  Texture texture = { nullptr, -1, -1, 0, 0 }; // light-map
  std::vector<Shape> objects; // solid shapes
  std::vector<Quad> quads;    // large polygons of the surface of those shapes
  std::vector<Patch> patches; // small square areas over the polygons
};


/*
struct OctreeNode
{
  uint16_t   normalsPattern;  // quantized normal to 16-bits
  OctreeNode children[8];     // dynamic pointer based idea - more flexible and way you normally build it

//  uint16_t  firstChildIndex; // static octree idea - can be smaller if use blocks of data - can imply child block from table from current block
                               // smaller blocks means smaller indexes and easier to manage the memory - perhaps 256 nodes per block for a 8-bit index
};


struct OctreeBlock
{
  int          childOffsets[8]; // 
  OctreeBlock* childBlocks[8];  // Up to 8 child blocks - this is using pointers, again could use indexes
  OctreeNode   nodes[256];
};
*/


float simpleFormFactorInternal(const vec4f& p1, const vec4f& p2, const vec4f& n1, const vec4f& n2)
{
  vec4f patch1patch2 = p2 - p1;
  float magSqr = dot3(patch1patch2, patch1patch2);
  float dot1 =  dot3(n1, patch1patch2);
  float dot2 = -dot3(n2, patch1patch2); // negative because it should be dot with patch2patch1 which is just -patch1patch2
  if (dot2 < 0.0f || dot1 < 0.0f)
    return 0.0f;
  return (dot1 * dot2) / (M_PI * magSqr * magSqr); // the additional magSqr is a built-in normalization of patch2patch that is optimized away
}


// 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, const Scene& scn)
{
  // get direction of patch 1 to patch 2
  vec4f patch1patch2 = patch2.position - patch1.position;
  float magSqr = dot3(patch1patch2, patch1patch2);

  /*
  if (magSqr < 3.0f)
  {
    float total = 0.0f;
    const Quad& q1 = scn.quads[patch1.quadIndex];
    const Quad& q2 = scn.quads[patch2.quadIndex];

    total += simpleFormFactorInternal(patch1.position + q1.basis.t + q1.basis.b, patch2.position + q2.basis.t + q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position + q1.basis.t + q1.basis.b, patch2.position + q2.basis.t - q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position + q1.basis.t + q1.basis.b, patch2.position - q2.basis.t + q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position + q1.basis.t + q1.basis.b, patch2.position - q2.basis.t - q2.basis.b, patch1.normal, patch2.normal);

    total += simpleFormFactorInternal(patch1.position + q1.basis.t - q1.basis.b, patch2.position + q2.basis.t + q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position + q1.basis.t - q1.basis.b, patch2.position + q2.basis.t - q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position + q1.basis.t - q1.basis.b, patch2.position - q2.basis.t + q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position + q1.basis.t - q1.basis.b, patch2.position - q2.basis.t - q2.basis.b, patch1.normal, patch2.normal);
    
    total += simpleFormFactorInternal(patch1.position - q1.basis.t + q1.basis.b, patch2.position + q2.basis.t + q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position - q1.basis.t + q1.basis.b, patch2.position + q2.basis.t - q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position - q1.basis.t + q1.basis.b, patch2.position - q2.basis.t + q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position - q1.basis.t + q1.basis.b, patch2.position - q2.basis.t - q2.basis.b, patch1.normal, patch2.normal);

    total += simpleFormFactorInternal(patch1.position - q1.basis.t - q1.basis.b, patch2.position + q2.basis.t + q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position - q1.basis.t - q1.basis.b, patch2.position + q2.basis.t - q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position - q1.basis.t - q1.basis.b, patch2.position - q2.basis.t + q2.basis.b, patch1.normal, patch2.normal);
    total += simpleFormFactorInternal(patch1.position - q1.basis.t - q1.basis.b, patch2.position - q2.basis.t - q2.basis.b, patch1.normal, patch2.normal);
    
    return total / 16.0f;
  }
  */

  float dot1 =  dot3(patch1.normal, patch1patch2);
  float dot2 = -dot3(patch2.normal, patch1patch2); // negative because it should be dot with patch2patch1 which is just -patch1patch2
  if (dot2 < 0.0f || dot1 < 0.0f)
    return 0.0f;
  return (dot1 * dot2) / (M_PI * magSqr * magSqr); // the additional magSqr is a built-in normalization of patch2patch that is optimized away
}


void fillTextureArea(uint32_t* tex, int texWidth, int x, int y, int w, int h, uint32_t col)
{
  uint32_t* ptr = tex + y * texWidth + x;
  for (y = 0; y < h; y++)
  {
    uint32_t* p = ptr + y * texWidth;
    for (x = 0; x < w; x++)
    {
      p[x] = 0xFF000000 | col;
      /*
      if (x < 1 || x > (w-1) || y < 1 || y > (h-1))
        p[x] = 0xFF0000FF;
      else
      {
        uint32_t w2 = w;
        uint32_t h2 = h;
        uint32_t x2 = x/2 + w/2;
        uint32_t y2 = y/2 + h/2;
        uint8_t red = ((255U * h2 / y2));// << 16U);
        uint8_t green = ((255U * w2 / x2));// << 8U);
        p[x] = 0xFF000000UL | (red << 16) | (green << 8);
      }
      */
    }
  }
}


void createTexture(Scene& scn)
{
  /*
  float totalArea = 0.0f;
  for (auto o : scn.objects)
  {
    if (o.m_type == Box)
    {
      totalArea += o.w * o.h * 2.0f;
      totalArea += o.w * o.d * 2.0f;
      totalArea += o.d * o.h * 2.0f;
    }
  }
  float patchSize = scn.settings.patchSize;
  float patchCount = totalArea / (patchSize * patchSize);
  float minTextureDim = sqrt(patchCount);
  if (minTextureDim > 4096.0f)
  {
    scn.texture = { nullptr, -1, -1, 0, 0 };
    printf("Scene will not be able to fit inside a single texture\n");
    return;
  }
  */

  float totalHeight = 0.0f; 
  float totalWidth = 0.0f; 
  for (auto o : scn.objects)
  {
    if (o.m_type == Box)
    {
      float w = o.w + o.w + o.d;
      float h = std::max(o.h, o.d);
      totalWidth = std::max(totalWidth, w) + 12.0f; // Add 2 pixel border region around each quad
      totalHeight += h + 4.0f;  // Add 2 pixels on either side
    }
  }
  float maxDim = std::max(totalWidth, totalHeight);
  float patchSize = scn.settings.patchSize;
  maxDim /= patchSize;
  int dim = (int)maxDim;
  int nextPowerOfTwo = 2;
  while (nextPowerOfTwo < dim)
    nextPowerOfTwo <<= 1;
  dim = nextPowerOfTwo;
  printf("Lightmap texture dim: %i\n", dim);
  Texture texture = { (uint32_t*)malloc(dim*dim*sizeof(uint32_t)), dim, dim, 0, 0 };
  for (int i = 0; i < dim*dim; i++)
    texture.bits[i] = 0xFF0F0FFF;
  scn.texture = texture;
}


void convertObjectsToQuads(Scene& scn)
{
  int v = 0;
  fillTextureArea(scn.texture.bits, scn.texture.w, 0, 0, scn.texture.w, scn.texture.h, 0);
  for (auto o : scn.objects)
  {
    if (o.m_type == Box)
    {
      // Populate the texture
      float w = o.w;
      float h = o.h;
      float d = o.d;
      float patchSize = scn.settings.patchSize;
      w /= patchSize;
      h /= patchSize;
      d /= patchSize;
      w += 1.0f;
      h += 1.0f;
      d += 1.0f;
      // TODO: trying to make them in to int positions, but need to have the fractional part for the
      // edge cases. When split in to patches, most are exactly patchSize x patchSize, but on the edges
      // it will be for example 0.3 . patchSize x 0.45 . patchSize  so the u2 and v2 for example will
      // not be whole numbers. Currently it is just rounding up to the next whole number, but this
      // will stretch the texture.
      // Need to also work out how the GPU samples the texture, might need a 0.5 bias to the u,v
      // coordinates so that is doesn't sample and blend from pixels off the edges of where we thing
      float u1 = 2.0f;
      float u2 = u1 + w + 4.0f;
      float u3 = u2 + w + 4.0f;
      float u4 = u3 + d + 2.0f;
      float v1 = v + 2.0f;
      float v2 = v1 + 2.0f + h;
      float v3 = v1 + 2.0f + d;
      float v4 = v1 + 2.0f + h;
      float tw = scn.texture.w;
      float th = scn.texture.h;
/*
      fillTextureArea(scn.texture.bits, scn.texture.w, u1, v, w, h, 0xFFFF0000);
      fillTextureArea(scn.texture.bits, scn.texture.w, u2, v, w, d, 0xFF00FF00);
      fillTextureArea(scn.texture.bits, scn.texture.w, u3, v, d, h, 0xFF0000FF);
      
      fillTextureArea(scn.texture.bits, scn.texture.w, u4+u1, v, w, h, 0xFFFF0000);
      fillTextureArea(scn.texture.bits, scn.texture.w, u4+u2, v, w, d, 0xFF00FF00);
      fillTextureArea(scn.texture.bits, scn.texture.w, u4+u3, v, d, h, 0xFF0000FF);
*/      
      float uvs[6][4][2] = {
        { { u1, v1 }, { u1, v2 }, { u2, v2 }, { u2, v1 } },  // 0 -> front  0
        { { u2, v1 }, { u2, v3 }, { u3, v3 }, { u3, v1 } },  // 1 -> bottom 1
        { { u3, v1 }, { u3, v4 }, { u4, v4 }, { u4, v1 } },  // 2 -> side   2
        
        { { u4+u3, v1 }, { u4+u3, v4 }, { u4+u4, v4 }, { u4+u4, v1 } },  // 2 -> side   3
        { { u4+u1, v1 }, { u4+u1, v2 }, { u4+u2, v2 }, { u4+u2, v1 } },  // 0 -> back   4
        { { u4+u2, v1 }, { u4+u2, v3 }, { u4+u3, v3 }, { u4+u3, v1 } },  // 1 -> top    5

        /*
        { { 0, 0 }, { 0, th }, { tw, th }, { tw, 0 } },
        { { 0, 0 }, { 0, th }, { tw, th }, { tw, 0 } },
        { { 0, 0 }, { 0, th }, { tw, th }, { tw, 0 } },
        { { 0, 0 }, { 0, th }, { tw, th }, { tw, 0 } },
        { { 0, 0 }, { 0, th }, { tw, th }, { tw, 0 } },
        */
      };
      v += std::max(h, d);
      v += 2.0f;

      vec4f verts[8];   // boxes have 8 verts
      for (int i = 0; i < 8; i++)
      {
        float x = o.x + ((i&1) ? o.w : 0.0f);
        float y = o.y + ((i&2) ? o.h : 0.0f);
        float z = o.z + ((i&4) ? o.d : 0.0f);
        verts[i] = {{{ x, y, z, 0.0f }}};  // 8 is 2^3 - 1 bit for each axis, bit 0 => x, bit 1 => y, bit 2 => z
      }

      // boxes have 6 faces, each face with 4 verts (one of the 8 verts above). A LUT of vert indexes for each face (with CW winding order)
      int vertIndexes[6][4] = { { 0, 2, 3, 1 }, { 2, 6, 7, 3 }, { 1, 3, 7, 5 }, { 0, 4, 6, 2 }, { 4, 5, 7, 6 }, { 0, 1, 5, 4 } };
      // 0, 4    1, 5     2,  3
      // G  R    B  R     G   B
      vec4f colors[6] = { {{{ 0.0f, 1.0f, 0.0f, 0.0f }}}, {{{ 0.0f, 0.0f, 1.0f, 0.0f }}}, {{{ 0.0f, 1.0f, 0.0f, 0.0f }}},
                          {{{ 0.0f, 0.0f, 1.0f, 0.0f }}}, {{{ 1.0f, 0.0f, 0.0f, 0.0f }}}, {{{ 1.0f, 0.0f, 0.0f, 0.0f }}} };
      for (int quad = 0; quad < 6; quad++)
      {
        Quad q;
        q.a = verts[vertIndexes[quad][0]];
        q.b = verts[vertIndexes[quad][1]];
        q.c = verts[vertIndexes[quad][2]];
        q.d = verts[vertIndexes[quad][3]];
        q.color = colors[quad];

        for (int i = 0; i < 4; i++)
          q.uvs[i] = {{{ uvs[quad][i][0]/float(tw), uvs[quad][i][1]/float(th), 0.0f, 0.0f }}};

        // Calculate basis vectors
        q.basis.t = q.b - q.a; // B->A
        q.basis.b = q.b - q.c; // B->C
        Math::normalizeVector(&q.basis.t.x);
        Math::normalizeVector(&q.basis.b.x);
        Math::crossProduct(&q.basis.n.x, &q.basis.t.x, &q.basis.b.x);
        q.basis.n = scaleVector(q.basis.n, -1.0f);
        Math::normalizeVector(&q.basis.n.x);
        q.basis.w = {{{ 0.0f, 0.0f, 0.0f, 1.0f }}};

        scn.quads.push_back(q);
      }
    }
  }
}


class PatchObject : public DTree<3>::ObjectInterface
{
public:
  PatchObject(Patch& ptch, size_t i)
    : idx(i)
    , m_patch(ptch)
  {
    for (int i = 0; i < 3; i++)
      p.v[i] = ptch.position.v[i];
  }
  DTree<3>::pos position() override
  {
    return p;
  }
  const Patch& patch() const
  {
    return m_patch;
  }
  size_t idx;
private:
  Patch& m_patch;
  DTree<3>::pos p;
};


void addPatchToScene(Scene& scn, Patch& p)
{
  size_t idx = scn.patches.size();
  PatchObject *obj = new PatchObject(p, idx);
  scn.patchOctree.AddObject(obj);
  scn.patches.push_back(p);
}


void convertQuadsToPatches(Scene& scn)
{
  float patchSize = scn.settings.patchSize;
  for (int qi = 0; qi < scn.quads.size(); qi++)
  {
    Quad& q = scn.quads[qi];

    float u1,u2,v1,v2;
    u1 = q.uvs[0].x * scn.texture.w;
    u2 = q.uvs[2].x * scn.texture.w;
    v1 = q.uvs[0].y * scn.texture.h;
    v2 = q.uvs[2].y * scn.texture.h;

    vec4f ba = q.b - q.a; // B->A
    vec4f bc = q.b - q.c; // B->C
    float len1 = vectorLength(bc);
    float len2 = vectorLength(ba);
    int jc = int((len1 /*+ patchSize*/) / patchSize);
    int ic = int((len2 /*+ patchSize*/) / patchSize);
    //for (float j = 0.0f; j <= len1; j+=patchSize)
    //for (float i = 0.0f; i <= len2; i+=patchSize)
    for (int ji = 0; ji <= jc; ji++)
    for (int ii = 0; ii <= ic; ii++)
    {
      float j = ji * patchSize;
      float i = ii * patchSize;
      Patch p;
      p.quadIndex = qi;
      p.color = q.color;
      p.normal = q.basis.n; // could avoid storing in the patch
      p.h = ((j + patchSize) <= len1) ? patchSize : (len1 - j);
      p.w = ((i + patchSize) <= len2) ? patchSize : (len2 - i);

      //p.u = int(u2 - ((u2-u1) * (j - 0.5*patchSize) / (len1 + patchSize)));
      //p.v = int(v1 + ((v2-v1) * (i + 0.5*patchSize) / (len2 + patchSize)));
      
      //p.u = int(u2 - ((u2-u1+2) * (j) / (len1 + patchSize)) + 0.0);//*patchSize);
      //p.v = int(v1 + ((v2-v1+2) * (i) / (len2 + patchSize)) + 0.0);//*patchSize);
      
      //p.u = int(u1 + (len1 - patchSize - j) / patchSize);
      //p.v = int(v1 + i / patchSize);
      
      // int ji2 = (ji == jc) ? jc-1 : ji;
      p.u = int(u1) + (jc - ji);//+ jc - (ji2 + 1);
      p.v = int(v1) + ii;
      
      //p.v = int(v1 + i / patchSize);
      //p.v = int(v1) + int(ii * float(ic+0.1f) / float(ic));
      
      //p.u = int(u2 - ((u2-u1+2) * (j) / (len1 + patchSize)) + 0.0);//*patchSize);
      //p.v = int(v1 + ((v2-v1+2) * (i) / (len2 + patchSize)) + 0.0);//*patchSize);
      
      // TODO: figure out why negative
      // b and t are facing wrong way so using q.d instead of q.b
      p.position = q.d + scaleVector(q.basis.t, i + (p.w*0.5f)) + scaleVector(q.basis.b, j + (p.h*0.5f));
      addPatchToScene(scn, p);
    }
  }
}

      
bool intersects(const Ray& r, const Shape& o)
{
  // ray:    start, end
  // shape:  x,y,z, w,h,d;
  //return false;
  float bMinX = o.x;
  float bMaxX = o.x + o.w;
  float bMinY = o.y;
  float bMaxY = o.y + o.h;
  float bMinZ = o.z;
  float bMaxZ = o.z + o.d;

  vec4f r0 = r.start;


  if (r.start.x < bMinX && r.end.x < bMinX)
    return false;
  if (r.start.y < bMinY && r.end.y < bMinY)
    return false;
  if (r.start.z < bMinZ && r.end.z < bMinZ)
    return false;
  if (r.start.x > bMaxX && r.end.x > bMaxX)
    return false;
  if (r.start.y > bMaxY && r.end.y > bMaxY)
    return false;
  if (r.start.z > bMaxZ && r.end.z > bMaxZ)
    return false;


  float d1 = (bMinX - r0.x)*r.invN.x;
  float d2 = (bMaxX - r0.x)*r.invN.x;
/*
  r0.x = bMinX - d1/r.invN.x

  if (d1 <= 0.0f)
    return false;
*/
  float tmin = std::min(d1, d2);
  float tmax = std::max(d1, d2);

  float ty1 = (bMinY - r0.y)*r.invN.y;
  float ty2 = (bMaxY - r0.y)*r.invN.y;
  //if (ty1 <= 0.0f)
    //return false;

  tmin = std::max(tmin, std::min(ty1, ty2));
  tmax = std::min(tmax, std::max(ty1, ty2));

  float tz1 = (bMinZ - r0.z)*r.invN.z;
  float tz2 = (bMaxZ - r0.z)*r.invN.z;

  tmin = std::max(tmin, std::min(tz1, tz2));
  tmax = std::min(tmax, std::max(tz1, tz2));
  if (tmax <= 0.0f)
    return false;

  return (tmax >= tmin); // && (tmax > 0.0f);
}


void processPatchLight(Patch& p, const LightParameters& light, const Scene& scn)
{
  vec4f l = light.position;
  bool shadowTest = light.castShadows;
  p.inShadow = false;
  if (shadowTest)
  {
    Ray r = { p.position, l };
    vec4f rayN = r.end - r.start;
    Math::normalizeVector(&rayN.x);
    r.start = r.start + (rayN * 0.00001f);
    vec4f invN = {{{ 1.0f/rayN.x, 1.0f/rayN.y, 1.0f/rayN.z, 0.0f }}};
    r.invN = invN;

    // TODO: can do this same thing for figuring out the TOPX FFs are directly connected
    // TODO: make TOPX a runtime value - perhaps could progressively increase over time
    //       until convergence - need a frame-to-frame measure of convergence and a quality
    //       setting on when to stop
    for (auto&o : scn.objects)
    {
      if (intersects(r, o))
      {
        p.inShadow = true;
        break;
      }
    }
  }

  if (!p.inShadow)
  {
    vec4f& n = p.normal;
    vec4f toL = l - p.position;
    Math::normalizeVector(&toL.x);
    float intensity = Math::dotProduct(&toL.x, &n.x);
    p.directLightIntensity += (0.7f * intensity);
  }
}


void initializePatches(Scene& scn)
{
  for (auto& p : scn.patches)
  {
    for (int i = 0; i < TOPX; i++)
    {
      p.worstFF = 0.0f;
      p.topFF[i] = 0.0f;
      p.topIdx[i] = -1;
    }
  }
}


void initializePatchLighting(Scene& scn)
{
  for (auto& p : scn.patches)
  {
    p.directLightIntensity = 0.3f;
    
    // TODO: iterate list of lights
    processPatchLight(p, scn.light, scn);

    // TODO: idea is we need a shadow bit to tell if we apply spec or not
    //       spec of a given light shouldn't happen where that light is in shadow
    //       could use the alpha channel and bits of it for this shadow bit for
    //       a number of shadow casting lights
    p.color = p.color * p.directLightIntensity;
  }
}


float nusseltFormFactor(const Patch& patch1, const Patch& patch2, Scene& scene)
{
  // Could cache the tangents and bi-normals
  mat4f p1 = scene.quads[patch1.quadIndex].basis;
  mat4f p2 = scene.quads[patch2.quadIndex].basis;

  // get relative position of patch 2 to patch 1
  vec4f vecToPatch2 = patch2.position - patch1.position;

  // The corners in world space could be cached instead but at more storage cost
  vec4f corner1 = vecToPatch2 + p2.t + p2.b;
  vec4f corner2 = vecToPatch2 - p2.t + p2.b;
  vec4f corner3 = vecToPatch2 - p2.t - p2.b;
  vec4f corner4 = 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
  corner1 = rotateVector(corner1, p1);
  corner2 = rotateVector(corner2, p1);
  corner3 = rotateVector(corner3, p1);
  corner4 = rotateVector(corner4, p1);

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

  return area;
}


struct ConeShape
{
   vec4f  apex;
   vec4f  normal;   // direction from apex out along the axis of the solid part of the cone
   float  angle;    // angle in radians of the direction from the normal to the outside of cone
   float  length;   // for a closed cone, the length from apex along the normal until reach the base of the cone
   // only need 3 float values for the apex and normal, so could put the angle and length in the w components of those
};


void CreateConeFromPatch(ConeShape& cone, const Patch& p)
{
  cone.apex = p.position;
  cone.normal = p.normal;
  cone.angle = Math::degreesToRadians(90.0);
  cone.length = 0.5f; // Should be some kind of tweakable value
}


void calculateFormFactorScales(Scene& scn)
{
  printf("calculating form factors\n");
  printf("please wait...\n");
  for (auto& p1 : scn.patches)
  {
    p1.totalFormFactors = 0.001f;

    // TODO: This is where we can use the octree to select a cone radiating from the patch p1 so
    // we can reduce the number of patches we test it against (instead of iterating all other patches)

    // Remember the idea to make the octree be a multi-tree of normals


    auto f = [](const DTree<3>::DTreeBounds& b)
    {
      // TODO: cone vs box intersection test
      return true;
    };
    auto ctx = scn.patchOctree.InitializeQuery(f);

    // This is alternative code to iterate via the octree, but seems to not be working - probably not returning all
    PatchObject* obj = nullptr;
    while ((obj = (PatchObject*)scn.patchOctree.QueryNext(ctx)) != nullptr)
    {
      int idx = obj->idx;

/*
    // //for (auto& p2 : scn.patches)
    for (int idx = 0; idx < scn.patches.size(); idx++)
    {
*/

      auto& p2 = scn.patches[idx];
      if (&p1 != &p2)
      {
        float ff = simpleFormFactor(p1, p2, scn);
        p1.totalFormFactors += ff;

        // Only add to the topX if it is better than the worst one we have
        if (ff > p1.worstFF)
        {
          // We aren't taking in to account shadows, are we?
          for (int i = 0; i < TOPX; i++)
          {
            if (p1.topIdx[i] == -1 || p1.topFF[i] < ff)
            {
              p1.topIdx[i] = idx;
              p1.topFF[i] = ff;
              break;
            }
          }

          // Update worstFF
          p1.worstFF = ff;
          for (int i = 0; i < TOPX; i++)
          {
            if (p1.topFF[i] < p1.worstFF)
            {
              p1.worstFF = p1.topFF[i];
            }
          }
        }


      }
    }
  }
  printf("done\n");
}


void calculateFormFactors(Scene& scn)
{
  //printf("calculating form factors\n");
  //printf("please wait...\n");
 
  for (auto& p1 : scn.patches)
    p1.newColor = p1.color;

  int topX = std::max(scn.settings.topXToUse, TOPX);

  // Collect pass
  for (auto& p1 : scn.patches)
  {
#if 1
    for (int i = 0; i < topX; i++)
    {
      if (p1.topIdx[i] != -1)
      {
        auto& p2 = scn.patches[p1.topIdx[i]];
        float ff = p1.topFF[i];
#else
    for (auto& p2 : scn.patches)
    {
      if (&p1 != &p2)
      {
        float ff = simpleFormFactor(p1, p2);
#endif
        //float a1 = p1.w * p1.h;
        //float a2 = p2.w * p2.h;
        float ff1 = 1.0f * ff;// / p1.totalFormFactors;
        float ff2 = 1.0f * ff;// / p2.totalFormFactors;

        /*
        //float ff = nusseltFormFactor(p1, p2, scn);
        const float maxTransfer = 2.0f;//0.05f;
        if ( ff1 < 0.0f )
          ff1 = 0.0f;
        if ( ff1 > maxTransfer )
          ff1 = maxTransfer;
        if ( ff2 < 0.0f )
          ff2 = 0.0f;
        if ( ff2 > maxTransfer )
          ff2 = maxTransfer;
          */

        /*
        float p1top2 = p1.intensity * ff;// / p1.totalFormFactors;
        float p2top1 = p2.intensity * ff;// / p2.totalFormFactors;
        p1.newIntensity += p2top1;
        p2.newIntensity -= p2top1;
        p1.newIntensity -= p1top2;
        p2.newIntensity += p1top2;
        */
        vec4f p1top2 = p1.color * ff1;
        vec4f p2top1 = p2.color * ff2;
        p1.newColor = p1.newColor + p2top1 - p1top2;
        p2.newColor = p2.newColor - p2top1 + p1top2;
      }
    }
  }

  for (auto& p : scn.patches)
  {
    float i = p.directLightIntensity;
    vec4f newColor = scn.quads[p.quadIndex].color * i * 0.05f;
    p.color = p.newColor * 0.95f + newColor;
    //p.intensity = std::min(1.0f, p.newIntensity * 0.95f + i * 0.05f);
    
    uint32_t col = 0xFF000000;
    float r = std::max(std::min(p.color.x, 1.0f), 0.0f);
    float g = std::max(std::min(p.color.y, 1.0f), 0.0f);
    float b = std::max(std::min(p.color.z, 1.0f), 0.0f);
    col |= int(r * 255.0f) << 16;
    col |= int(g * 255.0f) <<  8;
    col |= int(b * 255.0f) <<  0;
    scn.texture.bits[p.v * scn.texture.w + p.u] = col;
  }

  /*
  // Transmit pass
  for (auto& p1 : scn.patches)
  {
    for (auto& p2 : scn.patches)
    {
      if (&p1 != &p2)
      {
        float ff = -simpleFormFactor(p1, p2);
        float p1top2 = 0.1f * p1.intensity * ff / p1.totalFormFactors;
        //float p2top1 = p2.intensity * ff;// / p2.totalFormFactors;
        //p1.intensity += p2top1;
        //p2.intensity -= p2top1;
        p1.intensity -= p1top2;
        p2.intensity += p1top2;
      }
    }
  }
  */

  //printf("done\n");
}


Scene parseFile(const char* fileName)
{
  Scene scn;
  FILE* file = fopen(fileName, "rt");
  if (file)
  {
    char str[128];
    int converted = 0;
    float a, b, c, d, e, f, g, h;
    char v[128];
    while ((converted = fscanf(file, "%s %f, %f, %f, %f, %f, %f, %f, %f, %s\n", str, &a, &b, &c, &d, &e, &f, &g, &h, v)) >= 1)
    {
      if (strcmp(str, "Window,") == 0 && converted == 3)
      {
        scn.window = { a, b };
      }
      else if (strcmp(str, "Settings,") == 0 && converted == 8)
      {
        scn.settings = { int(a), b, c, d, e, f, g };
      }
      else if (strcmp(str, "Camera,") == 0 && converted == 7)
      {
        scn.camera = { {{{ a, b, c, 0.0f }}}, {{{ d, e, f, 0.0f }}} };
      }
      else if (strcmp(str, "Light,") == 0 && converted == 10)
      {
        bool castShadows = (strcmp(v, "true") == 0) ? true : false;
        scn.light = { {{{ a, b, c, 0.0f }}}, {{{ d, e, f, 0.0f }}}, g, h, castShadows };
      }
      else if (strcmp(str, "Box,") == 0 && converted == 7)
      {
        Shape obj = { Box, a, b, c, d, e, f };
        scn.objects.push_back(obj);
      }
      else
      {
        printf("Unknown type: %s\n", str);
      }
    }
  } else {
    printf("file not found\n");
  }
  fclose(file);
  return scn;
}


void dumpSceneInfo(Scene& scn)
{
  printf("scn   objs: %lu   quads: %lu   patchs: %lu\n", scn.objects.size(), scn.quads.size(), scn.patches.size());

  for (auto q : scn.quads)
  {
    printf("quad:   %04f %04f %04f   %04f %04f %04f   %04f %04f %04f   %04f %04f %04f\n",
        q.a.x, q.a.y, q.a.z,   q.b.x, q.b.y, q.b.z,   q.c.x, q.c.y, q.c.z,   q.d.x, q.d.y, q.d.z);
  }

  // 200,000 patches
  // 200,000 x 200,000 potential interactions
  // 40,000,000,000 - At 1 GHz, it is 40s for each cycle needed
  
  // 50,000 patches
  // 50,000 x 50,000 potential interactions
  // 2,500,000,000 - At 1 GHz, it is 2.5s for each cycle needed

  // TODO: optimization ideas:
  // perhaps could optimize with spatial structures
  // BSP could allow only checking patches in front of given patch
  // Quad-tree could allow testing near by patches
  // perhaps could check patches against quads first to speed up
  // perhaps could check patches against quads in a spatial structure
  // could put patches in buckets based on normals and find patches with opposite normals
  // lots of ways to narrow the search. Probably need both close by ones and opposite normal ones

  // could put in a octree/quadtree with bitmask of normal classification - can search by distance and classification quickly
  // could adaptively change the normal classification search with distance in the search - only search more widely for normals which are exactly opposite


  // 2,260 patches
  // 2,260 x 2,260 potential interactions
  // 5,107,600 - At 1 GHz, it is 195 cycles per second
}


vec4f reflect(vec4f in, vec4f norm)
{
  return in - (norm * (2.0f * dot3(in, norm)));
}


#include <OpenGL/gl.h> // suggest not include directly but make the framework provide an include which includes the system one

void updateTexture(Texture& tex)
{
  if (!tex.bits)
    return;
  
  // Creating texture
  if (tex.state == 0)
  {
    if (tex.texId != 0)
      glDeleteTextures(1, &tex.texId);
    glGenTextures(1, &tex.texId);
    tex.state = 1;
  }

  glBindTexture(GL_TEXTURE_2D, tex.texId);
  glActiveTexture(GL_TEXTURE0);
  
  // Setting parameters
  if (tex.state == 1)
  {
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);//NEAREST);
    tex.state = 2;
  }

  // Uploading
  if (tex.state == 2)
  {
    // upload texture
    //printf("Uploading texture %i %i\n", tex.w, tex.h);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, tex.w, tex.h, 0, GL_BGRA, GL_UNSIGNED_INT_8_8_8_8_REV, tex.bits);
    // We need to keep re-uploading if we are still refining the radiosity solution
    //tex.state = 3;
  }
}

void drawScene(Scene& scene, bool phongType, bool drawUsingPatches, bool useLightMap, float* modelViewMatrix, float* translation)
{
  if (!drawUsingPatches && useLightMap)
  {
    glEnable(GL_TEXTURE_2D);
    updateTexture(scene.texture);
  }
  glBegin(GL_TRIANGLES);
  {
    vec4f l = scene.light.position;
    float shininess = scene.light.shininess;
    float lightPower = scene.light.power;
    vec4f lightColor = scene.light.color;

    if (drawUsingPatches)
    {
      for (auto p : scene.patches)
      {
        mat4f p2 = scene.quads[p.quadIndex].basis;
        vec4f vecToPatch2 = p.position;
        vec4f t = scaleVector(p2.t, p.w * 0.5f);
        vec4f b = scaleVector(p2.b, p.h * 0.5f);
        vec4f qa = vecToPatch2 - t - b;
        vec4f qb = vecToPatch2 - t + b;
        vec4f qc = vecToPatch2 + t + b;
        vec4f qd = vecToPatch2 + t - b;
        vec4f v[4] = { qa, qb, qc, qd };
        int index[6] = { 0, 1, 2,  2, 3, 0 };
        for (int i = 0; i < 6; i++)
        {
		      const vec4f zero = {{{ 0.0f, 0.0f, 0.0f, 0.0f }}};
		      // const vec4f cam = {{{ translation[0], translation[1], translation[2], 0.0f }}};
		      // const vec4f cam = {{{ modelViewMatrix[12], modelViewMatrix[13], modelViewMatrix[14], 0.0f }}};
		      // const vec4f cam = {{{ modelViewMatrix[3], modelViewMatrix[7], modelViewMatrix[11], 0.0f }}};
          int j = index[i];

          //vec4f& n = q.basis.n;
          vec4f N = p2.n;
          vec4f V = v[j];
          vec4f L = l;

          Math::transformVector(&V.x, modelViewMatrix, &v[j].x);

          vec4f toL = L - V;
          float distSqr = dot3(toL, toL);
          toL = toL * (1.0f/sqrt(distSqr));
		      //vec4f viewDir = cam - V;
		      vec4f viewDir = zero - V;
          Math::normalizeVector(&viewDir.x);
          float angle;
          vec4f color = p.color;

          if (!p.inShadow)
          {
            float pf = 1.0f;
            if (phongType)
            {
              // Real Phong
              vec4f fromL = zero - toL;
              vec4f R = reflect(fromL, N);
              angle = dot3(R, viewDir);
              pf = 0.25f;
              pf = 0.35f;
            }
            else
            {
              // Blinn-Phong
              vec4f H = toL + viewDir;
              Math::normalizeVector(&H.x); // ??? How is Blinn-Phong an optimization??
              angle = dot3(H, N);
            }

            if (angle < 0.0f)
              angle = 0.0f;
            float specIntensity = std::pow(angle, shininess * pf);

            color = color + (lightColor * (specIntensity * lightPower / distSqr));
          }

          //glColor3f(p.color.x, p.color.y, p.color.z);
          glColor3f(color.x, color.y, color.z);
          
          glVertex3f(v[j].x, v[j].y, v[j].z);
        }
      }
    }
    else
    {
      if (useLightMap)
      {
        // Textured quads using light map
        for (auto q : scene.quads)
        {
          vec4f v[4] = { q.a, q.b, q.c, q.d };
          int index[6] = { 0, 1, 2,  2, 3, 0 };
          for (int i = 0; i < 6; i++)
          {
            int j = index[i];
            //glTexCoord2f(q.uvs[j].x/float(scene.texture.w), (scene.texture.h-q.uvs[j].y)/float(scene.texture.h));
            glTexCoord2f(q.uvs[j].x, q.uvs[j].y);
            //glColor3f(q.color.x, q.color.y, q.color.z);
            glVertex3f(v[j].x, v[j].y, v[j].z);
          }
        }
      }
      else
      {
        // Gourard L.N shaded flat color quads
        for (auto q : scene.quads)
        {
          vec4f v[4] = { q.a, q.b, q.c, q.d };
          float LdotN[4];
          for (int i = 0; i < 4; i++)
          {
            vec4f& n = q.basis.n;
            vec4f toL = l - v[i];
            Math::normalizeVector(&toL.x);
            LdotN[i] = Math::dotProduct(&toL.x, &n.x);
          }
          int index[6] = { 0, 1, 2,  2, 3, 0 };
          for (int i = 0; i < 6; i++)
          {
            int j = index[i];
            float d = LdotN[j];
            glColor3f(d, d, d); glVertex3f(v[j].x, v[j].y, v[j].z);
          }
        }
      }
    }
  }
  glEnd();
}


#include "Widget.h"
#include "CommonWidgets.h"
#include "Painter.h"
#include "Application.h"
USING_NAMESPACE


class OpenGLWindow : public Widget
{
public:
  OpenGLWindow(Widget* a_parent, const char* a_name, Scene& scn)
    : Widget(a_parent, a_name), scene(scn)
    , vbox(this)
    , quitButton(&vbox, "Quit")
  {
    new CheckBox(&vbox, drawPatches, "Render Patches");
    new CheckBox(&vbox, useLightMap, "Use Light Map");
    new CheckBox(&vbox, useRealPhong, "Use Real Phong (or Blinn-Phong)");
    HBox *fovH = new HBox(&vbox);
    new Label(fovH, fovLabel);
    new Slider(fovH, fovValue);

    // TODO: how to handle destruction of owned objects?
    //       in example above we have vbox which is a member of the window so will
    //       automatically be created and destroyed by the C++ language when the
    //       window is created and destroyed. However it is also 'owned' by the
    //       window in that we passed 'this' to it. Similarly quitButton is owned
    //       by vbox, but also will automatically be created and destroyed by the
    //       window. Another case is objects that are owned but are not automatically
    //       destroyed. In the above code, two unnamed checkboxes are created.
    //       They will currently not get destroyed. If the objects automatically
    //       destroy their children, that would be a problem for vbox and quitButton.
    //       A solution might be to deleteLater where if the object is not deleted
    //       within the next frame, it will then automatically be deleted. This
    //       causes a frame delay and is not explicit. Another solution is to use
    //       a policy - possibly application wide or possibly for the window.
    //       The policy could be one of many, firstly to do nothing as currently
    //       is the case (everything is the programmers responsibility). Second
    //       policy could be the default one which is a debug mode that does nothing
    //       however everything is reference counted and the code can check and
    //       warn at runtime that something could leak. The third policy could
    //       do the deleteLater thing.

    fovLabel = "FOV";
    fovValue = 32000;//45;
    onFovChanged(fovValue.value());
    connect(quitButton.activated, this, &OpenGLWindow::onQuit); 
    connect(fovValue.valueChanged, this, &OpenGLWindow::onFovChanged); 

    tx = (int)scn.camera.position.x;
    tx = (int)scn.camera.position.y;
    rx = (int)scn.camera.rotation.x;
    ry = (int)scn.camera.rotation.y;
  
    lightPosition = scene.light.position;
  }

  int frame = 0;
  vec4f lightPosition;
  void updateLight()
  {
    frame++;
    vec4f pos;
    pos.x =  20.0f * cos(float(frame) / 10.0f); 
    pos.z = -20.0f * sin(float(frame) / 10.0f); 
    pos.y = 0.0f;
    pos.w = 0.0f;
    scene.light.position = lightPosition + pos;
    void initializePatchLighting(Scene& scn);
    initializePatchLighting(scene);
  }

  float fovRadians;
  void onFovChanged(int f)
  {
    printf("fov changed: %i\n", f);
    float ang = 180.0f * float(f) / 65536.0f;
    fovRadians = Math::degreesToRadians(ang);
  }

  void onQuit(int)
  {
    exit(0);
  }

  void keyEvent(KeyEvent& a_event) override
  {
    printf("got key event %c\n", a_event.m_key);
    if (int(a_event.m_key) == 'w' || int(a_event.m_key) == 'W')
    {
      tz += 1;
    }
    if (int(a_event.m_key) == 's')
    {
      tz -= 1;
    }
    if (int(a_event.m_key) == 'a')
    {
      tx += 1;
    }
    if (int(a_event.m_key) == 'd')
    {
      tx -= 1;
    }
    repaint();
  }

  void mouseEvent(MouseEvent& a_event) override
  {
    int x = a_event.m_position.m_x;
    int y = a_event.m_position.m_y;
    if (lastX == -1) { lastX = x; lastY = y; }
    if (a_event.m_buttons == MB_Right)
    {
      tx += x - lastX; ty += y - lastY;
    }
    else if (a_event.m_buttons == MB_Left)
    {
      trackingMouse = !trackingMouse;
      x = -1;
    }
    else if (trackingMouse)
    {
      rx += x - lastX; ry += y - lastY;
    }
    else
    {
      x = -1;
    }
    lastX = x; lastY = y;
    repaint();
  }
    
  void paintEvent(PaintEvent& a_event) override
  {
    Painter p(this);
    p.setPen(0x000000);
    int w = width() / 2;
    int h = height() / 2;
    p.drawLine(5, 5, w-5, 5);
    p.drawLine(5, 5, 5, h-5);
    p.drawLine(w-5, 5, w-5, h-5);
    p.drawLine(5, h-5, w-5, h-5);
    //repaint();
  }

  void updateRadiosity()
  {
    static int frame = 0;
    int frameWait = 1;
    float timesPerFrame = scene.settings.timesPerFrame;
    if (timesPerFrame < 1.0f)
    {
      frameWait = int(1.0f / timesPerFrame);
      timesPerFrame = 1.0f;
    }
    if ((frame % frameWait) == 0)
      for (int i = 0; i < timesPerFrame; i++)
        calculateFormFactors(scene);
    frame++;
  }

  //void _glPaint()
  void glPaint() override
  {
    updateLight();
    updateRadiosity();

    float projectionMatrix[16];
    float w = width();
    float h = height();
    Math::makePerspectiveMatrix4x4(projectionMatrix, fovRadians, w / h, 1.0f, 1000.f);
    glMatrixMode(GL_PROJECTION);
    glLoadMatrixf(projectionMatrix);

    float modelViewMatrix[16];
    float translation[4] = { float(tx), float(ty), float(tz), 0.0f };
    float rotation[4] = { float(ry), float(rx), float(rz), 0.0f };
    Math::translationRotationScaleToMatrix4x4(modelViewMatrix, translation, rotation, 1.0f);
    glMatrixMode(GL_MODELVIEW);
    glLoadMatrixf(modelViewMatrix);

    glDepthMask(GL_TRUE);
    glDisable(GL_BLEND);
    glDisable(GL_TEXTURE_2D);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_CULL_FACE);
    glCullFace(GL_FRONT);
    glFrontFace(GL_CW);
    //glDisable(GL_CULL_FACE);
    
    //glDepthFunc(GL_LESS);
    glClearDepth(1.0f);
    glClear(GL_DEPTH_BUFFER_BIT);

    glViewport(25, 25, width() - 50, height() - 50); // clip the drawing to this and 

    /*
    glBegin(GL_TRIANGLES);
      glColor3f( 1.0f, 0.0f, 0.0f ); glVertex3f( 0.0f, 0.0f, 10.0f );
      glColor3f( 0.0f, 1.0f, 0.0f ); glVertex3f( 0.0f, 10.0f, 10.0f );
      glColor3f( 0.0f, 0.0f, 1.0f ); glVertex3f( 10.0f, 1.0f, 10.0f );
    glEnd();
    */

    drawScene(scene, useRealPhong.value(), drawPatches.value(), useLightMap.value(), modelViewMatrix, translation);

    // Reset state
    glColor3f(1.0f, 1.0f, 1.0f);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    glDepthMask(GL_FALSE);
    glDisable(GL_DEPTH_TEST);
    glDisable(GL_CULL_FACE);
    glDisable(GL_TEXTURE_2D);
  }

private:
  Scene&           scene;
  Property<bool>   useRealPhong;
  Property<bool>   drawPatches;
  Property<bool>   useLightMap;
  Property<String> fovLabel;
  Property<int>    fovValue;
  VBox             vbox;
  Button           quitButton;

  bool trackingMouse = false;
  int tx = 0, ty = 0, tz = -120;
  int rx = 0, ry = 0, rz = 0;
  int lastX = -1, lastY = -1;
};


void formFactorDisplayTest(Scene& scn)
{
  Application app;
  OpenGLWindow testWin(0, "Test Window", scn);
  testWin.setNewSize((int)scn.window.w, (int)scn.window.h);
  app.exec();
}


void octreeTest()
{
  DTree<3> testOctree;

  int idx = 0;
  for (int i = 0; i < 4; i++)
  for (int j = 0; j < 4; j++)
  for (int k = 0; k < 4; k++)
  {
    Patch p;
    p.position.v[0] = i;
    p.position.v[1] = j;
    p.position.v[2] = k;
    PatchObject *obj = new PatchObject(p, idx++);
    testOctree.AddObject(obj);
  }

  printf("bounds: %i,%i,%i  size: %i\n",
      testOctree.bounds.origin.v[0],
      testOctree.bounds.origin.v[1],
      testOctree.bounds.origin.v[2],
      testOctree.bounds.size);
  
  auto f = [](const DTree<3>::DTreeBounds& b)
  {
    return true;
  };
  
  std::vector<DTree<3>::ObjectInterface*> objList = testOctree.QueryExecute(f);
  for (auto o : objList)
  {
    PatchObject* obj = (PatchObject*)o;
    int idx = obj->idx;
    printf("idx: %i\n", idx);
  }
  printf("---\n");
    
  auto ctx = testOctree.InitializeQuery(f);

  // This is alternative code to iterate via the octree, but seems to not be working - probably not returning all
  PatchObject* obj = nullptr;
  while ((obj = (PatchObject*)testOctree.QueryNext(ctx)) != nullptr)
  {
    int idx = obj->idx;
    printf("idx: %i\n", idx);
  }
  

  exit(0);
}


void MakeRegularDodecahedron(std::vector<vec4f>& verts, float radius)
{
  verts.clear();
  verts.reserve(20); // The number of vertices expected

  float phi = Math::phi();
  float a = radius / sqrt(3.0f);
  float b = a / phi;
  float c = a * phi;

  float v[2] = { -1.0f, 1.0f };
  for (int i = 0; i < 2; i++)
  {
    for (int j = 0; j < 2; j++)
    {
      float ib = v[i] * b;
      float jc = v[j] * c;
      verts.push_back((vec4f){{{ 0.0f, ib, jc, 0.0f }}});
      verts.push_back((vec4f){{{ ib, jc, 0.0f, 0.0f }}});
      verts.push_back((vec4f){{{ ib, 0.0f, jc, 0.0f }}});
      for (int k = 0; k < 2; k++)
      {
        verts.push_back((vec4f){{{ v[i] * a, v[j] * a, v[k] * a, 0.0f }}});
      }
    }
  }
}


template <typename T>
class BlendProperty : public AbstractProperty<T>
{
public:
	BlendProperty() {}
	BlendProperty(const T& val) : m_value(val) {}
	BlendProperty(const BlendProperty<T>& other) : m_value(other.m_value) {}
  
	BlendProperty<T>& operator=(T const & a_newValue)
	{
		AbstractProperty<T>::setValue(a_newValue);
		return *this;
	}

  float tween(float time);

protected:
	T const & get() const
  {
    return m_value;
  }
	void set(T a_newValue)
  {
    m_lastSetTime = m_lastTime;
    m_previousValue = m_value;
    m_value = a_newValue;
  }

private:
  float m_lastSetTime = 0.0f;
  float m_lastTime = 0.0f;
	T m_previousValue;
	T m_value;
};

  
template <typename T>
float BlendProperty<T>::tween(float time)
{
  m_lastTime = time;
  float deltaTime = time - m_lastSetTime;
  T deltaValue = m_value - m_previousValue;
  if (deltaTime >= 1.0f) // 1 second
    return float(m_value);
  return float(m_previousValue) + float(deltaValue) * deltaTime;
}


template <>
float BlendProperty<bool>::tween(float time)
{
  float v = (m_value) ? 1.0f : 0.0f;
  float l = (m_previousValue) ? 1.0f : 0.0f;
  m_lastTime = time;
  float deltaTime = time - m_lastSetTime;
  if (deltaTime >= 1.0f) // 1 second
    return v;
  return l + (v - l) * deltaTime;
}


class GeometryExplorerWindow : public Widget
{
public:
  GeometryExplorerWindow(Widget* a_parent, const char* a_name)
    : Widget(a_parent, a_name)
    , vbox(this)
    , quitButton(&vbox, "Quit")
  {
    connect(quitButton.activated, this, &GeometryExplorerWindow::onQuit); 
    new CheckBox(&vbox, regular,   "Regular Shapes");   // IAP unlock
    new CheckBox(&vbox, euclidean, "Euclidean Space");  // IAP unlock
    new CheckBox(&vbox, fractal,   "Fractal Shapes");   // IAP unlock
    new CheckBox(&vbox, color,     "Use Colours");
    new CheckBox(&vbox, ortho,     "Orthographic Projection");
    new CheckBox(&vbox, spherical, "Projected on a Sphere");
    new CheckBox(&vbox, dual,      "Display the dual");
    Grid *g = new Grid(&vbox, 2);
    new GridSlider(g, fov,         "Field of view");
    new GridSlider(g, scale,       "Size");
    new GridSlider(g, dimensions,  "Dimensions");
    new GridSlider(g, complexity,  "Complexity");
    new GridSlider(g, view,        "View"); // Camera angle
    startTimer(30);
  }
  
  void onQuit()
  {
    exit(0);
  }
  
  VBox                vbox;
  Button              quitButton;
  float               time = 0.0f;       // 0 -> infinity
  BlendProperty<bool> regular = true;    // true or false  => restrict to platonic or constrained type of shapes
  BlendProperty<bool> euclidean = true;  // true or false  => allow non-euclidean geometry so space can curve?
  BlendProperty<bool> fractal = false;   // true or false  => show fractal geometries
  BlendProperty<bool> color = false;     // true or false  => colorize
  BlendProperty<bool> ortho = false;     // true or false  => display with orthographic projection
  BlendProperty<bool> spherical = false; // true or false  => projected on to a sphere
  BlendProperty<bool> dual = false;      // true or false  => display the dual
  BlendProperty<int>  fov = 32000;       // field of view
  BlendProperty<int>  scale = 32000;     // scale
  BlendProperty<int>  dimensions = 2;    // 0 -> 3         => perhaps later could expand to higher dimensions, but only 3 at first  
  BlendProperty<int>  complexity = 10;   // 0.0 -> 1.0     => from least complex to most complex type of geometry
  BlendProperty<int>  view = 0;          // the orientation to center the orthographic projection about - spinning, edge, normal, vertex, face

	void timerEvent(TimerEvent& a_event) override
  {
    repaint();
  }

  void paintEvent(PaintEvent& a_event) override
  {
    time += 0.05f;

    Painter p(this);
    p.setPen(0x000000);
    int w = width() / 2;
    int h = height() / 2;
    p.drawLine(5, 5, w-5, 5);
    p.drawLine(5, 5, 5, h-5);
    p.drawLine(w-5, 5, w-5, h-5);
    p.drawLine(5, h-5, w-5, h-5);
  }

  void glPaint() override
  {
    float size = float(scale.tween(time)) / 3200.0f;
    float dim = (dimensions.tween(time) *  4.0f) / 65536.0f;//>> 16;
    float num = (complexity.tween(time) * 60.0f) / 65536.0f;//>> 16;
    
    float d1 = std::min(std::max(dim - 0.0f, 0.0f), 1.0f);
    float d2 = std::min(std::max(dim - 1.0f, 0.0f), 1.0f);
    float d3 = std::min(std::max(dim - 2.0f, 0.0f), 1.0f);
    float d4 = std::min(std::max(dim - 3.0f, 0.0f), 1.0f);

    float projectionMatrix[16];
    float w = width();
    float h = height();
    float ang = 180.0f * float(fov()) / 65536.0f;
    float fovRadians = Math::degreesToRadians(ang);
  
    Math::makePerspectiveMatrix4x4(projectionMatrix, fovRadians, w / h, 1.0f, 1000.f);
    glMatrixMode(GL_PROJECTION);
    glLoadMatrixf(projectionMatrix);

    float modelViewMatrix[16];
    int tx = 0, ty = 0, tz = -120;
    float rx = time * 10.0f * d3;//std::max(dim - 2.0f, 0.0f);
    float ry = time * 12.0f * d4;//std::max(dim - 3.0f, 0.0f);
    float rz = time * 14.0f;
    float translation[4] = { float(tx), float(ty), float(tz), 0.0f };
    float rotation[4] = { float(ry), float(rx), float(rz), 0.0f };
    Math::translationRotationScaleToMatrix4x4(modelViewMatrix, translation, rotation, 1.0f);
    glMatrixMode(GL_MODELVIEW);
    glLoadMatrixf(modelViewMatrix);

    glDepthMask(GL_TRUE);
    glDisable(GL_BLEND);
    glDisable(GL_TEXTURE_2D);
    glEnable(GL_DEPTH_TEST);

    glEnable(GL_CULL_FACE);
    glCullFace(GL_FRONT);
    glFrontFace(GL_CW);
    glDisable(GL_CULL_FACE);
    
    //glDepthFunc(GL_LESS);
    glClearDepth(1.0f);
    glClear(GL_DEPTH_BUFFER_BIT);

    glViewport(25, 25, width() - 50, height() - 50); // clip the drawing to this and 

    /*
    glBegin(GL_TRIANGLES);
      glColor3f( 1.0f, 0.0f, 0.0f ); glVertex3f( 0.0f, 0.0f, 10.0f );
      glColor3f( 0.0f, 1.0f, 0.0f ); glVertex3f( 0.0f, 10.0f, 10.0f );
      glColor3f( 0.0f, 0.0f, 1.0f ); glVertex3f( 10.0f, 1.0f, 10.0f );
    glEnd();
    */




    float a = d1 * size;//std::min(size, 1.0f);  //   dim 0 -> 1   1.0 -> 10.0
    float b = d2 * size * sqrt(2.0);//std::min(size, 1.0f);  //   dim 0 -> 1   1.0 -> 10.0
    float c = d3 * size * sqrt(2.0);//std::min(size, 1.0f);  //   dim 0 -> 1   1.0 -> 10.0
    float radians = 0.5f * 3.1429f;
    vec4f verts[5][32];
    verts[0][0] = (vec4f){{{ 0.0f, b * 0.333f, c, 0.0f }}};
    verts[0][1] = (vec4f){{{ cosf(radians), sinf(radians) + b, 0.0f, 0.0f }}};
    for (int i = 0; i < 14; i++)
    {
      radians = (0.5f - float(i)/13.0f) * 3.1429f;
      verts[0][i+2] = (vec4f){{{ cosf(radians) + a, sinf(radians), 0.0f, 0.0f }}};
    }
    radians = -0.5f * 3.1429f;
    verts[0][16] = (vec4f){{{ cosf(radians), sinf(radians), 0.0f, 0.0f }}};
    for (int i = 0; i < 14; i++)
    {
      radians = (-0.5f - float(i)/13.0f) * 3.1429f;
      verts[0][i+1+16] = (vec4f){{{ cosf(radians) - a, sinf(radians), 0.0f, 0.0f }}};
    }
    verts[0][31] = verts[0][1];




    verts[1][0] = (vec4f){{{ 0.0f, b * 0.333f, c, 0.0f }}};
    verts[1][1] = (vec4f){{{ cosf(radians), sinf(radians) + b, 0.0f, 0.0f }}};
    for (int i = 0; i < 14; i++)
    {
      radians = (0.5f - float(i)/13.0f) * 3.1429f;
      verts[1][i+2] = (vec4f){{{ cosf(radians) + a, sinf(radians), 0.0f, 0.0f }}};
    }
    radians = -0.5f * 3.1429f;
    verts[1][16] = (vec4f){{{ cosf(radians), sinf(radians), 0.0f, 0.0f }}};
    for (int i = 0; i < 14; i++)
    {
      radians = (-0.5f - float(i)/13.0f) * 3.1429f;
      verts[1][i+1+16] = (vec4f){{{ cosf(radians) - a, sinf(radians), 0.0f, 0.0f }}};
    }
    verts[1][31] = verts[0][1];


    {
      vec4f vert[32];
      for (int i = 0; i < 32; i++)
      {
        // TODO: blend verts[x] and verts[x+1]
        //  vert[i] = verts[0][i];
        vert[i] = verts[1][i];
      }
      glBegin(GL_TRIANGLES);
      for (int i = 0; i < 30; i++)
      {
        glColor3f( 0.0f, 0.0f, 0.0f ); glVertex3f( vert[0].v[0],   vert[0].v[1],   vert[0].v[2]   );
        glColor3f( 0.0f, 0.0f, 0.0f ); glVertex3f( vert[i+1].v[0], vert[i+1].v[1], vert[i+1].v[2] );
        glColor3f( 0.0f, 0.0f, 0.0f ); glVertex3f( vert[i+2].v[0], vert[i+2].v[1], vert[i+2].v[2] );
      }
      glEnd();
    }

    glBegin(GL_TRIANGLES);
    {
      std::vector<vec4f> dodecahedronVerts;
      MakeRegularDodecahedron(dodecahedronVerts, 10.0f);
      //for (vec4f& v : dodecahedronVerts)
      for (int i = 0; i < dodecahedronVerts.size() - 2; i++)
      {
        //vec4f& v = dodecahedronVerts[i];
        vec4f* vert = &dodecahedronVerts[0];
        glColor3f( 1.0f, 0.0f, 0.0f ); glVertex3f( vert[0].v[0],   vert[0].v[1],   vert[0].v[2]   );
        glColor3f( 0.0f, 1.0f, 0.0f ); glVertex3f( vert[i+1].v[0], vert[i+1].v[1], vert[i+1].v[2] );
        glColor3f( 0.0f, 0.0f, 1.0f ); glVertex3f( vert[i+2].v[0], vert[i+2].v[1], vert[i+2].v[2] );

      }
    }
    glEnd();



    /*
    if (dim == 0)
    {
      glBegin(GL_TRIANGLES);
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 0.0f, 0.0f, 0.0f );
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 0.0f, 0.0f, 0.0f );
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 0.0f, 0.0f, 0.0f );
      glEnd();
    }
    else if (dim == 1)
    {
      glBegin(GL_TRIANGLES);
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 0.0f, 10.0f, 0.0f );
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 0.0f, 10.0f, 1.0f );
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 0.0f, 0.0f, 0.0f );
        
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 0.0f, 0.0f, 0.0f );
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 0.0f, 0.0f, 1.0f );
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 0.0f, 10.0f, 1.0f );
      glEnd();
    }
    else if (dim == 2)
    {
      glBegin(GL_TRIANGLES);
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 10.0f, -4.0f, 0.0f );
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( 0.0f, 8.0f, 0.0f );
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex3f( -10.0f, -4.0f, 0.0f );
      glEnd();
    }
    else
    {
      MakeRegularDodecahedron(verts, size);
      glBegin(GL_TRIANGLES);
      for (vec4f& v : verts)
      {
        glColor3f( 1.0f, 0.0f, 0.0f );
        glVertex3f( v.v[0], v.v[1], v.v[2] );
        glColor3f( 1.0f, 0.0f, 0.0f );
        glVertex3f( v.v[0]+1.0f, v.v[1], v.v[2] );
        glColor3f( 1.0f, 0.0f, 0.0f );
        glVertex3f( v.v[0], v.v[1]+1.0f, v.v[2] );
      }
      glEnd();
    }
    */
    

    // Reset state
    glColor3f(1.0f, 1.0f, 1.0f);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    glDepthMask(GL_FALSE);
    glDisable(GL_DEPTH_TEST);
    glDisable(GL_CULL_FACE);
    glDisable(GL_TEXTURE_2D);
  }
};


void GeometryExplorerMain()
{
  Application app;
  GeometryExplorerWindow testWin(0, "Geometry Explorer");
  testWin.setNewSize(600, 800);
  app.exec();
}


void GeometryTests()
{
  printf("phi = %f\n", Math::phi());
  std::vector<vec4f> dodecahedronVerts;
  MakeRegularDodecahedron(dodecahedronVerts, 10.0f);
  for (vec4f& v : dodecahedronVerts)
  {
    printf("vert[] = { %f, %f, %f }\n", v.v[0], v.v[1], v.v[2]);
  }
  
  GeometryExplorerMain();
  exit(0);
}


void formFactorTest()
{
  //GeometryTests();
  
  // octreeTest();
  // exit(0);

  Scene scn = parseFile("scene.txt");
  createTexture(scn);
  convertObjectsToQuads(scn);
  convertQuadsToPatches(scn);
  initializePatches(scn);
  initializePatchLighting(scn);
  calculateFormFactorScales(scn);
  calculateFormFactors(scn);
  //dumpSceneInfo(scn);
  formFactorDisplayTest(scn);
  exit(0);
}