#include <sstream>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>

#include "Epsilon.hxx"
BOOST_CLASS_EXPORT(Epsilon)
#include "eval.hxx"

Epsilon::Epsilon(char const i1, char const i2, char const i3, char const i4) : i1(i1), i2(i2), i3(i3), i4(i4) {}

template<class Archive>
void Epsilon::serialize (Archive & ar, unsigned int const) {
  ar & boost::serialization::base_object<Node>(*this);
  ar & i1;
  ar & i2;
  ar & i3;
  ar & i4;
}

void Epsilon::exchangeTensorIndices (std::map<char, char> const & exchange_map) {
  auto it = exchange_map.find(i1);
  if (it != exchange_map.end()) {
    i1 = it->second;
  }
  it = exchange_map.find(i2);
  if (it != exchange_map.end()) {
    i2 = it->second;
  }
  it = exchange_map.find(i3);
  if (it != exchange_map.end()) {
    i3 = it->second;
  }
  it = exchange_map.find(i4);
  if (it != exchange_map.end()) {
    i4 = it->second;
  }
}

int Epsilon::sortIndices() {
  int ret = 1;
  bool swapped = true;
  do {
    swapped = false;
    if (i1 == i2) {
      ret = 0;
    } else if (i1 > i2) {
      std::swap (i1, i2);
      swapped = true;
      ret *= -1;
    }
    if (i2 == i3) {
      ret = 0;
    } else if (i2 > i3) {
      std::swap (i2, i3);
      swapped = true;
      ret *= -1;
    }
    if (i3 == i4) {
      ret = 0;
    } else if (i3 > i4) {
      std::swap (i3, i4);
      swapped = true;
      ret *= -1;
    }
  } while (swapped);

  return ret;
}

char Epsilon::order () const { return 0; }

std::string Epsilon::print () const {
  std::stringstream ss;
  ss << "Epsilon {" << i1 << i2 << i3 << i4 << "}";

  return ss.str();
}

std::string Epsilon::printMaple () const {
  std::stringstream ss;
  ss << "LeviCivita[~" << i1 << ", ~" << i2 << ", ~" << i3 << ", ~" << i4 << "]";

  return ss.str();
}

int Epsilon::evaluate(std::map <char, char> const & eval_map) const {
  return (- 1 * epsilon_eval.at(64 * eval_map.at(i1)
                       + 16 * eval_map.at(i2)
                       +  4 * eval_map.at(i3)
                       +      eval_map.at(i4)));
}

mpq_class Epsilon::symmetrize() {
  return sortIndices();
}

bool Epsilon::containsIndex (char i) const {
  return (i == i1 || i == i2 || i == i3 || i == i4);
}

int Epsilon::applyTensorSymmetries (int parity) {
  int epsilon_parity = sortIndices();
  return (parity * epsilon_parity);
}

bool Epsilon::lessThan(Node const * other) const {
  auto other_eps = static_cast<Epsilon const *>(other);
  if (i1 < other_eps->i1) {
    return true;
  } else if (i1 > other_eps->i1) {
    return false;
  } else if (i2 < other_eps->i2) {
    return true;
  } else if (i2 > other_eps->i2) {
    return false;
  } else if (i3 < other_eps->i3) {
    return true;
  } else if (i3 > other_eps->i3) {
    return false;
  } else if (i4 < other_eps->i4) {
    return true;
  } else {
    return false;
  }
}

bool Epsilon::equals(Node const * other) const {
  if (other == nullptr) {
    return false;
  } else if (typeid(*this) != typeid(*other)) {
    return false;
  } else {
    auto other_eps = static_cast<Epsilon const *> (other);
    return (i1 == other_eps->i1 && i2 == other_eps->i2 && i3 == other_eps->i3 && i4 == other_eps->i4);
  }
}

std::unique_ptr<Node> Epsilon::clone () const {
  return std::unique_ptr<Node>(new Epsilon(*this));
}

std::tuple<int, char, std::map<char, char>> Epsilon::multiplyWithOther3 (char j1, char j2, char j3) const {
  assert (j1 != j2 && j1 != j3 && j2 != j3);
  bool const is_contained_j1 = containsIndex (j1);
  bool const is_contained_j2 = containsIndex (j2);
  bool const is_contained_j3 = containsIndex (j3);
  std::vector<char> contained;
  if (is_contained_j1) { contained.push_back (j1); }
  if (is_contained_j2) { contained.push_back (j2); }
  if (is_contained_j3) { contained.push_back (j3); }

  if (contained.size() == 0) {
    return {-24, i1, {{j1, i2}, {j2, i3}, {j3, i4}}};
  } else if (contained.size() == 3) {
    std::vector<char> i_vec {i1, i2, i3, i4};
    int parity = 1;
    if (std::find (contained.cbegin(), contained.cend(), i_vec[0]) == contained.cend()) {
      // do nothing
    } else if (std::find (contained.cbegin(), contained.cend(), i_vec[1]) == contained.cend()) {
      std::iter_swap (i_vec.begin(), i_vec.begin() + 1);
      parity *= -1;
    } else if (std::find (contained.cbegin(), contained.cend(), i_vec[2]) == contained.cend()) {
      std::iter_swap (i_vec.begin(), i_vec.begin() + 2);
      parity *= -1;
    } else {
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[3]) == contained.cend());
      std::iter_swap (i_vec.begin(), i_vec.begin() + 3);
      parity *= -1;
    }
    if ( (i_vec[1] == j1 && i_vec[2] == j2 && i_vec[3] == j3) ||
         (i_vec[2] == j1 && i_vec[3] == j2 && i_vec[1] == j3) ||
         (i_vec[3] == j1 && i_vec[1] == j2 && i_vec[2] == j3) ) {
      // sorted
    } else {
      // unsorted
      assert ( (i_vec[1] == j1 && i_vec[3] == j2 && i_vec[2] == j3) ||
               (i_vec[3] == j1 && i_vec[2] == j2 && i_vec[1] == j3) ||
               (i_vec[2] == j1 && i_vec[1] == j2 && i_vec[3] == j3));
      parity *= -1;
    }
    return {-6 * parity, i_vec[0], { }};
  } else if (contained.size() == 2) {
    std::vector<char> i_vec {i1, i2, i3, i4};
    std::vector<char> j_vec {j1, j2, j3};
    int parity = 1;
    if (!is_contained_j1) {
      // nothing
    } else if (!is_contained_j2) {
      j_vec = {j2, j1, j3};
      parity *= -1;
    } else {
      assert (!is_contained_j3);
      j_vec = {j3, j1, j2};
    }
    if (std::find (contained.cbegin(), contained.cend(), i_vec[0]) == contained.cend()) {
      if (std::find (contained.cbegin(), contained.cend(), i_vec[1]) == contained.cend()) {
        // do nothing
      } else if (std::find (contained.cbegin(), contained.cend(), i_vec[2]) == contained.cend()) {
        std::iter_swap (i_vec.begin() + 1, i_vec.begin() + 2);
        parity *= -1;
      } else {
        assert (std::find (contained.cbegin(), contained.cend(), i_vec[3]) == contained.cend());
        std::iter_swap (i_vec.begin() + 1, i_vec.begin() + 3);
        parity *= -1;
      }
    } else if (std::find (contained.cbegin(), contained.cend(), i_vec[1]) == contained.cend()) {
      std::iter_swap (i_vec.begin(), i_vec.begin() + 1);
      parity *= -1;
      if (std::find (contained.cbegin(), contained.cend(), i_vec[2]) == contained.cend()) {
        std::iter_swap (i_vec.begin() + 1, i_vec.begin() + 2);
        parity *= -1;
      } else {
        assert (std::find (contained.cbegin(), contained.cend(), i_vec[3]) == contained.cend());
        std::iter_swap (i_vec.begin() + 1, i_vec.begin() + 3);
        parity *= -1;
      }
    } else {
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[2]) == contained.cend());
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[3]) == contained.cend());
      std::iter_swap (i_vec.begin(), i_vec.begin() + 2);
      std::iter_swap (i_vec.begin() + 1, i_vec.begin() + 3);
    }
    if (i_vec[2] == contained[0]) {
      // sorted
    } else {
      assert (i_vec[2] == contained[1]);
      parity *= -1;
    }
    return { -4 * parity, i_vec[0], {{j_vec[0], i_vec[1]}} };
  } else if (contained.size() == 1) {
    std::vector<char> i_vec {i1, i2, i3, i4};
    std::vector<char> j_vec {j1, j2, j3};
    int parity = 1;
    if (!is_contained_j1) {
      if (!is_contained_j2) {
        // nothing
      } else {
        assert (!is_contained_j3);
        j_vec = {j1, j3, j2};
        parity *= -1;
      }
    } else {
      assert (!is_contained_j2);
      assert (!is_contained_j3);
      j_vec = {j2, j3, j1};
    }
    if (std::find (contained.cbegin(), contained.cend(), i_vec[0]) != contained.cend()) {
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[1]) == contained.cend());
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[2]) == contained.cend());
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[3]) == contained.cend());
      std::iter_swap (i_vec.begin(), i_vec.begin() + 1);
      std::iter_swap (i_vec.begin() + 1, i_vec.begin() + 2);
      std::iter_swap (i_vec.begin() + 2, i_vec.begin() + 3);
      parity *= -1;
    } else if (std::find (contained.cbegin(), contained.cend(), i_vec[1]) != contained.cend()) {
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[0]) == contained.cend());
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[2]) == contained.cend());
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[3]) == contained.cend());
      std::iter_swap (i_vec.begin() + 1, i_vec.begin() + 2);
      std::iter_swap (i_vec.begin() + 2, i_vec.begin() + 3);
    } else if (std::find (contained.cbegin(), contained.cend(), i_vec[2]) != contained.cend()) {
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[0]) == contained.cend());
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[1]) == contained.cend());
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[3]) == contained.cend());
      std::iter_swap (i_vec.begin() + 2, i_vec.begin() + 3);
      parity *= -1;
    } else if (std::find (contained.cbegin(), contained.cend(), i_vec[3]) != contained.cend()) {
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[0]) == contained.cend());
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[1]) == contained.cend());
      assert (std::find (contained.cbegin(), contained.cend(), i_vec[2]) == contained.cend());
      // nothing
    }
    return { -6 * parity, i_vec[0], {{j_vec[0], i_vec[1]}, {j_vec[1], i_vec[2]}} };
  } else {
    assert (false);
    return {0, 0, {}};
  }
}


