//
// This file is part of MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2015 Richard S. Smith and collaborators.
//
// If you use MorphoGraphX in your work, please cite:
//   http://dx.doi.org/10.7554/eLife.05864
//
// MorphoGraphX is free software, and is licensed under under the terms of the 
// GNU General (GPL) Public License version 2.0, http://www.gnu.org/licenses.
// 
//
// This is Jules Bloomenthal's implicit surface polygonizer from GRAPHICS GEMS IV.
// http://www.unchainedgeometry.com/jbloom/misc/PolygonizerC++.zip
// Converted to C++ by J. Andreas Baerentzen 2003.
// Addapted for MorphoGraphX by Richard S. Smith 2010.
//
#include "Polygonizer.hpp"

#include "Geometry.hpp"

#include <iostream>
#include <list>
#include <math.h>
#include <set>
#include <stdlib.h>
#include <sys/types.h>
#include <vector>

namespace mgx 
{
  using namespace std;
  
  // Types for vertex cache
  typedef Vector<6, float> Point6f;
  typedef std::map<Point6f, int> Point6fIntMap;
  typedef Point6fIntMap::iterator Point6fIntMapIter;
  typedef std::pair<Point6f, int> Point6fIntPair;
  typedef Vector<12, float> Point12f;
  typedef std::map<Point12f, int> Point12fIntMap;
  typedef Point12fIntMap::iterator Point12fIntMapIter;
  typedef std::pair<Point12f, int> Point12fIntPair;
  
  // An Implicit Surface Polygonizer
  // polygonize: polygonize the implicit surface function
  //   arguments are:
  //   ImplicitFunction
  //       the implicit surface function
  //       return negative for inside, positive for outside
  //   float size
  //       width of the partitioning cube
  //   vertices, triangles
  //       the data structures into which information is put.
  const int RES = 20; // # converge iterations
  
  const int LBN = 0; // left bottom near corner
  const int LBF = 1; // left bottom far corner
  const int LTN = 2; // left top near corner
  const int LTF = 3; // left top far corner
  const int RBN = 4; // right bottom near corner
  const int RBF = 5; // right bottom far corner
  const int RTN = 6; // right top near corner
  const int RTF = 7; // right top far corner
  
  // Polygonize process class
  class Polygonizer::Process {
    // parameters, function, storage
    std::vector<Point3f>& vertices;
    std::vector<Point3i>& triangles;
  
    ImplicitFunction& f;   // implicit surface function
    float cubeSize;        // cube size
    Point3f voxSize;       // voxel size
    Point3f* bBox;         // Bounding box
  
    Point6fIntMap vIds;   // Vertex map
    int maxvId;           // Max vertex id
  
    bool triangle(int i1, int i2, int i3)   // Add a triangle to the list
    {
      triangles.push_back(Point3i(i1, i2, i3));
      return (true);
    }
  
    bool quad(int q1, int q2, int q3, int q4)   // Add a quad to the list
    {
      // Insert the same triangles regardless of order
      int minq = std::min(q1, std::min(q2, std::min(q3, q4)));
      if(minq == q1 or minq == q3)
        return triangle(q1, q2, q3) and triangle(q1, q3, q4);
      return triangle(q2, q3, q4) and triangle(q1, q2, q4);
    }
  
    // vId: return index for a vertex
    // return saved index if any, else compute vertex and save
    int vId(Point3f c1, Point3f c2)
    {
      if(c1 > c2)
        std::swap(c1, c2);
      Point6f key(c1.x(), c1.y(), c1.z(), c2.x(), c2.y(), c2.z());
      Point6fIntMapIter ip;
      if((ip = vIds.find(key)) != vIds.end())
        return ip->second;
  
      // If not already there find position
      Point3f pos = lineSearch(c1, c2);
  
      int vid = vertices.size();
      maxvId = vid;
      vertices.push_back(pos);     // save vertex
      vIds[key] = vid;
      return vid;
    }
  
    // Line search from two points of differing sign (or label), converge to crossing)
    // (calls implicit surface evaluation functions)
    Point3f lineSearch(Point3f p1, Point3f p2)
    {
      Point3f p, inside = p1, outside = p2;
  
      if(not f.eval(p1))
        std::swap(inside, outside);
  
      int steps = RES;
      while(steps--) {
        p = 0.5f * (inside + outside);
        if(f.eval(p))
          inside = p;
        else
          outside = p;
      }
  
      return inside;
    }
  