/*  Sirikata
 *  MeshSimplifier.cpp
 *
 *  Copyright (c) 2010, Tahir Azim.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are
 *  met:
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *  * Neither the name of Sirikata nor the names of its contributors may
 *    be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
 * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <sirikata/mesh/MeshSimplifier.hpp>

#include <boost/functional/hash.hpp>

#include <sirikata/core/util/Timer.hpp>
#ifdef _WIN32
#include <float.h>
#else
#include <cmath>
#endif
#include <math.h>
#include <iomanip>
#define SIMPLIFY_LOG(lvl, msg) SILOG(simplify, lvl, msg)

namespace Sirikata {

namespace Mesh {

#define SIMPLIFIER_INVALID_VECTOR Vector3f(-1000000,-1000000,-1000000)

class IndexedFaceContainer {
public:

  bool valid;
  uint32 idx1;
  uint32 idx2;
  uint32 idx3;

  IndexedFaceContainer(uint32 i, uint32 j, uint32 k) :
    valid(true), idx1(i), idx2(j), idx3(k)
  {
  }

  size_t hash() const {
      size_t seed = 0;

      std::vector<uint32> sortedIndices;
      sortedIndices.push_back(idx1);
      sortedIndices.push_back(idx2);
      sortedIndices.push_back(idx3);
      std::sort(sortedIndices.begin(), sortedIndices.end());

      seed = 0;
      boost::hash_combine(seed, sortedIndices[0]);
      boost::hash_combine(seed, sortedIndices[1]);
      boost::hash_combine(seed, sortedIndices[2]);

      return seed;
  }

  bool operator==(const IndexedFaceContainer&other) const {
    std::vector<uint32> sortedIndices;
    sortedIndices.push_back(idx1);
    sortedIndices.push_back(idx2);
    sortedIndices.push_back(idx3);
    std::sort(sortedIndices.begin(), sortedIndices.end());

    std::vector<uint32> sortedIndices2;
    sortedIndices2.push_back(other.idx1);
    sortedIndices2.push_back(other.idx2);
    sortedIndices2.push_back(other.idx3);
    std::sort(sortedIndices2.begin(), sortedIndices2.end());

    return (sortedIndices[0] == sortedIndices2[0]
            && sortedIndices[1] == sortedIndices2[1]
            && sortedIndices[2] == sortedIndices2[2] );
  }

  class Hasher{
  public:
        size_t operator() (const IndexedFaceContainer& g) const {
            return g.hash();
        }
  };

};

class FaceContainer {
public:
  Vector3f v1;
  Vector3f v2;
  Vector3f v3;

  FaceContainer(Vector3f& v1, Vector3f& v2, Vector3f& v3) {
    this->v1 = v1;
    this->v2 = v2;
    this->v3 = v3;
  }

  FaceContainer() {
  }

  size_t hash() const {
      size_t seed = 0;

      size_t pos1Hash = v1.hash();
      size_t pos2Hash = v2.hash();
      size_t pos3Hash = v3.hash();

      std::vector<size_t> facevecs;
      facevecs.push_back(pos1Hash);facevecs.push_back(pos2Hash);facevecs.push_back(pos3Hash);
      sort(facevecs.begin(), facevecs.end());

      seed = 0;
      boost::hash_combine(seed, facevecs[0]);
      boost::hash_combine(seed, facevecs[1]);
      boost::hash_combine(seed, facevecs[2]);

      return seed;
  }

  bool operator==(const FaceContainer&other)const {
    if (v1+v2+v3 != other.v1+other.v2+other.v3) return false;

    //There is a more elegant way to do this, but this is
    //easier to code and faster.
    if (v1 == other.v1) {
       if ((v2==other.v2 && v3==other.v3) ||
           (v2==other.v3 && v3==other.v2))
          return true;
    }

    if (v1 == other.v2) {
      if ((v2==other.v1 && v3==other.v3) ||
          (v2==other.v3 && v3==other.v1))
        return true;
    }

    if (v1 == other.v3) {
     if ((v2==other.v1 && v3==other.v2) ||
         (v2==other.v2 && v3==other.v1))
       return true;
    }

    return false;
  }

  class Hasher{
  public:
        size_t operator() (const FaceContainer& g) const {
            return g.hash();
        }
  };

};

class GeomContainer {
public:
  uint32 geomIdx;
  uint32 vertexIdx;

  GeomContainer(int g, int v) {
    geomIdx = g;
    vertexIdx = v;
  }

  GeomContainer() {
    geomIdx = vertexIdx = 0;
  }

  size_t hash() const {
      size_t seed = 0;
      boost::hash_combine(seed, geomIdx);
      boost::hash_combine(seed, vertexIdx);

      return seed;
  }

  bool operator==(const GeomContainer&other)const {
        return geomIdx==other.geomIdx && vertexIdx==other.vertexIdx;
  }

  class Hasher{
  public:
        size_t operator() (const GeomContainer& g) const {
            return g.hash();
        }
  };

};

class GeomPairContainer {
public:
  uint32 mGeomIdx;
  uint32 mVertexIdx1;
  uint32 mVertexIdx2;
  float64 mCost;
  Vector3f mReplacementVector;

  void init(uint32 g, uint32 v1, uint32 v2) {
    mGeomIdx = g;
    mVertexIdx1 = v1;
    mVertexIdx2 = v2;

    if (mVertexIdx1 > mVertexIdx2) {
      uint32 temp = mVertexIdx1;
      mVertexIdx1 = mVertexIdx2;
      mVertexIdx2 = temp;
    }

    mCost = 0;
  }

  GeomPairContainer(uint32 g, uint32 v1, uint32 v2) {
    init(g,v1,v2);
  }

  GeomPairContainer(uint32 g, uint32 v1, uint32 v2, float64 cost)  {
    init(g,v1,v2);
    mCost = cost;
  }

  GeomPairContainer() {
    mGeomIdx = mVertexIdx1 = mVertexIdx2 = 0;
    mCost = 0;
  }

  size_t hash() const {
      size_t seed = 0;
      boost::hash_combine(seed, mGeomIdx);
      boost::hash_combine(seed, mVertexIdx1);
      boost::hash_combine(seed, mVertexIdx2);

      return seed;
  }

  bool operator==(const GeomPairContainer&other)const {
    return mGeomIdx==other.mGeomIdx && mVertexIdx1==other.mVertexIdx1 && mVertexIdx2==other.mVertexIdx2;
  }

  bool operator<(const GeomPairContainer&other) const {

    if (*this == other) return false;

    if (mCost == other.mCost) {
      if (mGeomIdx == other.mGeomIdx) {
        if (mVertexIdx1 == other.mVertexIdx1) {
          assert(mVertexIdx2 != other.mVertexIdx2);
          return mVertexIdx2 < other.mVertexIdx2;
        }

        return mVertexIdx1 < other.mVertexIdx1;
      }

      return mGeomIdx < other.mGeomIdx;
    }

    return mCost < other.mCost;
  }

  class Hasher{
  public:
        size_t operator() (const GeomPairContainer& g) const {
            return g.hash();
        }
  };

};
bool custom_isnan (double data) {
#ifdef _WIN32
    return _isnan(data);
#else
    return std::isnan(data);
#endif
}

bool optimize(Matrix4x4d& Q, const Vector3f& v11, const Vector3f& v21, Vector3f& best) {
    Vector3d v1(v11.x,v11.y,v11.z);
    Vector3d v2(v21.x,v21.y,v21.z);

    ///First compute cost of contracting to endpoint
    Vector4d vbar4f (v1.x, v1.y, v1.z, 1);
    float64 cost1 = vbar4f[0]*vbar4f[0]*Q(0,0) + 2*vbar4f[0]*vbar4f[1]*Q(0,1) + 2*vbar4f[0]*vbar4f[2]*Q(0,2) + 2*vbar4f[0]*Q(0,3)
      + vbar4f[1]*vbar4f[1]*Q(1,1) + 2*vbar4f[1]*vbar4f[2]*Q(1,2) + 2*vbar4f[1]*Q(1,3)
      + vbar4f[2]*vbar4f[2]*Q(2,2) + 2*vbar4f[2]*Q(2,3)
      + Q(3,3);
    cost1 = (cost1 < 0.0) ? -cost1 : cost1;

    vbar4f = Vector4d (v2.x, v2.y, v2.z, 1);
    float64 cost2 = vbar4f[0]*vbar4f[0]*Q(0,0) + 2*vbar4f[0]*vbar4f[1]*Q(0,1) + 2*vbar4f[0]*vbar4f[2]*Q(0,2) + 2*vbar4f[0]*Q(0,3)
      + vbar4f[1]*vbar4f[1]*Q(1,1) + 2*vbar4f[1]*vbar4f[2]*Q(1,2) + 2*vbar4f[1]*Q(1,3)
      + vbar4f[2]*vbar4f[2]*Q(2,2) + 2*vbar4f[2]*Q(2,3)
      + Q(3,3);
    cost2 = (cost2 < 0.0) ? -cost2 : cost2;

    //Now find cost of contracting to an "optimal" vertex
    Vector3d d = v1 - v2;
    Matrix3x3d A(Vector3d(Q(0,0), Q(0,1), Q(0,2)),
                 Vector3d(Q(0,1), Q(1,1), Q(1,2)),
                 Vector3d(Q(0,2), Q(1,2), Q(2,2)),
                 ROWS());       //tensor of Q;

    Vector3d Av2 = A*v2;
    Vector3d Ad  = A*d;

    float64 denom = 2.0*(d.dot(Ad));
    if (   denom <= 1e-12 ) {
      if (cost2 < cost1) best = v21;
      else best = v11;

      return false;
    }

    Vector3d vec(Q(3,0),Q(3,1),Q(3,2))  ;
    double a =  ( -2.0*(vec.dot(d)) - (d.dot(Av2)) - (v2.dot(Ad)) ) / ( 2.0*(d.dot(Ad)) );

    if( a<0.0 ) a=0.0; else if( a>1.0 ) a=1.0;

    Vector3d best64 = a*d + v2;
    best.x = best64.x; best.y = best64.y; best.z = best64.z;

    //Optimal found: now find cost of contracting to it
    vbar4f = Vector4d (best.x, best.y, best.z, 1);
    float64 cost3 = vbar4f[0]*vbar4f[0]*Q(0,0) + 2*vbar4f[0]*vbar4f[1]*Q(0,1) + 2*vbar4f[0]*vbar4f[2]*Q(0,2) + 2*vbar4f[0]*Q(0,3)
      + vbar4f[1]*vbar4f[1]*Q(1,1) + 2*vbar4f[1]*vbar4f[2]*Q(1,2) + 2*vbar4f[1]*Q(1,3)
      + vbar4f[2]*vbar4f[2]*Q(2,2) + 2*vbar4f[2]*Q(2,3)
      + Q(3,3);

    if (cost1<cost2 && cost1<cost3){
      best = v11;
    }
    else if (cost2<cost1 && cost2<cost3){
      best = v21;
    }

    return true;
}

void computeCosts(Mesh::MeshdataPtr agg_mesh,
                  std::tr1::unordered_map<uint32, std::tr1::unordered_map<uint32, Matrix4x4d> >& submeshPositionQs,
                  std::set<GeomPairContainer>& vertexPairs,
                  std::tr1::unordered_map<GeomPairContainer, float64, GeomPairContainer::Hasher>& pairPriorities,
                  std::tr1::unordered_map<uint32, std::tr1::unordered_map<uint32, std::tr1::unordered_set<uint32> > >& submeshNeighborVertices,
                  std::tr1::unordered_map<uint32, std::tr1::unordered_map<uint32, std::vector<uint32> > >& vertexToFacesMap,
                  std::tr1::unordered_map<GeomPairContainer, uint32, GeomPairContainer::Hasher>& pairFrequency,
                  std::tr1::unordered_map<GeomContainer, uint8, GeomContainer::Hasher>& pairNonCollapsible
                 )
{

  for (uint32 i = 0;  i < agg_mesh->geometry.size(); i++) {

    SubMeshGeometry& curGeometry = agg_mesh->geometry[i];
    std::tr1::unordered_map<uint32, Matrix4x4d>& positionQs = submeshPositionQs[i];
    std::tr1::unordered_map<uint32, std::tr1::unordered_set<uint32> >& neighborVertices = submeshNeighborVertices[i];
    std::tr1::unordered_map<uint32, std::vector<uint32> >& submeshVertexToFacesMap = vertexToFacesMap[i];

    for (uint32 j = 0; j < curGeometry.positions.size(); j++) {

      if (neighborVertices.find(j) == neighborVertices.end()) continue;

      bool j_edgeVertex = (submeshVertexToFacesMap[j].size() == 1);

      const Vector3f& position = curGeometry.positions[j];

      Matrix4x4d Q = positionQs[j];

      std::tr1::unordered_set<uint32>& neighbors = neighborVertices[j];

      for (std::tr1::unordered_set<uint32>::iterator neighbor_it = neighbors.begin(); neighbor_it != neighbors.end(); neighbor_it++) {
        uint32 neighbor = *neighbor_it;

        bool neighbor_edgeVertex = (submeshVertexToFacesMap[neighbor].size() == 1);

        Vector3f best;

        const Vector3f& neighborPosition = curGeometry.positions[neighbor];
        Matrix4x4d Q = positionQs[j] + positionQs[neighbor];

        optimize(Q, position, neighborPosition, best);

        Vector4d vbar4f (best.x, best.y, best.z, 1);

        float64 cost = vbar4f[0]*vbar4f[0]*Q(0,0) + 2*vbar4f[0]*vbar4f[1]*Q(0,1) + 2*vbar4f[0]*vbar4f[2]*Q(0,2) + 2*vbar4f[0]*Q(0,3)
          + vbar4f[1]*vbar4f[1]*Q(1,1) + 2*vbar4f[1]*vbar4f[2]*Q(1,2) + 2*vbar4f[1]*Q(1,3)
          + vbar4f[2]*vbar4f[2]*Q(2,2) + 2*vbar4f[2]*Q(2,3)
          + Q(3,3);
        cost = (cost < 0.0) ? -cost : cost;

        uint32 idx1 = j, idx2 = neighbor;
        if (best == position) {
          uint32 temp = idx1;
          idx1 = idx2;
          idx2 = temp;
        }

        GeomPairContainer gpc(i, idx1, idx2);

        //Insert this cost as the cost of the edge if the edge hasnt been inserted
        //before or if this cost is less than the previously computed cost.
        if (pairPriorities[gpc] == -1 || (pairPriorities[gpc] != -1 && cost < pairPriorities[gpc]) ) {
          gpc.mCost = pairPriorities[gpc];
          vertexPairs.erase(gpc);

          gpc.mReplacementVector = best;

          gpc.mCost = cost;
          pairPriorities[gpc] = gpc.mCost;
          vertexPairs.insert(gpc);
        }
      }
    }
  }
}

inline uint32 findMappedVertex(std::tr1::unordered_map<int, int>& vertexMapping, uint32 idx) {
  uint32 startIdx = idx;

  int countIter = 0;
  while (vertexMapping.find(idx) != vertexMapping.end()) {
    countIter++;
    if ((int)idx == vertexMapping[idx]) {
      break;
    }
    idx = vertexMapping[idx];
  }

  if (countIter > 1) {
    vertexMapping[startIdx] = idx;
  }

  return idx;
}

void computeCosts(Mesh::MeshdataPtr agg_mesh, uint32 geomIdx, uint32 sourcePositionIdx, uint32 targetPositionIdx,
                  std::tr1::unordered_map<int, int>& vertexMapping,
                  std::tr1::unordered_map<uint32, std::tr1::unordered_map<uint32, Matrix4x4d> >& submeshPositionQs,
                  std::set<GeomPairContainer>& vertexPairs,
                  std::tr1::unordered_map<GeomPairContainer, float64, GeomPairContainer::Hasher>& pairPriorities,
                  std::tr1::unordered_map<uint32, std::tr1::unordered_map<uint32, std::tr1::unordered_set<uint32> > >& submeshNeighborVertices,
                  std::tr1::unordered_map<uint32, std::tr1::unordered_map<uint32, std::vector<uint32> > >& vertexToFacesMap,
                  std::tr1::unordered_map<GeomPairContainer, uint32, GeomPairContainer::Hasher>& pairFrequency,
                  std::tr1::unordered_map<GeomContainer, uint8, GeomContainer::Hasher>& pairNonCollapsible
                  )
{

  SubMeshGeometry& curGeometry = agg_mesh->geometry[geomIdx];
  std::tr1::unordered_map<uint32, Matrix4x4d>& positionQs = submeshPositionQs[geomIdx];
  std::tr1::unordered_map<uint32, std::tr1::unordered_set<uint32> >& neighborVertices = submeshNeighborVertices[geomIdx];
  std::tr1::unordered_map<uint32, std::vector<uint32> >& submeshVertexToFacesMap = vertexToFacesMap[geomIdx];

  targetPositionIdx = findMappedVertex(vertexMapping, targetPositionIdx);

  const Vector3f& position = curGeometry.positions[targetPositionIdx];

  std::tr1::unordered_set<uint32>& neighbors = neighborVertices[targetPositionIdx];

  for (std::tr1::unordered_set<uint32>::iterator neighbor_it = neighbors.begin(); neighbor_it != neighbors.end(); neighbor_it++) {
    uint32 neighborIdx = *neighbor_it;

    neighborIdx = findMappedVertex(vertexMapping, neighborIdx);

    if (neighborIdx == targetPositionIdx) continue;

    uint32 idx1 = targetPositionIdx;
    uint32 idx2 = neighborIdx;

    float64 cost = 0;
    const Vector3f& neighborPosition = curGeometry.positions[neighborIdx];
    Matrix4x4d Q = positionQs[targetPositionIdx] + positionQs[neighborIdx];

    Vector3f best;
    optimize(Q, position, neighborPosition, best);

    Vector4d vbar4f (best.x, best.y, best.z, 1);
    cost = vbar4f[0]*vbar4f[0]*Q(0,0) + 2*vbar4f[0]*vbar4f[1]*Q(0,1) + 2*vbar4f[0]*vbar4f[2]*Q(0,2) + 2*vbar4f[0]*Q(0,3)
            + vbar4f[1]*vbar4f[1]*Q(1,1) + 2*vbar4f[1]*vbar4f[2]*Q(1,2) + 2*vbar4f[1]*Q(1,3)
            + vbar4f[2]*vbar4f[2]*Q(2,2) + 2*vbar4f[2]*Q(2,3)
            + Q(3,3);
    cost = (cost < 0.0) ? -cost : cost;

    if (best == position) {
      uint32 temp = idx1;
      idx1 = idx2;
      idx2 = temp;
    }

    //Remove the previous vertex pair that existed before one vertex collapsed.
    GeomPairContainer gpc(geomIdx, sourcePositionIdx, neighborIdx);
    gpc.mCost = pairPriorities[gpc];
    vertexPairs.erase(gpc);
    gpc = GeomPairContainer(geomIdx, neighborIdx, sourcePositionIdx);
    gpc.mCost = pairPriorities[gpc];
    vertexPairs.erase(gpc);

    //Insert this vertex pair with the new cost, but first remove this pair if it already exists, then re-insert it.
    gpc = GeomPairContainer(geomIdx, idx1, idx2);
    gpc.mCost = pairPriorities[gpc];
    vertexPairs.erase(gpc);
    gpc = GeomPairContainer(geomIdx, idx2, idx1);
    gpc.mCost = pairPriorities[gpc];
    vertexPairs.erase(gpc);

    gpc = GeomPairContainer(geomIdx, idx1, idx2);
    gpc.mCost = pairPriorities[gpc];

    gpc.mCost = cost;
    gpc.mReplacementVector = best;
    pairPriorities[gpc] = gpc.mCost;

    vertexPairs.insert(gpc);
  }
}

bool MeshSimplifier::okToApplyStochastic(float totalInstances,
					 std::tr1::unordered_map<uint32, uint32>& submeshInstanceCount,
					 std::map<int, BoundingBox3f>& instanceToBBoxMap)
{
  if (instanceToBBoxMap.size() == 0) return false;

  bool canApplyStochastic = false;
  for (std::tr1::unordered_map<uint32, uint32>::iterator it = submeshInstanceCount.begin();
         it != submeshInstanceCount.end(); it++)
  {
      uint32 submeshIdx = it->first;
      float freq = it->second;

      if (freq/totalInstances >= 0.05 && freq >= 1000) {
        canApplyStochastic = true;
        break;
      }
  }

  return canApplyStochastic;
}

bool MeshSimplifier::applyStochastic(float totalInstances,
				   Mesh::MeshdataPtr agg_mesh,
				   int32 targetFaces,
				   std::tr1::unordered_map<uint32, uint32>& submeshInstanceCount,
				   std::tr1::unordered_map<int, std::tr1::unordered_map<int,int> >& vertexMapping1,
              			   std::map<int, BoundingBox3f>& instanceToBBoxMap
				   )
{
    //if it gets in here, I think it's ok to assume that
    //all submeshes have 4 or fewer triangles.

    std::set<uint32> nodesToRemove;
    for (std::tr1::unordered_map<uint32, uint32>::iterator it = submeshInstanceCount.begin();
         it != submeshInstanceCount.end(); it++)
    {
      uint32 submeshIdx = it->first;
      float freq = it->second;

      //Assuming 3 triangles per submesh, targetFaces/3.0 is the number of targetInstances.
      //So for the current submesh, # of faces included should be that number times
      //the ratio of instances of the current submesh.
      float numberIncluded = freq/totalInstances * (targetFaces/3.0);
      float percentIncluded = numberIncluded / freq * 100;

      //if this submesh is referenced by less than 5% of the total instances in the mesh
      //and it has fewer than 1000 instances, then just keep it.
      if (freq/totalInstances < 0.05 && freq < 1000) {
        continue;
      }

      //First determine which instances can be immediately eliminated because
      //they cannot be inflated, or otherwise, go outside bounding box.
      std::set<uint32> nodesTooBig;
      for (uint32 i = 0; i < agg_mesh->nodes.size(); i++) {
        const GeometryInstance& geomInstance = agg_mesh->instances[i];
        int geomIdx = geomInstance.geometryIndex;

        if (geomIdx == submeshIdx) {

          Node& node = agg_mesh->nodes[i];
          SubMeshGeometry& curGeometry = agg_mesh->geometry[geomIdx];
          std::tr1::unordered_map<int, int>& vertexMapping = vertexMapping1[geomIdx];
          assert( instanceToBBoxMap.find(i) != instanceToBBoxMap.end());
          BoundingBox3f& bbox = instanceToBBoxMap[i];

          float currentScale = sqrtf(100.0/percentIncluded);
          Matrix4x4f transform = node.transform * Matrix4x4f::scale(currentScale);
          bool transformOK = true;

	  for (uint32 j = 0; j < curGeometry.primitives.size() && transformOK; j++) {
              if (curGeometry.primitives[j].primitiveType != SubMeshGeometry::Primitive::TRIANGLES) continue;

              for (uint32 k = 0; k+2 < curGeometry.primitives[j].indices.size() && transformOK; k+=3) {
                unsigned short idx = curGeometry.primitives[j].indices[k];
                unsigned short idx2 = curGeometry.primitives[j].indices[k+1];
                unsigned short idx3 = curGeometry.primitives[j].indices[k+2];

                idx = findMappedVertex(vertexMapping, idx);
                idx2 = findMappedVertex(vertexMapping, idx2);
                idx3 = findMappedVertex(vertexMapping, idx3);

                Vector3f pos1 = curGeometry.positions[idx];
                Vector3f pos2 = curGeometry.positions[idx2];
                Vector3f pos3 = curGeometry.positions[idx3];

                Vector4f npos1 = node.transform * Vector4f(pos1.x, pos1.y, pos1.z, 1);
                Vector4f npos2 = node.transform * Vector4f(pos2.x, pos2.y, pos2.z, 1);
                Vector4f npos3 = node.transform * Vector4f(pos3.x, pos3.y, pos3.z, 1);

                pos1.x = npos1.x; pos1.y = npos1.y; pos1.z = npos1.z;
                pos2.x = npos2.x; pos2.y = npos2.y; pos2.z = npos2.z;
                pos3.x = npos3.x; pos3.y = npos3.y; pos3.z = npos3.z;

                if (!bbox.contains(pos1) ) {
                  bbox.mergeIn(pos1);
                }
                if (!bbox.contains(pos2) ) {
                  bbox.mergeIn(pos2);
                }
                if (!bbox.contains(pos3) ) {
                  bbox.mergeIn(pos3);
                }
                pos1 = curGeometry.positions[idx];
                pos2 = curGeometry.positions[idx2];
                pos3 = curGeometry.positions[idx3];

                Vector4f tpos1 = transform * Vector4f(pos1.x, pos1.y, pos1.z, 1);
                Vector4f tpos2 = transform * Vector4f(pos2.x, pos2.y, pos2.z, 1);
                Vector4f tpos3 = transform * Vector4f(pos3.x, pos3.y, pos3.z, 1);

                pos1.x = tpos1.x; pos1.y = tpos1.y; pos1.z = tpos1.z;
                pos2.x = tpos2.x; pos2.y = tpos2.y; pos2.z = tpos2.z;
                pos3.x = tpos3.x; pos3.y = tpos3.y; pos3.z = tpos3.z;

                if (pos1 != pos2 && pos2 != pos3 && pos1 != pos3) {
                  //std::cout << pos1 << " " << pos2 <<" "<< pos3 << "  " <<  bbox <<" " << currentScale  << "\n";
                  if (!bbox.contains(pos1, 0.1)  || !bbox.contains(pos2, 0.1) || !bbox.contains(pos3, 0.1)) {
                    transformOK = false;
                    nodesTooBig.insert(i);
                  }
                }
              }
          }
        }
      }

      if (nodesTooBig.size() >= freq - numberIncluded && nodesTooBig.size() > 0) {
        std::deque<int> mydeque;
        for (std::set<uint32>::iterator it = nodesTooBig.begin(); it != nodesTooBig.end(); it++) {
          mydeque.push_back(*it);
        }
        while (nodesTooBig.size() >= freq - numberIncluded && nodesTooBig.size() > 0) {
          int rand_pos = rand() % nodesTooBig.size();
          nodesTooBig.erase(mydeque[rand_pos]);
          mydeque.erase(mydeque.begin() + rand_pos);
        }
      }

      //nodesTooBig.clear();
      float newPercentIncluded = numberIncluded/(freq - nodesTooBig.size()) * 100.0;
      //std::cout << newPercentIncluded << " : " << percentIncluded << " " << nodesTooBig.size() << " " << numberIncluded << "  " << freq <<  " pcincl\n";

      for (uint32 i = 0; i < agg_mesh->nodes.size(); i++) {
        const GeometryInstance& geomInstance = agg_mesh->instances[i];
        int geomIdx = geomInstance.geometryIndex;

        if (geomIdx == submeshIdx) {
          if (nodesTooBig.find(i) != nodesTooBig.end()) {
            nodesToRemove.insert(i);
            continue;
          }

          if (rand() % 100 >= newPercentIncluded) {
            nodesToRemove.insert(i);
            continue;
          }

          Node& node = agg_mesh->nodes[i];
          SubMeshGeometry& curGeometry = agg_mesh->geometry[geomIdx];
          std::tr1::unordered_map<int, int>& vertexMapping = vertexMapping1[geomIdx];
          assert( instanceToBBoxMap.find(i) != instanceToBBoxMap.end());
          BoundingBox3f& bbox = instanceToBBoxMap[i];

          float currentScale = sqrtf(100.0/percentIncluded);
          Matrix4x4f transform = node.transform * Matrix4x4f::scale(currentScale);
          bool transformOK = false;

          while (!transformOK) {
            transformOK = true;  break;
            if (currentScale < 1) {
              currentScale = 1;
              transform = node.transform;
              break;
            }

	    for (uint32 j = 0; j < curGeometry.primitives.size() && transformOK; j++) {
	      if (curGeometry.primitives[j].primitiveType != SubMeshGeometry::Primitive::TRIANGLES) continue;

	      for (uint32 k = 0; k+2 < curGeometry.primitives[j].indices.size(); k+=3) {
		unsigned short idx = curGeometry.primitives[j].indices[k];
		unsigned short idx2 = curGeometry.primitives[j].indices[k+1];
		unsigned short idx3 = curGeometry.primitives[j].indices[k+2];

		idx = findMappedVertex(vertexMapping, idx);
		idx2 = findMappedVertex(vertexMapping, idx2);
		idx3 = findMappedVertex(vertexMapping, idx3);

		Vector3f pos1 = curGeometry.positions[idx];
		Vector3f pos2 = curGeometry.positions[idx2];
		Vector3f pos3 = curGeometry.positions[idx3];

                Vector4f npos1 = node.transform * Vector4f(pos1.x, pos1.y, pos1.z, 1);
                Vector4f npos2 = node.transform * Vector4f(pos2.x, pos2.y, pos2.z, 1);
                Vector4f npos3 = node.transform * Vector4f(pos3.x, pos3.y, pos3.z, 1);

                pos1.x = npos1.x; pos1.y = npos1.y; pos1.z = npos1.z;
                pos2.x = npos2.x; pos2.y = npos2.y; pos2.z = npos2.z;
                pos3.x = npos3.x; pos3.y = npos3.y; pos3.z = npos3.z;

                if (!bbox.contains(pos1) ) {
                  bbox.mergeIn(pos1);
                }
                if (!bbox.contains(pos2) ) {
                  bbox.mergeIn(pos2);
                }
                if (!bbox.contains(pos3) ) {
                  bbox.mergeIn(pos3);
                }

                pos1 = curGeometry.positions[idx];
                pos2 = curGeometry.positions[idx2];
                pos3 = curGeometry.positions[idx3];

                Vector4f tpos1 = transform * Vector4f(pos1.x, pos1.y, pos1.z, 1);
                Vector4f tpos2 = transform * Vector4f(pos2.x, pos2.y, pos2.z, 1);
                Vector4f tpos3 = transform * Vector4f(pos3.x, pos3.y, pos3.z, 1);

                pos1.x = tpos1.x; pos1.y = tpos1.y; pos1.z = tpos1.z;
                pos2.x = tpos2.x; pos2.y = tpos2.y; pos2.z = tpos2.z;
                pos3.x = tpos3.x; pos3.y = tpos3.y; pos3.z = tpos3.z;

		if (pos1 != pos2 && pos2 != pos3 && pos1 != pos3) {
                  //std::cout << pos1 << " " << pos2 <<" "<< pos3 << "  " <<  bbox <<" " << currentScale  << "\n";

		  if (!bbox.contains(pos1, 0.1)  || !bbox.contains(pos2, 0.1) || !bbox.contains(pos3, 0.1)) {
                    transformOK = false;
                    //std::cout << "Not contains\n";
                    break;
                  }
                  //std::cout << "Contains\n";
		}
	      }
	    }
            if (!transformOK) {
              std::cout << "currentScale reducing from " << currentScale << " to " << currentScale/1.5 << "\n";
              currentScale = currentScale / 1.50;
              transform = node.transform * Matrix4x4f::scale(currentScale);
            }
          }
          node.transform = transform;
          std::cout << currentScale << " : currentScale\n";
        }
      }
    }

    NodeList newNodeList;
    NodeIndexList newRootNodesList;
    GeometryInstanceList newInstanceList;

    NodeList& nodes = agg_mesh->nodes;
    NodeIndexList& rootNodes = agg_mesh->rootNodes;
    GeometryInstanceList& instanceList = agg_mesh->instances;
    for (uint32 i = 0; i < nodes.size(); i++) {
      if (nodesToRemove.find(i) == nodesToRemove.end() )
      {
	NodeIndex geom_node_idx = newNodeList.size();
	newNodeList.push_back(nodes[i]);
	newRootNodesList.push_back(geom_node_idx);
	GeometryInstance geomInstance = instanceList[i];
	geomInstance.parentNode = geom_node_idx;
	newInstanceList.push_back(geomInstance);
      }
    }
    agg_mesh->nodes = newNodeList;
    agg_mesh->rootNodes = newRootNodesList;
    agg_mesh->instances = newInstanceList;
}

void MeshSimplifier::simplify(Mesh::MeshdataPtr agg_mesh, int32 numFacesLeft) {
  std::map<int, BoundingBox3f> emptyMap;
  return simplify(agg_mesh, numFacesLeft, emptyMap );
}

void MeshSimplifier::simplify(Mesh::MeshdataPtr agg_mesh,
			      int32 targetFaces,
			      std::map<int, BoundingBox3f>& instanceToBBoxMap)
{
  std::tr1::unordered_map<uint32, uint32> submeshInstanceCount;

  int countFaces = 0;

  uint32 geoinst_idx;
  Matrix4x4f geoinst_pos_xform;
  Meshdata::GeometryInstanceIterator geoinst_it = agg_mesh->getGeometryInstanceIterator();

  std::set<GeomPairContainer> vertexPairs;
  std::tr1::unordered_map<GeomPairContainer, float64, GeomPairContainer::Hasher> pairPriorities;
  std::tr1::unordered_map<GeomPairContainer, uint32, GeomPairContainer::Hasher> pairFrequency;

  std::tr1::unordered_map<uint32, std::tr1::unordered_map<uint32, Matrix4x4d> > submeshPositionQs;

  std::tr1::unordered_map<uint32, std::tr1::unordered_map<uint32, std::tr1::unordered_set<uint32> > > submeshNeighborVertices;

  bool meshChangedDuringPreprocess = false;

  //variables for stochastic simplification
  std::tr1::unordered_map<uint32, uint32> numTrianglesInSubmesh;
  float totalInstances = 0;
  std::map<uint32, BoundingBox3f> submeshBBoxAreas;
  std::map<uint32, double> submeshTriangleAreas;

  /* Make every index in prims specification point to the earliest occurrence of the corresponding position vector */
  for (uint32 i = 0; i < agg_mesh->geometry.size(); i++) {
    SubMeshGeometry& curGeometry = agg_mesh->geometry[i];

    std::tr1::unordered_map<Vector3f, uint32, Vector3f::Hasher> firstPositionMap;

    std::tr1::unordered_set<uint32> deletedIndices;
    for (uint32 j = 0; j < curGeometry.primitives.size(); j++) {
      SubMeshGeometry::Primitive& primitive = curGeometry.primitives[j];

      for (uint32 k = 0; k+2 < primitive.indices.size(); k+=3) {
        unsigned short idx = primitive.indices[k];
        unsigned short idx2 = primitive.indices[k+1];
        unsigned short idx3 = primitive.indices[k+2];

        Vector3f& pos1 = curGeometry.positions[idx];
        Vector3f& pos2 = curGeometry.positions[idx2];
        Vector3f& pos3 = curGeometry.positions[idx3];

        if (firstPositionMap.find(pos1) == firstPositionMap.end()) {
          firstPositionMap[pos1] = idx;
        }
        else {
          if (idx != firstPositionMap[pos1]) {
            primitive.indices[k] = firstPositionMap[pos1];
            deletedIndices.insert(idx);
          }
        }

        if (firstPositionMap.find(pos2) == firstPositionMap.end()) {
          firstPositionMap[pos2] = idx2;
        }
        else {
           if (idx2 != firstPositionMap[pos2]) {
             primitive.indices[k+1] = firstPositionMap[pos2];
             deletedIndices.insert(idx2);
           }
        }

        if (firstPositionMap.find(pos3) == firstPositionMap.end()) {
          firstPositionMap[pos3] = idx3;
        }
        else {
          if (idx3 != firstPositionMap[pos3]) {
            primitive.indices[k+2] = firstPositionMap[pos3];
            deletedIndices.insert(idx3);
          }
        }
      }
    }

    for (std::tr1::unordered_set<uint32>::iterator it = deletedIndices.begin();
          it != deletedIndices.end(); it++)
    {
      curGeometry.positions[*it] = SIMPLIFIER_INVALID_VECTOR;
    }

    if (deletedIndices.size() > 0) {
      meshChangedDuringPreprocess = true;
    }
  }

  std::tr1::unordered_map<uint32, std::vector<IndexedFaceContainer> > geomFacesList;
  std::tr1::unordered_map<uint32, std::tr1::unordered_map<uint32, std::vector<uint32> > > vertexToFacesMap;

  /* Identify the non-unique faces in the geometry. Insert the vertex pairs */
  for (uint32 i = 0; i < agg_mesh->geometry.size(); i++) {
    SubMeshGeometry& curGeometry = agg_mesh->geometry[i];

    std::tr1::unordered_map<uint32, std::tr1::unordered_set<uint32> >& neighborVertices = submeshNeighborVertices[i];
    std::tr1::unordered_map<uint32, std::vector<uint32> >& geomsVertexToFacesMap = vertexToFacesMap[i];
    std::vector<IndexedFaceContainer>& facesList = geomFacesList[i];

    std::tr1::unordered_map<uint32, Matrix4x4d>& positionQs = submeshPositionQs[i];

    std::tr1::unordered_set<IndexedFaceContainer, IndexedFaceContainer::Hasher> duplicateFaces;
    for (uint32 j = 0; j < curGeometry.primitives.size(); j++) {
        SubMeshGeometry::Primitive& primitive = curGeometry.primitives[j];

        for (uint32 k = 0; k+2 < primitive.indices.size(); k+=3) {
          unsigned short idx = primitive.indices[k];
          unsigned short idx2 = primitive.indices[k+1];
          unsigned short idx3 = primitive.indices[k+2];

          if (idx == idx2 || idx == idx3 || idx2 == idx3)
            continue;

          Vector3f& pos1 = curGeometry.positions[idx];
          Vector3f& pos2 = curGeometry.positions[idx2];
          Vector3f& pos3 = curGeometry.positions[idx3];

          IndexedFaceContainer origface(idx, idx2, idx3);

          if (duplicateFaces.find(origface) != duplicateFaces.end()) {
            primitive.indices[k] = USHRT_MAX;
            primitive.indices[k+1] = USHRT_MAX;
            primitive.indices[k+2] = USHRT_MAX;
            meshChangedDuringPreprocess = true;

            continue;
          }

          GeomPairContainer gpc1(i, idx, idx2, -1);
          GeomPairContainer gpc2(i, idx2, idx3, -1);
          GeomPairContainer gpc3(i, idx, idx3, -1);

          neighborVertices[idx].insert(idx2);
          neighborVertices[idx].insert(idx3);
          neighborVertices[idx2].insert(idx);
          neighborVertices[idx2].insert(idx3);
          neighborVertices[idx3].insert(idx);
          neighborVertices[idx3].insert(idx2);

          vertexPairs.insert(gpc1);
          pairPriorities[gpc1] = -1;
          pairFrequency[gpc1]++;

          vertexPairs.insert(gpc2);
          pairPriorities[gpc2] = -1;
          pairFrequency[gpc2]++;

          vertexPairs.insert(gpc3);
          pairPriorities[gpc3] = -1;
          pairFrequency[gpc3]++;

          duplicateFaces.insert(origface);

          IndexedFaceContainer ifc(idx, idx2, idx3);
          uint32 faceIndex = facesList.size();
          facesList.push_back(ifc);

          geomsVertexToFacesMap[idx].push_back(faceIndex);
          geomsVertexToFacesMap[idx2].push_back(faceIndex);
          geomsVertexToFacesMap[idx3].push_back(faceIndex);
        }
    }
  }

  std::tr1::unordered_map<GeomContainer, uint8, GeomContainer::Hasher> pairNonCollapsible;
  for (uint32 i = 0; i < agg_mesh->geometry.size(); i++) {
    SubMeshGeometry& curGeometry = agg_mesh->geometry[i];

    for (uint32 j = 0; j < curGeometry.primitives.size(); j++) {
      SubMeshGeometry::Primitive& primitive = curGeometry.primitives[j];

      for (uint32 k = 0; k+2 < primitive.indices.size(); k+=3) {
        unsigned short idx = primitive.indices[k];
        unsigned short idx2 = primitive.indices[k+1];
        unsigned short idx3 = primitive.indices[k+2];

        if (idx == USHRT_MAX && idx2 == USHRT_MAX && idx3 == USHRT_MAX) {
          continue;
        }

        if (idx == idx2 || idx == idx3 || idx2 == idx3)          continue;

        Vector3f& pos1 = curGeometry.positions[idx];
        Vector3f& pos2 = curGeometry.positions[idx2];
        Vector3f& pos3 = curGeometry.positions[idx3];

        GeomPairContainer gpc1(i, idx, idx2, -1);
        GeomPairContainer gpc2(i, idx2, idx3, -1);
        GeomPairContainer gpc3(i, idx, idx3, -1);

        if (pairFrequency[gpc1]==1 || pairFrequency[gpc2]==1 || pairFrequency[gpc3]==1) {
          pairNonCollapsible[GeomContainer(i,idx)] = 1;
          pairNonCollapsible[GeomContainer(i,idx2)] = 1;
          pairNonCollapsible[GeomContainer(i,idx3)] = 1;
        }
      }
    }
  }

  //Find the list of instances associated with each submesh
  countFaces = 0;

  while( geoinst_it.next(&geoinst_idx, &geoinst_pos_xform) ) {
    const GeometryInstance& geomInstance = agg_mesh->instances[geoinst_idx];
    Matrix4x4f geoinst_pos_xform_transpose = geoinst_pos_xform.transpose();
    Matrix4x4d transform;
    for (int row=0; row<4; row++) {
      for (int col=0; col<4; col++) {
        transform(row,col) = geoinst_pos_xform(row,col);
      }
    }

    int geomIdx = geomInstance.geometryIndex;
    submeshInstanceCount[geomIdx]++;
    totalInstances++;

    SubMeshGeometry& curGeometry = agg_mesh->geometry[geomIdx];
    std::tr1::unordered_map<uint32, Matrix4x4d>& positionQs = submeshPositionQs[geomIdx];

    numTrianglesInSubmesh[geomIdx] = 0;
    for (uint32 j = 0; j < curGeometry.primitives.size(); j++) {
      SubMeshGeometry::Primitive& primitive = curGeometry.primitives[j];

      for (uint32 k = 0; k+2 < primitive.indices.size(); k+=3) {
        unsigned short idx = primitive.indices[k];
        unsigned short idx2 = primitive.indices[k+1];
        unsigned short idx3 = primitive.indices[k+2];

        if (idx == USHRT_MAX && idx2 == USHRT_MAX && idx3 == USHRT_MAX) {
          continue;
        }

        if (idx == idx2 || idx == idx3 || idx2 == idx3) continue;

        countFaces++;
        numTrianglesInSubmesh[geomIdx]++;

        Vector3d orig_pos1 (curGeometry.positions[idx].x, curGeometry.positions[idx].y,
                           curGeometry.positions[idx].z);
        Vector3d orig_pos2 (curGeometry.positions[idx2].x, curGeometry.positions[idx2].y,
                            curGeometry.positions[idx2].z);
        Vector3d orig_pos3 (curGeometry.positions[idx3].x, curGeometry.positions[idx3].y,
                            curGeometry.positions[idx3].z);

        Vector3d pos1 = transform * orig_pos1;
        Vector3d pos2 = transform * orig_pos2;
        Vector3d pos3 = transform * orig_pos3;

        Vector3d normal = (pos2 - pos1).cross(pos3-pos1);
        normal = normal.normal();
        float64 A = normal[0];
        float64 B = normal[1];
        float64 C = normal[2];
        float64 D = -(normal.dot(pos1));

        Matrix4x4d Qmat ( Vector4d(A*A, A*B, A*C, A*D),
                          Vector4d(A*B, B*B, B*C, B*D),
                          Vector4d(A*C, B*C, C*C, C*D),
                          Vector4d(A*D, B*D, C*D, D*D), Matrix4x4d::ROWS() );

        Qmat = transform.transpose() * Qmat * transform;

        float64 face_area = (pos1-pos2).cross(pos1-pos3).length() * ((double)0.5);
        Qmat *= face_area;

        positionQs[idx] += Qmat;
        positionQs[idx2] += Qmat;
        positionQs[idx3] += Qmat;

        GeomPairContainer gpc1(geomIdx, idx, idx2, -1);
        GeomPairContainer gpc2(geomIdx, idx3, idx2, -1);
        GeomPairContainer gpc3(geomIdx, idx, idx3, -1);

        //Handle boundary edges adding the perpendicular constraint plane.
        //Implementation could be much much better though.
        if (pairFrequency[gpc1] == 1) {
          Vector3d org = pos1, dest = pos2;
          Vector3d e = dest - org;
          Vector3d constraint = e.cross(normal);
          constraint = constraint.normal();

          float64 A = constraint[0];
          float64 B = constraint[1];
          float64 C = constraint[2];
          float64 D = -(constraint.dot(org));

          Matrix4x4d Qmat ( Vector4d(A*A, A*B, A*C, A*D),
                          Vector4d(A*B, B*B, B*C, B*D),
                          Vector4d(A*C, B*C, C*C, C*D),
                          Vector4d(A*D, B*D, C*D, D*D), Matrix4x4d::ROWS() );
          //Qmat*=100.0;
          Qmat*=e.lengthSquared();

          Qmat=transform.transpose()*Qmat*transform;
          positionQs[idx]+=Qmat;
          positionQs[idx2]+=Qmat;
        }
        if (pairFrequency[gpc2] == 1) {
          Vector3d org = pos3, dest = pos2;
          Vector3d e = dest - org;
          Vector3d constraint = e.cross(normal);
          constraint = constraint.normal();

          float64 A = constraint[0];
          float64 B = constraint[1];
          float64 C = constraint[2];
          float64 D = -(constraint.dot(org));

          Matrix4x4d Qmat ( Vector4d(A*A, A*B, A*C, A*D),
                          Vector4d(A*B, B*B, B*C, B*D),
                          Vector4d(A*C, B*C, C*C, C*D),
                          Vector4d(A*D, B*D, C*D, D*D), Matrix4x4d::ROWS() );
          //Qmat*=100.0;
          Qmat*=e.lengthSquared();

          Qmat=transform.transpose()*Qmat*transform;
          positionQs[idx3]+=Qmat;
          positionQs[idx2]+=Qmat;

        }
        if (pairFrequency[gpc3] == 1) {
          Vector3d org = pos1, dest = pos3;
          Vector3d e = dest - org;
          Vector3d constraint = e.cross(normal);
          constraint = constraint.normal();

          float64 A = constraint[0];
          float64 B = constraint[1];
          float64 C = constraint[2];
          float64 D = -(constraint.dot(org));

          Matrix4x4d Qmat ( Vector4d(A*A, A*B, A*C, A*D),
                          Vector4d(A*B, B*B, B*C, B*D),
                          Vector4d(A*C, B*C, C*C, C*D),
                          Vector4d(A*D, B*D, C*D, D*D), Matrix4x4d::ROWS() );
          //Qmat*=100.0;
          Qmat*=e.lengthSquared();

          Qmat=transform.transpose()*Qmat*transform;
          positionQs[idx]+=Qmat;
          positionQs[idx3]+=Qmat;
        }

      }
    }
  }

  targetFaces = countFaces / 5.0;
  SIMPLIFY_LOG(warn, "countFaces = " << countFaces);
  SIMPLIFY_LOG(warn, "targetFaces = " << targetFaces);
  if (targetFaces < countFaces) {
      SIMPLIFY_LOG(warn, "targetFaces < countFaces: Simplification needed");
      computeCosts(agg_mesh, submeshPositionQs, vertexPairs, pairPriorities, submeshNeighborVertices, vertexToFacesMap, pairFrequency, pairNonCollapsible);
  }
  else if (!meshChangedDuringPreprocess) {
    return;
  }

  bool canApplyStochastic = okToApplyStochastic(totalInstances, submeshInstanceCount, instanceToBBoxMap);

  std::tr1::unordered_map<int, std::tr1::unordered_map<int,int>  > vertexMapping1;
  //Do the actual edge collapses.
  while (countFaces > targetFaces && vertexPairs.size() > 0) {
    GeomPairContainer top = *(vertexPairs.begin());

    int i = top.mGeomIdx;

    //Don't simplify if number of triangles in submesh <= 4.
    if (canApplyStochastic && numTrianglesInSubmesh[i] <= 4) {
      vertexPairs.erase(top);
      pairPriorities.erase(top);
      continue;
    }

    uint32 targetIdx = top.mVertexIdx1;
    uint32 sourceIdx = top.mVertexIdx2;

    SubMeshGeometry& curGeometry = agg_mesh->geometry[i];
    std::tr1::unordered_map<int, int>& vertexMapping = vertexMapping1[i];

    targetIdx = findMappedVertex(vertexMapping, targetIdx);
    sourceIdx = findMappedVertex(vertexMapping, sourceIdx);

    Vector3f& targetPos = curGeometry.positions[targetIdx];
    Vector3f& sourcePos = curGeometry.positions[sourceIdx];

    //Collapse vertex at sourceIdx into targetIdx.
    if (targetIdx != sourceIdx && targetPos != sourcePos) {
      vertexMapping[sourceIdx] = targetIdx;

      std::tr1::unordered_map<uint32, std::vector<uint32> >& geomsVertexToFacesMap = vertexToFacesMap[i];
      std::vector<uint32>& ifcVector = geomsVertexToFacesMap[sourceIdx];
      std::vector<IndexedFaceContainer>& facesList = geomFacesList[i];

      //count how many faces get invalidated and degenerate because of this edge collapse.
      for (uint32 ifcVectorIdx = 0; ifcVectorIdx < ifcVector.size(); ifcVectorIdx++) {
        uint32 faceIndex = ifcVector[ifcVectorIdx];
        IndexedFaceContainer& ifc = facesList[faceIndex];

        if (!ifc.valid) continue;

        unsigned short vidx = ifc.idx1;
        unsigned short vidx2 = ifc.idx2;
        unsigned short vidx3 = ifc.idx3;

        vidx = findMappedVertex(vertexMapping, vidx);
        vidx2 = findMappedVertex(vertexMapping, vidx2);
        vidx3 = findMappedVertex(vertexMapping, vidx3);

        if (vidx == vidx2 || vidx2 == vidx3 || vidx == vidx3) {
          //degenerate face; invalidate it.
          countFaces -= submeshInstanceCount[i];
          ifc.valid = false;
        }
        else {
          //add this face to the neighbors of the target vertex.
          geomsVertexToFacesMap[targetIdx].push_back(faceIndex);
        }
      }
      //done invalidating degenerate faces.

      //Now update the neighbors of the target vertex and its quadric matrix.
      std::tr1::unordered_set<uint32>& sourceNeighborVertices =  submeshNeighborVertices[i][sourceIdx];
      std::tr1::unordered_set<uint32>& targetNeighborVertices =  submeshNeighborVertices[i][targetIdx];
      std::tr1::unordered_map<uint32, Matrix4x4d>& positionQs = submeshPositionQs[i];
      for (std::tr1::unordered_set<uint32>::iterator neighbor_it = sourceNeighborVertices.begin();
           neighbor_it != sourceNeighborVertices.end(); neighbor_it++)
      {
        uint32 neighborIdx = *neighbor_it;

        if (neighborIdx != targetIdx) {
          targetNeighborVertices.insert(neighborIdx);
        }
      }

      positionQs[ targetIdx ] += positionQs[sourceIdx];

      curGeometry.positions[targetIdx] = top.mReplacementVector;
      //Finally recompute the costs of the neighbors.
      computeCosts(agg_mesh, i, sourceIdx, targetIdx, vertexMapping,
                   submeshPositionQs, vertexPairs, pairPriorities, submeshNeighborVertices,
                   vertexToFacesMap, pairFrequency, pairNonCollapsible);
    }

    vertexPairs.erase(top);
    pairPriorities.erase(top);
  }

  if (canApplyStochastic &&
      countFaces > targetFaces * 1.1 &&
      countFaces > targetFaces + 10 )
  {
    applyStochastic(totalInstances, agg_mesh, targetFaces,
		    submeshInstanceCount, vertexMapping1, instanceToBBoxMap);
  }

  //remove unused vertices; get new mapping from previous vertex indices to new vertex indices in vertexMapping2;
  std::tr1::unordered_map<int, std::tr1::unordered_map<int,int> > vertexMapping2;

  //Remove vertices no longer used in the simplified mesh.
  for (uint32 i = 0; i < agg_mesh->geometry.size(); i++) {
    SubMeshGeometry& curGeometry = agg_mesh->geometry[i];
    std::tr1::unordered_map<int, int>& vertexMapping = vertexMapping1[i];
    std::tr1::unordered_map<int, int>& oldToNewMap = vertexMapping2[i];

    std::vector<Sirikata::Vector3f> positions;
    std::vector<Sirikata::Vector3f> normals;
    std::vector<SubMeshGeometry::TextureSet>texUVs;

    for (uint32 j = 0; j < curGeometry.texUVs.size(); j++) {
      SubMeshGeometry::TextureSet ts;
      ts.stride = curGeometry.texUVs[j].stride;
      texUVs.push_back(ts);
    }

    std::tr1::unordered_map<Vector3f, uint32, Vector3f::Hasher> vector3fSet;

    for (uint32 j = 0 ; j < curGeometry.positions.size() ; j++) {
      if (vertexMapping.find(j) == vertexMapping.end()) {

        if (curGeometry.positions[j] == SIMPLIFIER_INVALID_VECTOR) {
          continue;
        }

        if (vector3fSet.find(curGeometry.positions[j]) != vector3fSet.end()) {
          oldToNewMap[j] = vector3fSet[ curGeometry.positions[j] ];
          continue;
        }

        oldToNewMap[j] = positions.size();

        vector3fSet[ curGeometry.positions[j] ] = positions.size();

        positions.push_back(curGeometry.positions[j]);

        if (j < curGeometry.normals.size())
          normals.push_back(curGeometry.normals[j]);

        for (uint32 k = 0; k < curGeometry.texUVs.size(); k++) {
          unsigned int stride = curGeometry.texUVs[k].stride;
          if (stride*j < curGeometry.texUVs[k].uvs.size()) {
            uint32 idx = stride * j;
            while ( idx < stride*j+stride){
              texUVs[k].uvs.push_back(curGeometry.texUVs[k].uvs[idx]);
              idx++;
            }
          }
        }
      }
    }

    curGeometry.positions = positions;
    curGeometry.normals = normals;
    curGeometry.texUVs = texUVs;
  }

  //Now adjust the primitives to point to the new indexes of the submesh geometry vertices.
  std::tr1::unordered_map<uint32, std::tr1::unordered_map<uint32, std::vector<unsigned short> > > newIndices;

  //First create new indices from all the non-degenerate faces.
  for (uint32 i = 0; i < agg_mesh->geometry.size(); i++) {
    SubMeshGeometry& curGeometry = agg_mesh->geometry[i];

    std::tr1::unordered_map<int, int>& vertexMapping = vertexMapping1[i];
    std::tr1::unordered_map<int, int>& oldToNewMap = vertexMapping2[i];

    for (uint32 j = 0; j < curGeometry.primitives.size(); j++) {
      if (curGeometry.primitives[j].primitiveType != SubMeshGeometry::Primitive::TRIANGLES) continue;

      std::vector<unsigned short>& indices = newIndices[i][j];

      for (uint32 k = 0; k+2 < curGeometry.primitives[j].indices.size(); k+=3) {

        unsigned short idx = curGeometry.primitives[j].indices[k];
        unsigned short idx2 = curGeometry.primitives[j].indices[k+1];
        unsigned short idx3 = curGeometry.primitives[j].indices[k+2];

        if (idx == USHRT_MAX && idx2 == USHRT_MAX && idx3 == USHRT_MAX) {
          continue;
        }

        unsigned short midx = findMappedVertex(vertexMapping, idx);
        unsigned short midx2 = findMappedVertex(vertexMapping, idx2);
        unsigned short midx3 = findMappedVertex(vertexMapping, idx3);

        idx = findMappedVertex(vertexMapping, idx);
        idx2 = findMappedVertex(vertexMapping, idx2);
        idx3 = findMappedVertex(vertexMapping, idx3);

        if (idx != idx2 && idx2 != idx3 && idx != idx3) {
          indices.push_back(oldToNewMap[idx]);
          indices.push_back(oldToNewMap[idx2]);
          indices.push_back(oldToNewMap[idx3]);
        }
      }
    }
  }

  //Now set the new indices.
  for (uint32 i = 0; i < agg_mesh->geometry.size(); i++) {
    SubMeshGeometry& curGeometry = agg_mesh->geometry[i];

    for (uint32 j = 0 ; j < curGeometry.primitives.size() ; j++) {
      curGeometry.primitives[j].indices = newIndices[i][j];
    }
  }
}

}

}


// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)

#include "base/homography_matrix.h"

#include <array>

#include <Eigen/Dense>

#include "base/pose.h"
#include "util/logging.h"
#include "util/math.h"

namespace colmap {
namespace {

double ComputeOppositeOfMinor(const Eigen::Matrix3d& matrix, const size_t row,
                              const size_t col) {
  const size_t col1 = col == 0 ? 1 : 0;
  const size_t col2 = col == 2 ? 1 : 2;
  const size_t row1 = row == 0 ? 1 : 0;
  const size_t row2 = row == 2 ? 1 : 2;
  return (matrix(row1, col2) * matrix(row2, col1) -
          matrix(row1, col1) * matrix(row2, col2));
}

Eigen::Matrix3d ComputeHomographyRotation(const Eigen::Matrix3d& H_normalized,
                                          const Eigen::Vector3d& tstar,
                                          const Eigen::Vector3d& n,
                                          const double v) {
  return H_normalized *
         (Eigen::Matrix3d::Identity() - (2.0 / v) * tstar * n.transpose());
}

}  // namespace

void DecomposeHomographyMatrix(const Eigen::Matrix3d& H,
                               const Eigen::Matrix3d& K1,
                               const Eigen::Matrix3d& K2,
                               std::vector<Eigen::Matrix3d>* R,
                               std::vector<Eigen::Vector3d>* t,
                               std::vector<Eigen::Vector3d>* n) {
  // Remove calibration from homography.
  Eigen::Matrix3d H_normalized = K2.inverse() * H * K1;

  // Remove scale from normalized homography.
  Eigen::JacobiSVD<Eigen::Matrix3d> hmatrix_norm_svd(H_normalized);
  H_normalized.array() /= hmatrix_norm_svd.singularValues()[1];

  // Ensure that we always return rotations, and never reflections.
  //
  // It's enough to take det(H_normalized) > 0.
  //
  // To see this:
  // - In the paper: R := H_normalized * (Id + x y^t)^{-1} (page 32).
  // - Can check that this implies that R is orthogonal: RR^t = Id.
  // - To return a rotation, we also need det(R) > 0.
  // - By Sylvester's idenitity: det(Id + x y^t) = (1 + x^t y), which
  //   is positive by choice of x and y (page 24).
  // - So det(R) and det(H_normalized) have the same sign.
  if (H_normalized.determinant() < 0) {
    H_normalized.array() *= -1.0;
  }

  const Eigen::Matrix3d S =
      H_normalized.transpose() * H_normalized - Eigen::Matrix3d::Identity();

  // Check if H is rotation matrix.
  const double kMinInfinityNorm = 1e-3;
  if (S.lpNorm<Eigen::Infinity>() < kMinInfinityNorm) {
    *R = {H_normalized};
    *t = {Eigen::Vector3d::Zero()};
    *n = {Eigen::Vector3d::Zero()};
    return;
  }

  const double M00 = ComputeOppositeOfMinor(S, 0, 0);
  const double M11 = ComputeOppositeOfMinor(S, 1, 1);
  const double M22 = ComputeOppositeOfMinor(S, 2, 2);

  const double rtM00 = std::sqrt(M00);
  const double rtM11 = std::sqrt(M11);
  const double rtM22 = std::sqrt(M22);

  const double M01 = ComputeOppositeOfMinor(S, 0, 1);
  const double M12 = ComputeOppositeOfMinor(S, 1, 2);
  const double M02 = ComputeOppositeOfMinor(S, 0, 2);

  const int e12 = SignOfNumber(M12);
  const int e02 = SignOfNumber(M02);
  const int e01 = SignOfNumber(M01);

  const double nS00 = std::abs(S(0, 0));
  const double nS11 = std::abs(S(1, 1));
  const double nS22 = std::abs(S(2, 2));

  const std::array<double, 3> nS{{nS00, nS11, nS22}};
  const size_t idx =
      std::distance(nS.begin(), std::max_element(nS.begin(), nS.end()));

  Eigen::Vector3d np1;
  Eigen::Vector3d np2;
  if (idx == 0) {
    np1[0] = S(0, 0);
    np2[0] = S(0, 0);
    np1[1] = S(0, 1) + rtM22;
    np2[1] = S(0, 1) - rtM22;
    np1[2] = S(0, 2) + e12 * rtM11;
    np2[2] = S(0, 2) - e12 * rtM11;
  } else if (idx == 1) {
    np1[0] = S(0, 1) + rtM22;
    np2[0] = S(0, 1) - rtM22;
    np1[1] = S(1, 1);
    np2[1] = S(1, 1);
    np1[2] = S(1, 2) - e02 * rtM00;
    np2[2] = S(1, 2) + e02 * rtM00;
  } else if (idx == 2) {
    np1[0] = S(0, 2) + e01 * rtM11;
    np2[0] = S(0, 2) - e01 * rtM11;
    np1[1] = S(1, 2) + rtM00;
    np2[1] = S(1, 2) - rtM00;
    np1[2] = S(2, 2);
    np2[2] = S(2, 2);
  }

  const double traceS = S.trace();
  const double v = 2.0 * std::sqrt(1.0 + traceS - M00 - M11 - M22);

  const double ESii = SignOfNumber(S(idx, idx));
  const double r_2 = 2 + traceS + v;
  const double nt_2 = 2 + traceS - v;

  const double r = std::sqrt(r_2);
  const double n_t = std::sqrt(nt_2);

  const Eigen::Vector3d n1 = np1.normalized();
  const Eigen::Vector3d n2 = np2.normalized();

  const double half_nt = 0.5 * n_t;
  const double esii_t_r = ESii * r;

  const Eigen::Vector3d t1_star = half_nt * (esii_t_r * n2 - n_t * n1);
  const Eigen::Vector3d t2_star = half_nt * (esii_t_r * n1 - n_t * n2);

  const Eigen::Matrix3d R1 =
      ComputeHomographyRotation(H_normalized, t1_star, n1, v);
  const Eigen::Vector3d t1 = R1 * t1_star;

  const Eigen::Matrix3d R2 =
      ComputeHomographyRotation(H_normalized, t2_star, n2, v);
  const Eigen::Vector3d t2 = R2 * t2_star;

  *R = {R1, R1, R2, R2};
  *t = {t1, -t1, t2, -t2};
  *n = {-n1, n1, -n2, n2};
}

void PoseFromHomographyMatrix(const Eigen::Matrix3d& H,
                              const Eigen::Matrix3d& K1,
                              const Eigen::Matrix3d& K2,
                              const std::vector<Eigen::Vector2d>& points1,
                              const std::vector<Eigen::Vector2d>& points2,
                              Eigen::Matrix3d* R, Eigen::Vector3d* t,
                              Eigen::Vector3d* n,
                              std::vector<Eigen::Vector3d>* points3D) {
  CHECK_EQ(points1.size(), points2.size());

  std::vector<Eigen::Matrix3d> R_cmbs;
  std::vector<Eigen::Vector3d> t_cmbs;
  std::vector<Eigen::Vector3d> n_cmbs;
  DecomposeHomographyMatrix(H, K1, K2, &R_cmbs, &t_cmbs, &n_cmbs);

  points3D->clear();
  for (size_t i = 0; i < R_cmbs.size(); ++i) {
    std::vector<Eigen::Vector3d> points3D_cmb;
    CheckCheirality(R_cmbs[i], t_cmbs[i], points1, points2, &points3D_cmb);
    if (points3D_cmb.size() >= points3D->size()) {
      *R = R_cmbs[i];
      *t = t_cmbs[i];
      *n = n_cmbs[i];
      *points3D = points3D_cmb;
    }
  }
}

Eigen::Matrix3d HomographyMatrixFromPose(const Eigen::Matrix3d& K1,
                                         const Eigen::Matrix3d& K2,
                                         const Eigen::Matrix3d& R,
                                         const Eigen::Vector3d& t,
                                         const Eigen::Vector3d& n,
                                         const double d) {
  CHECK_GT(d, 0);
  return K2 * (R - t * n.normalized().transpose() / d) * K1.inverse();
}

}  // namespace colmap