    // dotet: triangulate the tetrahedron for 3D
    // b-c-d are clockwise viewed from a
    bool dotet(Point3f a, Point3f b, Point3f c, Point3f d)
    {
      int index = 0, e1 = -1, e2 = -1, e3 = -1, e4 = -1, e5 = -1, e6 = -1;
      bool aIn, bIn, cIn, dIn;
  
      // index is a 4-bit number representing one of the 16 possible cases
      if((aIn = f.eval(a)))
        index += 8;
      if((bIn = f.eval(b)))
        index += 4;
      if((cIn = f.eval(c)))
        index += 2;
      if((dIn = f.eval(d)))
        index += 1;
  
      // Calculate surface intersection points
      // index is now 4-bit number representing one of the 16 possible cases
      if(aIn != bIn)
        e1 = vId(a, b);
      if(aIn != cIn)
        e2 = vId(a, c);
      if(aIn != dIn)
        e3 = vId(a, d);
      if(bIn != cIn)
        e4 = vId(b, c);
      if(bIn != dIn)
        e5 = vId(b, d);
      if(cIn != dIn)
        e6 = vId(c, d);
  
      // 14 productive tetrahedral cases (0000 and 1111 do not yield polygons
      switch(index) {
      case 1:
        return triangle(e5, e6, e3);
      case 2:
        return triangle(e2, e6, e4);
      case 3:
        return quad(e3, e5, e4, e2);
      case 4:
        return triangle(e1, e4, e5);
      case 5:
        return quad(e3, e1, e4, e6);
      case 6:
        return quad(e1, e2, e6, e5);
      case 7:
        return triangle(e1, e2, e3);
      case 8:
        return triangle(e1, e3, e2);
      case 9:
        return quad(e1, e5, e6, e2);
      case 10:
        return quad(e1, e3, e6, e4);
      case 11:
        return triangle(e1, e5, e4);
      case 12:
        return quad(e3, e2, e4, e5);
      case 13:
        return triangle(e6, e2, e4);
      case 14:
        return triangle(e5, e3, e6);
      }
      return true;
    }
  
  public:
    Process(ImplicitFunction& _f, float _cubeSize, Point3f _voxSize, vector<Point3f>& _vertices,
            vector<Point3i>& _triangles)
      : vertices(_vertices)
      , triangles(_triangles)
      , f(_f)
      , cubeSize(_cubeSize)
      , voxSize(_voxSize)
      , maxvId(0)
    {
    }
    ~Process() {
    }
  
    // Do tetrahedral polygonization
    void march(Point3f* bBox)
    {
      // Check that vertex cache is still valid (or at least wont crash).
      if(maxvId > (int)vertices.size())
        throw(string("Polygonizer::march:Error Vertex cache invalid"));
  
      // Pad bounding box with at least two voxels worth of cubes
      Point3i pad(int(2.0 * std::max(1.0f, voxSize.x() / cubeSize)),
                  int(2.0 * std::max(1.0f, voxSize.y() / cubeSize)),
                  int(2.0 * std::max(1.0f, voxSize.z() / cubeSize)));
      int imin = int(bBox[0].x() / cubeSize) - pad.x();
      int imax = int(bBox[1].x() / cubeSize) + pad.x();
      int jmin = int(bBox[0].y() / cubeSize) - pad.y();
      int jmax = int(bBox[1].y() / cubeSize) + pad.y();
      int kmin = int(bBox[0].z() / cubeSize) - pad.z();
      int kmax = int(bBox[1].z() / cubeSize) + pad.z();
  
      // Start progress bar
      // progressStart("Extracting surface", steps);
      // Create cubes, force them to be in same place for same size cubes.
      for(int i = imin; i <= imax; i++) {
        // if(!progressAdvance(i-imin))
        //  break;
        for(int j = jmin; j <= jmax; j++) {
          for(int k = kmin; k <= kmax; k++) {
            Point3f corners[8];
            for(int n = 0; n < 8; n++) {
              corners[n].x() = ((float)(i + ((n >> 2) & 1)) - .5) * cubeSize;
              corners[n].y() = ((float)(j + ((n >> 1) & 1)) - .5) * cubeSize;
              corners[n].z() = ((float)(k + ((n >> 0) & 1)) - .5) * cubeSize;
            }
  
            // decompose into tetrahedra and polygonize:
            bool noabort = dotet(corners[LBN], corners[LTN], corners[RBN], corners[LBF])
              and dotet(corners[RTN], corners[LTN], corners[LBF], corners[RBN])
              and dotet(corners[RTN], corners[LTN], corners[LTF], corners[LBF])
              and dotet(corners[RTN], corners[RBN], corners[LBF], corners[RBF])
              and dotet(corners[RTN], corners[LBF], corners[LTF], corners[RBF])
              and dotet(corners[RTN], corners[LTF], corners[RTF], corners[RBF]);
  
            if(!noabort)
              throw string("Polygonzer::march:Error Unknown error");
          }
        }
      }
    }
  };
  
  // Public methods of Polygonizer class
  
  // Do the marching cubes (tetrahedrons)
  // Supply bounding box, and flag to join cells
  void Polygonizer::march(Point3f* bBox) {
    process->march(bBox);
  }
  
  // Create process class to do the work
  Polygonizer::Polygonizer(ImplicitFunction& func, float cubeSize, Point3f voxSize, std::vector<Point3f>& vertices,
                           std::vector<Point3i>& triangles)
  {
    process = new Process(func, cubeSize, voxSize, vertices, triangles);
  }
  
  // Cleanup
  Polygonizer::~Polygonizer() {
    delete process;
  }
}