/**
* @file: MyUtility.hpp
* @brief:
* @author: Changjiang Cai, caicj5351@gmail.com
* @version: 0.0.1
* @creation date: 17-12-2015
* @last modified: Thu 10 Mar 2016 09:32:02 AM EST
*/

#ifndef __HEADER__CROWD_SOURCING_PROJECT_UTILITY_H_
#define __HEADER__CROWD_SOURCING_PROJECT_UTILITY_H_
#include <iostream>
#include <fstream> // std::fstream
#include <vector>
#include <string>
#include <map> /*ordered map*/
#include <unordered_map> /*unordered map*/
#include <set>
#include <algorithm> /*Permutation*/
#include <functional>
#include <array> /* array */
#include <cmath> /*pow etc*/
#include <cstdio>/*printf*/
#include <string>
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */
#include <limits.h>
#include <cstring> /* memset */
#include <queue>
#include <set>
#include <vector>
#include <random>  /*std::uniform_int_distribution*/
#include <Eigen/Dense>
//#include "CrowdBT.h" // Crowd_BT
#include <boost/math/special_functions/digamma.hpp>

typedef unsigned long ulong;
typedef unsigned long long ullong;
ullong factorial(unsigned int n);
ullong factorial(int n);
void printTiePath(const int & v, // vertex number;
	const std::vector<int> & v_tiePath, // tie-paths;
	const std::string  & fn); // output file name;

std::string toString(const std::vector<int> &p);

void printFactorial(std::ofstream  & of, const int & n);

std::vector<std::vector<double> > add(const std::vector<std::vector<double> >& lhs, const std::vector<std::vector<double> >& rhs);

std::vector<std::vector<int>> add(const std::vector<std::vector<int> >& lhs, const std::vector<std::vector<int> >& rhs);

std::vector<std::vector<double> > multiply(const std::vector<std::vector<double> >& lhs, const std::vector<std::vector<double> >& rhs);
std::vector<std::vector<double> > multiplyEigen(const std::vector<std::vector<double> >& lhs, const std::vector<std::vector<double> >& rhs);

void unify_BT_D(Eigen::MatrixXd & M, const int & n);/*direct use of voting results*/
void unify_BT_D(std::vector<std::vector<double> >& M);/*direct use of voting results*/
void unify_BT_E(std::vector<std::vector<double> >& M);/*using the exponentials of voting results*/
void unify_BT_L(std::vector<std::vector<double> >& M, const double & alpha = 1.0);/*Laplace smoothing*/

void
output_matrix(std::vector<std::vector<int> > &matrix);

void
output_matrix(std::vector<std::vector<double> > &matrix);

template<typename T>
void output_matrix(std::vector<std::vector<T> > &matrix, std::ofstream & of1){
	for (int i = 0; i < matrix.size(); ++i){
		for (int j = 0; j < matrix[i].size(); ++j){
			of1 << matrix[i][j];
			if (matrix[i].size() - 1 == j)
				of1 << "\n";
			else of1 << ",";
		}
	}
}

// print boundary;
template<typename T>
void printBoundary(const T & val, const int & vertexNum, std::ofstream & of_csv){
	for (auto i = 0; i < vertexNum; ++i)
		of_csv << val << ",";
	of_csv << endl;
}

template <typename T, typename T2>
bool isHP(T ** tc, T2 * v, const int & size){

		for (int i = 0; i < size - 1; i++)
		{
			if (tc[v[i]][v[i + 1]] == 0){
				/*std::cout << " (" << v[i] << ", " << v[i + 1] <<
					") = 0, ";*/
				return false;
			}
		}
		return true;
}

#endif


/*!
 * @file    swp_silicon_inception.cpp
 * @author  William J Menz
 * @brief   Implementation of the SiliconInception class.
 *
 *   Author(s):      William J Menz
 *   Project:        sweepc (population balance solver)
 *   Copyright (C) 2012 William J Menz
 *
 *   File purpose:
 *      Based on swp_dimer_inception by Robert Patterson, Markus Sander
 *      and Matthew Celnik.
 *
 *      Implementation of the SiliconInception class.
 *
 *   Licence:
 *      This file is part of "sweepc".
 *
 *      sweepc is free software; you can redistribute it and/or
 *      modify it under the terms of the GNU Lesser General Public License
 *      as published by the Free Software Foundation; either version 2
 *      of the License, or (at your option) any later version.
 *
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU Lesser General Public License for more details.
 *
 *      You should have received a copy of the GNU Lesser General Public
 *      License along with this program; if not, write to the Free Software
 *      Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
 *      02111-1307, USA.
 *
 *   Contact:
 *      Prof Markus Kraft
 *      Dept of Chemical Engineering
 *      University of Cambridge
 *      New Museums Site
 *      Pembroke Street
 *      Cambridge
 *      CB2 3RA, UK
 *
 *      Email:       mk306@cam.ac.uk
 *      Website:     http://como.cheng.cam.ac.uk
*/

#include "swp_silicon_inception.h"
#include "swp_mechanism.h"
#include "swp_params.h"
#include "swp_primary.h"
#include "swp_sprog_idealgas_wrapper.h"

#include <boost/random/uniform_01.hpp>
#include <boost/random/discrete_distribution.hpp>

using namespace Sweep;
using namespace Sweep::Processes;
using namespace std;

// Mass of a silicon monomer, kg
const double SiliconInception::m_m1  = 4.664e-26;
// Vol of a silicon atom, m3
const double SiliconInception::m_v1  = 2.003e-29;
// Spherical diameter of a bulk silicon atom, m
const double SiliconInception::m_d1  = 3.369e-10;

// CONSTRUCTORS AND DESTRUCTORS.

//! Default constructor (protected).
SiliconInception::SiliconInception(void)
: Inception(), m_kfm(0.0), m_ksf1(0.0), m_ksf2(0.0),
  m_efm(2.2),
  m_itype(iCollisional), m_vi(0.0), m_di(0.0),
  m_sidata(0), m_reacs(0), m_concs(0)
{
    m_name = "SiliconInception";
}

//! Initialising constructor.
SiliconInception::SiliconInception(const Sweep::Mechanism &mech)
: Inception(mech), m_kfm(0.0), m_ksf1(0.0), m_ksf2(0.0),
  m_efm(mech.GetEnhancementFM()),
  m_itype(iCollisional), m_vi(0.0), m_di(0.0),
  m_sidata(0), m_reacs(0), m_concs(0)
{
    m_name = "SiliconInception";
}

//! Copy constructor.
SiliconInception::SiliconInception(const SiliconInception &copy)
: m_efm(copy.m_efm)
{
    *this = copy;
}

//! Stream-reading constructor.
SiliconInception::SiliconInception(std::istream &in, const Sweep::Mechanism &mech)
: m_efm(mech.GetEnhancementFM())
{
    Deserialize(in, mech);
}

//! Default destructor.
SiliconInception::~SiliconInception(void)
{
}

// OPERATOR OVERLOADS.

/*!
 * @brief           Assignment operator
 *
 * @param rhs       Pointer to right hand side object
 * @return          Pointer to object
 */
SiliconInception &SiliconInception::operator =(const SiliconInception &rhs)
{
    if (this != &rhs) {
        Inception::operator =(rhs);
        m_kfm  = rhs.m_kfm;
        m_ksf1 = rhs.m_ksf1;
        m_ksf2 = rhs.m_ksf2;
        m_itype = rhs.m_itype;
        m_vi   = rhs.m_vi;
        m_di   = rhs.m_di;

        m_sidata = rhs.m_sidata;
        m_reacs  = rhs.m_reacs;
        m_concs  = rhs.m_concs;
    }
    return *this;
}

/*!
 * Create a new particle and add it to the ensemble with position uniformly
 * distributed over the grid cell.
 *
 * The iterm parameter is included because it will be needed for many process
 * types and this function is meant to have a general signature.
 *
 * \param[in]       t               Time
 * \param[in]       local_geom      Details of geometry around current location
 * \param[in,out]   sys             System to update
 * \param[in]       iterm           Process term responsible for this event
 * \param[in,out]   rng             Random number generator
 *
 * \return      0 on success, otherwise negative.
 */
int SiliconInception::Perform(const double t, Cell &sys,
                            const Geometry::LocalGeometry1d &local_geom,
                            const unsigned int iterm,
                            rng_type &rng) const {

    // This routine performs the inception on the given chemical system.

    // Create a new particle of the type specified
    // by the system ensemble.
    Particle *sp = m_mech->CreateParticle(t);

    // Get the cell vertices
    fvector vertices = local_geom.cellVertices();

    // Sample a uniformly distributed position, note that this method
    // works whether the vertices come in increasing or decreasing order,
    // but 1d is assumed for now.
    double posn = vertices.front();

    const double width = vertices.back() - posn;
    boost::uniform_01<rng_type&, double> uniformGenerator(rng);
    posn += width * uniformGenerator();

    sp->setPositionAndTime(posn, t);

    if (m_itype == iCollisional) {

        // If the normal collisional-type rate is used, create the particle
        // and adjust the gas-phase as in normal Inception

        // Initialise the new particle.
        sp->Primary()->SetComposition(ParticleComp());
        sp->Primary()->SetValues(ParticleTrackers());
        sp->UpdateCache();

        // Add particle to system's ensemble.
        sys.Particles().Add(*sp, rng);

        // Update gas-phase chemistry of system.
        adjustGas(sys, sp->getStatisticalWeight());

    } else {

        // Otherwise, use information from m_sidata to select a gas-phase
        // species to transform into a new particle
        const SiliconData* species = ChooseData(sys, rng);
        if (species != NULL) {
            sp->Primary()->SetComposition(species->_track);
            sp->Primary()->SetValues(ParticleTrackers());
            sp->UpdateCache();

            // Add particle to system's ensemble.
            sys.Particles().Add(*sp, rng);

            // Update gas-phase chemistry of system.
            adjustGasPhase(sys, (*species), sp->getStatisticalWeight());
        } else {
            std::cout << "Warning: silicon inception chosen with no precursor."
                    << endl;
        }
    }

    return 0;
}

// INCEPTION KERNEL.

/*!
 * The inception rate is based on a transition coagulation kernel that
 * is calculated using a molecule diameter and mass.  These values do not
 * have to correspond the the physical properties of the molecule, but they
 * are fed into the transition regime kernel.
 *
 * @param[in]    m1    mass of first molecule
 * @param[in]    m2    mass of second molecule
 * @param[in]    d1    diameter of first molecule
 * @param[in]    d2    diameter of second molecule
 */
void SiliconInception::SetInceptingSpecies(double m1, double m2, double d1, double d2)
{
    // The free mol part can be handled by the free mol specific method
    SetInceptingSpeciesFreeMol(m1, m2, d1, d2);

    // Now the slip flow part
    double invd1=1.0/d1, invd2=1.0/d2;
    m_ksf1 = CSF * (d1+d2);
    m_ksf2 = 2.0 * 1.257 * m_ksf1 * ((invd1*invd1) + (invd2*invd2));
    m_ksf1 = m_ksf1 * (invd1+invd2);
}

/*!
 * The inception rate is based on a free molecular coagulation kernel only,
 * contrast \ref SetInceptingSpecies that
 * is calculated using a molecule diameter and mass.  These values do not
 * have to correspond the the physical properties of the molecule, but they
 * are fed into the transition regime kernel.
 *
 * @param[in]    m1    mass of first molecule
 * @param[in]    m2    mass of second molecule
 * @param[in]    d1    diameter of first molecule
 * @param[in]    d2    diameter of second molecule
 */
void SiliconInception::SetInceptingSpeciesFreeMol(double m1, double m2, double d1, double d2)
{
    // This routine sets the free-mol and slip flow kernel parameters given
    // the mass and diameter of the incepting species.
    m_kfm  = m_efm * CFM * sqrt((1.0/m1) + (1.0/m2)) * (d1+d2) * (d1+d2);
    m_ksf1 = 0.0;
    m_ksf2 = 0.0;
}

/*!
 * @brief           Sets the incepting volume
 *
 * The volume of the incepting particle is determined upon initialisation,
 * based on the density of molwt of each component.
 *
 * @param mech      Particle mechanism
 */
void SiliconInception::SetInceptingVolume(const Sweep::Mechanism &mech)
{
    double v1(0.0);
    CompPtrVector comp = mech.Components();
    for (unsigned int i=0; i!=comp.size(); i++) {
        // returns in m3, due to Density()
        v1 += 1.0 * mech.Components(i)->MolWt() /
                (Sweep::NA * mech.Components(i)->Density());
    }

    m_vi = v1;
}

/*!
 * @brief       Calculates the diameter of an incepting particle
 *
 * Pre-calculates the incepting diameter of a particle (when a collisional
 * inception process is used) for this inception reaction. This is done
 * using the molwt, density and initial number of each component in
 * the new particle.
 *
 * @param mech  Particle mechanism
 */
void SiliconInception::SetInceptingDiameter(const Sweep::Mechanism &mech)
{
    if (m_itype != iCollisional) {
        double dmin(1.0);
        for (unsigned int i=0; i!=m_sidata.size(); i++) {
            if (m_sidata[i]._diam < dmin) dmin = m_sidata[i]._diam;
        }
        m_di = dmin;
    } else {
        //Pre-define volume and diameters
        double v(0.0), d(0.0);

        // Loop over list of components in incepting particle to find initial vol
        for (unsigned int i=0; i!=ParticleComp().size(); i++) {

            // returns in m3, due to Density()
            v += ParticleComp()[i] * mech.Components(i)->MolWt() /
                    (Sweep::NA * mech.Components(i)->Density());
        }

        // Now calculate the equivalent spherical diameter
        d = pow(6 * v / Sweep::PI, Sweep::ONE_THIRD);
        m_di = d;
    }
}

/*!
 * @brief           Generates the silicon species data
 *
 * The silicon species data hold information on each species such as name,
 * diameter and the incepting particle composition.
 *
 * @param mech      Particle mechanism
 */
void SiliconInception::GenerateSpeciesData(const Sweep::Mechanism &mech)
{
    // First scan the species to determine possible candidates for inception
    // Loop over all gas-phase species
    const Sprog::SpeciesPtrVector *spec = mech.Species();
    for (unsigned int i=0; i!=spec->size(); i++) {

        string name = spec->at(i)->Name();
        CompPtrVector comp = mech.Components();

        if (IsCandidate(name)) {
            SiliconData *data;
            data = new SiliconData();
            data->_fracIndex = i;
            data->_name      = name;

            // Create a new component vector for storing new particle comp
            data->_track.resize(comp.size(), 0.0);

            // Initialise variable for volume
            double v(0.0);

            // Loop over particle components to work out how many the
            // gas-phase species of interest will  contribute
            for (unsigned int j=0; j!=comp.size(); j++) {

                if (comp[j]->Name() == "silicon") {
                    data->_track[j] = spec->at(i)->AtomCount("SI");
                    v += data->_track[j] * comp[j]->MolWt() /
                            (NA * comp[j]->Density());
                } else if (comp[j]->Name() == "hydrogen") {
                    data->_track[j] = spec->at(i)->AtomCount("H");
                    v += data->_track[j] * comp[j]->MolWt() /
                            (NA * comp[j]->Density());
                } else {
                    std::cout << "Warning! Unrecognised component name!" << std::endl;
                }
            }
            data->_diam = pow(6.0 * v / Sweep::PI, Sweep::ONE_THIRD);
            printSiliconData(*data);
            m_sidata.push_back(*data);
        }

    }

    // Now generate other data
    SetInceptingDiameter(mech);
    SetInceptingVolume(mech);

    // Resize the counter vectors
    m_reacs.resize(spec->size(), 0);
    m_concs.resize(spec->size(), 0.0);
}

const SiliconInception::SiliconData* SiliconInception::ChooseData(const Sweep::Cell &sys,
        rng_type &rng) const
{
    // Choose a species which is ABOVE THE CRITICAL DIAMETER and
    // weight the choice based on mole fractions.
    const SiliconData* ans(NULL);

    double dcrit = GetCriticalNucleus(sys);

    // Hold fractions of available species in vector
    fvector availFracs;
    std::vector<const SiliconData*> availSpecies;
    double sum(0.0);
    double frac(0.0);

    // Loop over candidate species
    for (unsigned int i=0; i != m_sidata.size(); i++) {
        if (m_sidata[i]._diam >= dcrit) {
            frac = sys.GasPhase().SpeciesConcentration(m_sidata[i]._fracIndex) /
                    sys.GasPhase().MolarDensity();
            sum += frac;
            availFracs.push_back(frac);
            availSpecies.push_back(&(m_sidata[i]));
        }
    }

    // Scale these to be probabilities
    if (sum > 0.0) {
        for (unsigned int i=0; i != availFracs.size(); i++) {
            availFracs[i] /= sum;
        }

        // Select an index based on its mole fraction
        boost::random::discrete_distribution<> dist(availFracs);
        unsigned int j = dist(rng);
        ans = availSpecies[j];
    }
    return ans;
}

/*!
 * Adjust the gas phase for the effects of this process.
 *
 *@param[in]    sys         Cell in which the process is taking place
 *@param[in]    species     Silicon hydride species to be incepted
 *@param[in]    wt          Statistical weight of particle
 *
 *@pre      The gas phase in sys must be of type SprogIdealGasWrapper
 *
 *@exception    std::runtime_error      Could not cast gas phase to SprogIdealGasWrapper
 */
void SiliconInception::adjustGasPhase(Sweep::Cell &sys,
        const SiliconInception::SiliconData &species,
        double wt) const
{
    if(!sys.FixedChem()) {
    // This method requires write access to the gas phase, which is not
    // standard in sweep.  This means it cannot use the generic gas
    // phase interface
    SprogIdealGasWrapper *gasWrapper = dynamic_cast<SprogIdealGasWrapper*>(&sys.GasPhase());
    if(gasWrapper == NULL)
        throw std::runtime_error("Could not cast gas phase to SprogIdealGasWrapper in SiliconInception::adjustGasPhase");

    // If excecution reaches here, the cast must have been successful
    Sprog::Thermo::IdealGas *gas = gasWrapper->Implementation();

    // Get the existing concentrations
    fvector newConcs;
    gas->GetConcs(newConcs);

    double n_NAvol = wt / (NA * sys.SampleVolume());

    // Update the internal counters
    m_reacs[species._fracIndex] += 1;
    m_concs[species._fracIndex] += n_NAvol;

    // Use the fracindex to calculate the amount of precursor to be
    // removed.
    newConcs[species._fracIndex] -= 1.0 * n_NAvol;

    // Now adjust the gas-phase!
    gas->SetConcs(newConcs);
    }
}

/*!
 * @brief       Does this species contribute to inception?
 *
 * @param name  Species name
 * @return      True/false
 */
bool SiliconInception::IsCandidate(std::string name) const
{
    bool ans(false);
    // Permit silylenes to incept
    if (std::string::npos != name.find("B")
            && std::string::npos != name.find("SI")) {
        ans = true;
    } else if (std::string::npos != name.find("A")
            && std::string::npos != name.find("SI")) {
        ans = true;
    } else if (name == "SI2H2") {
        ans = true;
    } else if (name == "SIH2") {
        ans = true;
    } else if (name == "SIH") {
        ans = true;
    } else if (name == "SI") {
        ans = true;
    }
    return ans;
}

/*!
 * @brief       Calculates the mole fraction of precursor
 *
 * @param sys   System for analysis
 * @return      Mole fraction of precursor
 */
double SiliconInception::GetPrecursorFraction(const Sweep::Cell &sys) const
{
    // Initialise the fraction
    double frac(0.0);

    // Loop over all gas-phase species
    for (unsigned int i=0; i!=m_sidata.size(); i++) {
        frac += sys.GasPhase().SpeciesConcentration(m_sidata[i]._fracIndex) /
                sys.GasPhase().MolarDensity();
    }

    if (frac > 1.0) {
        throw runtime_error("Mole fraction greater than 1.0! "
                            "(Sweep, SiliconInception::GetPrecursorFraction).");
    }
    return frac;
}

/*!
 * @brief       Returns the supersaturation
 *
 * Supersaturation for this system is defined by the ratio of the
 * monomer pressure to the saturation vapour pressure.
 *
 * @param sys   System for which to calculate
 * @return      Supersaturation (-)
 */
double SiliconInception::GetSupersaturation(const Sweep::Cell &sys) const
{
    double s(1.0);

    // First get the silicon pressure
    s *= (GetPrecursorFraction(sys) * sys.GasPhase().Pressure());

    // Now divide by the saturation vapour pressure
    s /= GetSatVapourPressure(sys.GasPhase().Temperature());

    return s;

}

/*!
 * @brief       Returns the surface energy of silicon
 *
 * Koermer et al (2010) J. Aerosol Sci, 41, 1007-
 *
 * @param T     Temperature (K)
 * @return      Surface energy (N/m)
 */
double SiliconInception::GetSurfaceEnergy(double T) const
{
    return 1.152 - 1.574e-4 * T;
}

/*!
 * @brief       Returns the saturation vapour pressure of silicon
 *
 * Koermer et al (2010) J. Aerosol Sci, 41, 1007-
 *
 * @param T     Temperature (K)
 * @return      Saturation vapour pressure (Pa)
 */
double SiliconInception::GetSatVapourPressure(double T) const
{
    return 101325.0 * pow(10, 7.534 - (23399 / T));
}

/*!
 * @brief       Returns the saturated monomer number concentration
 *
 * Estimates the number concentration from the saturation vapour pressure
 *
 * @param T     Temperature (K)
 * @return      Sat. monomer number concentration (#/m3)
 */
double SiliconInception::GetMonomerConc(double T) const
{
    double psat = GetSatVapourPressure(T);
    psat *= (NA / (T * R));
    return psat;
}

/*!
 * @brief       Calculates the critical nucleus size
 *
 * Uses the Kelvin equation to calculate the critical nucleus size. This
 * is given by:
 * d* = 4 gamma v1 / (kB T ln(S))
 * where:
 *  gamma is the surface energy (N/m)
 *  v1 is the volume of a monomer (m)
 *  kB is Boltzmann's constant
 *  S is the supersaturation
 *
 * @param sys   System for analysis
 * @return      Critical diameter, in metres
 */
double SiliconInception::GetCriticalNucleus(const Sweep::Cell &sys) const
{
    double d = 4.0/Sweep::KB;
    double s = GetSupersaturation(sys);

    // Get chemical conditions
    double T = sys.GasPhase().Temperature();      // in K

    // Set d to an arbitrarily high value if psi = 0 (i.e. no precursor)
    if (s <= 1.0) {
        d = 1.0;                            // in m
    } else {
        d *= (GetSurfaceEnergy(T) * m_v1 / (T * log(s)));      // in m
    }

    return d;
}

bool SiliconInception::IsInceptionAllowed(const Sweep::Cell &sys) const
{
    bool val(false);
    // Only allow inception if d_incep >= d_crit
    if (m_di >= GetCriticalNucleus(sys)) {
        val = true;
    } else{
        val = false;
    }
    return val;
}

// TOTAL RATE CALCULATIONS.

// Returns rate of the process for the given system.
double SiliconInception::Rate(double t, const Cell &sys, const Geometry::LocalGeometry1d &local_geom) const
{
    // Get the current chemical conditions.
    double T = sys.GasPhase().Temperature();
    double P = sys.GasPhase().Pressure();

    // Calculate the rate.
    return Rate(sys.GasPhase(), sqrt(T),
                T/sys.GasPhase().Viscosity(), MeanFreePathAir(T,P),
                sys.SampleVolume(), sys);
}

/*!
 * Calculate inception rate using a the transition coagulation kernel
 * with the values provided by the user.  The result is this value
 * multiplied by the square of the number concentration of the gas
 * phase species.
 *
 * @param[in]    gas      interface to gas mixture
 * @param[in]    sqrtT    square root of temperature
 * @param[in]    T_mu     temperature divided by air viscosity
 * @param[in]    MFP      mean free path in gas
 * @param[in]    vol      sample volume
 *
 * @return    Inception rate for a cell of size vol. (\f$ \mathrm{s}^{-1}\f$)
 */
double SiliconInception::Rate(const EnvironmentInterface &gas, double sqrtT,
                     double T_mu, double MFP, double vol, const Cell &sys) const
{
    double rate(1.0);
    double fm(0.0);
    double sf(0.0);
    double n(0.0);
    double s(0.0);
    double y(0.0);
    double Theta(0.0);
    double T = sys.GasPhase().Temperature();

    // Check if the rate is > 0 as found by homogeneous nucleation
    if (IsInceptionAllowed(sys)) {
        switch (m_itype) {
        case iCollisional:
            // Use the 'normal' MOPS collisional rate
            rate = A() * vol * chemRatePart(gas);

            fm   = sqrtT * m_kfm;
            if((m_ksf1 > 0) || (m_ksf2 > 0))  {
                // Transition regime
                sf   = T_mu  * (m_ksf1 + (MFP*m_ksf2));
                rate *= ((fm*sf) / (fm+sf));
            }
            else {
                // Free mol regime only
                rate *= fm;
            }

            break;
        case iVBDZ:
            // Wu et al (1987) Langmuir 3:266-271
            rate = A() * vol;

            s = GetSupersaturation(sys);
            y = GetSurfaceEnergy(T);
            n = GetMonomerConc(T);

            rate *= (s * s * m_v1 * n * n);
            rate *= sqrt(2.0 * y / (PI * m_m1));
            rate *= exp(-16.0 * PI * y * y * y * m_v1 * m_v1 * m_v1
                    / (3.0 * KB * KB * KB * T * T * T * log(s) * log(s)));

            break;
        case iGirshick:
            // Girshick & Chiu (1990) J. Chem. Phys. 93:1273-1277
            rate = A() * vol;

            s = GetSupersaturation(sys);
            y = GetSurfaceEnergy(T);
            n = GetMonomerConc(T);

            Theta = PI * m_d1 * m_d1 * y / (KB * T);

            rate *= (s  * m_v1 * n * n);
            rate *= sqrt(2.0 * y / (PI * m_m1));
            rate *= exp(Theta - (4.0 * Theta * Theta * Theta / (27.0 * log(s) * log(s))));

            break;
        }
    } else {
        // We have d < d_crit, return 0.
        rate = 0.0;
    }

    return rate;
}

/*!
 * Calculates the gas-phase chemistry contribution to the rate
 * expression.  This is overloaded as Avogadro's number must be
 * included in the terms for inception processes.
 *
 * @param[in]    gas      interface to gas mixture
 */
double SiliconInception::chemRatePart(const EnvironmentInterface &gas) const
{
    // Factor of 0.5 adjusts for doubling counting of pairs of molecules in the number
    // of possible collisions.
    double rate = 0.5;

    Sprog::StoichMap::const_iterator i;
    for (i=m_reac.begin(); i!=m_reac.end(); ++i) {
        //std::cerr << "Mole frac to use " << fracs[i->first] << std::endl;
        double conc = gas.SpeciesConcentration(i->first);
        for (int j=0; j!=i->second; ++j) {
            rate *= (NA * conc);
        }
    }

    return rate;
}

// RATE TERM CALCULATIONS.

// Returns the number of rate terms for this process (one).
unsigned int SiliconInception::TermCount(void) const {return 1;}

// Calculates the rate terms given an iterator to a double vector. The
// iterator is advanced to the position after the last term for this
// process.  Returns the sum of all terms.
double SiliconInception::RateTerms(const double t, const Cell &sys,
                               const Geometry::LocalGeometry1d &local_geom,
                               fvector::iterator &iterm) const
{
    // Get the current chemical conditions.
    double T = sys.GasPhase().Temperature();
    double P = sys.GasPhase().Pressure();

    // Calculate the single rate term and advance iterator.
    *iterm = Rate(sys.GasPhase(), sqrt(T),
                  T/sys.GasPhase().Viscosity(), MeanFreePathAir(T,P),
                  sys.SampleVolume(), sys);
    return *(iterm++);
}

// READ/WRITE/COPY.

// Creates a copy of the inception.
SiliconInception *const SiliconInception::Clone(void) const {return new SiliconInception(*this);}

// Returns the process type.  Used to identify different
// processes and for serialisation.
ProcessType SiliconInception::ID(void) const {return Silicon_Inception_ID;}

// Writes the object to a binary stream.
void SiliconInception::Serialize(std::ostream &out) const
{
    if (out.good()) {
        // Output the version ID (=0 at the moment).
        const unsigned int version = 0;
        out.write((char*)&version, sizeof(version));

        // Serialize base class.
        Inception::Serialize(out);

        // Write free-mol parameter.
        double v = (double)m_kfm;
        out.write((char*)&v, sizeof(v));

        // Write slip-flow parameters.
        v = (double)m_ksf1;
        out.write((char*)&v, sizeof(v));
        v = (double)m_ksf2;
        out.write((char*)&v, sizeof(v));

        // Write HNT parameters
        unsigned int num = (unsigned int)m_itype;
        out.write((char*)&num, sizeof(num));
        v = (double)m_vi;
        out.write((char*)&v, sizeof(v));
        v = (double)m_di;
        out.write((char*)&v, sizeof(v));

    } else {
        throw invalid_argument("Output stream not ready "
                               "(Sweep, SiliconInception::Serialize).");
    }
}

// Reads the object from a binary stream.
void SiliconInception::Deserialize(std::istream &in, const Sweep::Mechanism &mech)
{
    if (in.good()) {
        // Read the output version.  Currently there is only one
        // output version, so we don't do anything with this variable.
        // Still needs to be read though.
        unsigned int version = 0;
        in.read(reinterpret_cast<char*>(&version), sizeof(version));

        double val = 0.0;
        unsigned int num(0);

        switch (version) {
            case 0:
                // Deserialize base class.
                Inception::Deserialize(in, mech);

                // Read free-mol parameter.
                in.read(reinterpret_cast<char*>(&val), sizeof(val));
                m_kfm = (double)val;

                // Read slip-flow parameters.
                in.read(reinterpret_cast<char*>(&val), sizeof(val));
                m_ksf1 = (double)val;
                in.read(reinterpret_cast<char*>(&val), sizeof(val));
                m_ksf2 = (double)val;

                // Read HNT parameters
                in.read(reinterpret_cast<char*>(&num), sizeof(num));
                m_itype = (InceptionType)num;
                in.read(reinterpret_cast<char*>(&val), sizeof(val));
                m_vi = (double)val;
                in.read(reinterpret_cast<char*>(&val), sizeof(val));
                m_di = (double)val;

                break;
            default:
                throw runtime_error("Serialized version number is invalid "
                                    "(Sweep, SiliconInception::Deserialize).");
        }
    } else {
        throw invalid_argument("Input stream not ready "
                               "(Sweep, SiliconInception::Deserialize).");
    }
}