#include <gen/terrain.hpp>

#include <boost/graph/astar_search.hpp>
#include <gen/graphutils.hpp>

#include <gen/poissonsampling.hpp>
#include <math/mathtools.hpp>

#include <armadillo>

namespace eXl
{

  class ArmaLaplaceMatrix : public LaplaceMatrix
  {
  public:
    arma::sp_mat m_Matrix;
  };

  struct found_goal{};

  template <class Vertex>
  class astar_goal_visitor : public boost::default_astar_visitor
  {
  public:
    inline astar_goal_visitor(Vertex goal) : m_goal(goal) {}
    template <class Graph>
    void examine_vertex(Vertex u, Graph& g) {
      if(u == m_goal)
        throw found_goal();
    }
  private:
    Vertex m_goal;
  };

  template<typename PosMap>
  class VoroDiagGraphDistance : public boost::astar_heuristic<VoronoiGraph::CellGraphImpl, float>
  {
  public:
    typedef boost::graph_traits<VoronoiGraph::CellGraphImpl>::vertex_descriptor Vertex;
    VoroDiagGraphDistance(PosMap const& iCenters, Vertex iGoal, TIndexMap<VoronoiGraph::CellGraphImpl> const& iIndex)
      : m_CellCenter(iCenters), m_Goal(iGoal), m_Index(iIndex) {}
    float operator()(Vertex u)
    {
      unsigned int goalIdx = boost::get(m_Index, m_Goal);
      unsigned int idx = boost::get(m_Index, u);

      Vector2f goalPos = m_CellCenter(goalIdx);
      Vector2f curPos = m_CellCenter(idx);

      return (curPos - goalPos).Length();
    }
  private:
    PosMap const& m_CellCenter;
    TIndexMap<VoronoiGraph::CellGraphImpl> const& m_Index;
    Vertex m_Goal;
  };

  template<typename PosMap>
  void FindPath(VoronoiGraph::CellGraph const& iGraph,
    PosMap const& iCellCenters,
    VoronoiGraph::CellGraphImpl::vertex_descriptor start,
    VoronoiGraph::CellGraphImpl::vertex_descriptor goal,
    Vector<VoronoiGraph::CellGraphImpl::vertex_descriptor>& oPath)
  {
    IndexMap<VoronoiGraph::CellGraphImpl> index(iGraph.m_CellGraph);
    Vector<VoronoiGraph::CellGraphImpl::vertex_descriptor> p(boost::num_vertices(iGraph.m_CellGraph), start);
    Vector<float> d(boost::num_vertices(iGraph.m_CellGraph));
    bool found = false;
    try
    {
      // call astar named parameter interface
      boost::astar_search_tree
      (iGraph.m_CellGraph, start,
        VoroDiagGraphDistance<PosMap>(iCellCenters, goal, index),
        //iFactory.Make<boost::filtered_graph<OutGraph, boost::keep_all, TagFilter> >(filteredGr, goal),
        boost::predecessor_map(boost::make_iterator_property_map(p.begin(), index)).
        distance_map(make_iterator_property_map(d.begin(), index)).
        visitor(astar_goal_visitor<VoronoiGraph::CellGraphImpl::vertex_descriptor>(goal)));
    }
    catch(found_goal )
    { // found a path to the goal
      Vector<VoronoiGraph::CellGraphImpl::vertex_descriptor> path;
      for(auto v = goal;; v = p[v])
      {
        path.push_back(v);

        if(p[v] == v)
          break;
      }
      oPath.swap(path);
      found = true;
    }
  }

  struct TerrainPosMap
  {
    TerrainPosMap(Vector<Terrain::CellProperties> const& iCells)
      :m_Cells(iCells){}

    Vector2f const& operator()(unsigned int i) const {return m_Cells[i].position;}

    Vector<Terrain::CellProperties> const& m_Cells;
  };

  Terrain::Terrain()
  {
  }

  void Terrain::MakeGrid(Vector2f const& iSize, Vector2i const& iGridSize)
  {
    m_Size = iSize;

    Vector2f gridStep(iSize.X() / iGridSize.X(), iSize.Y() / iGridSize.Y());

    m_QueryBox = AABB2Df(-gridStep * 0.51, gridStep * 1.02);

    m_Cells.clear();
    m_Graph.m_CellGraph.clear();
    m_Graph.m_Edges.clear();
    m_Graph.m_Vertices.clear();
    m_Neigh.clear();
    m_Index.clear();

    uint32_t totNumCells = iGridSize.X() * iGridSize.Y();

    m_Cells.reserve(totNumCells);
    m_Neigh.reserve(totNumCells * 4);
    for (uint32_t i = 0; i < totNumCells; ++i)
    {
      boost::add_vertex(m_Graph.m_CellGraph);
    }

    for (int32_t y = 0; y < iGridSize.Y() + 1; ++y)
    {
      for (int32_t x = 0; x < iGridSize.X() + 1; ++x)
      {
        VoronoiGraph::Vertex gridVtx;
        gridVtx.m_Position = Vector2f(gridStep.X() * x, gridStep.Y() * y);
        m_Graph.m_Vertices.push_back(gridVtx);
      }
    }

    uint32_t cellOffset = 0;
    Vector<CellLoc> locs;
    locs.reserve(totNumCells);
    for (int32_t y = 0; y < iGridSize.Y(); ++y)
    {
      for (int32_t x = 0; x < iGridSize.X(); ++x)
      {
        uint32_t gridLowLeftCorner = y * (iGridSize.X() + 1) + x;

        CellProperties cell;
        cell.neighCount = 0;
        cell.position = Vector2f(((float)x + 0.5) * gridStep.X(), ((float)y + 0.5) * gridStep.Y());
        cell.neighStart = m_Neigh.size();

        Vector2f const& cellPos = cell.position;
        locs.push_back(std::make_pair(AABB2Df(cellPos - gridStep * 0.5, gridStep), cellOffset));

        if (x > 0)
        {
          cell.neighCount++;
          m_Neigh.push_back(cellOffset - 1);
        //  auto edgeDesc = boost::add_edge(cellOffset, cellOffset - 1, m_Graph.m_CellGraph);
        //  VoronoiGraph::Edge newEdge;
        //  newEdge.m_Pt1 = gridLowLeftCorner;
        //  newEdge.m_Pt2 = gridLowLeftCorner + iGridSize.X() + 1;
        //  m_Graph.m_Edges.insert(std::make_pair(edgeDesc, newEdge));
        }

        if (x < iGridSize.X() - 1)
        {
          cell.neighCount++;
          m_Neigh.push_back(cellOffset + 1);
          auto edgeDesc = boost::add_edge(cellOffset, cellOffset + 1, m_Graph.m_CellGraph).first;
          VoronoiGraph::Edge newEdge;
          newEdge.m_Pt1 = gridLowLeftCorner + 1;
          newEdge.m_Pt2 = gridLowLeftCorner + 1 + iGridSize.X() + 1;
          m_Graph.m_Edges[edgeDesc] = newEdge;
        }

        if (y > 0)
        {
          cell.neighCount++;
          m_Neigh.push_back(cellOffset - iGridSize.X());
        //  auto edgeDesc = boost::add_edge(cellOffset, cellOffset - iGridSize.X(), m_Graph.m_CellGraph);
        //  VoronoiGraph::Edge newEdge;
        //  newEdge.m_Pt1 = gridLowLeftCorner;
        //  newEdge.m_Pt2 = gridLowLeftCorner + 1;
        //  m_Graph.m_Edges.insert(std::make_pair(edgeDesc, newEdge));
        }

        if (y < iGridSize.Y() - 1)
        {
          cell.neighCount++;
          m_Neigh.push_back(cellOffset + iGridSize.X());

          auto edgeDesc = boost::add_edge(cellOffset, cellOffset + iGridSize.X(), m_Graph.m_CellGraph).first;
          VoronoiGraph::Edge newEdge;
          newEdge.m_Pt1 = gridLowLeftCorner + iGridSize.X() + 1;
          newEdge.m_Pt2 = gridLowLeftCorner + 1 + iGridSize.X() + 1;
          m_Graph.m_Edges[edgeDesc] = newEdge;
        }
        m_Cells.push_back(cell);
        ++cellOffset;
      }
    }

    m_Index = CellIndex(locs);
  }

  void Terrain::MakeCirclePacking(Random& iRand, Vector2f const& iSize, float iCellSize)
  {
    m_Size = iSize;

    m_QueryBox = AABB2Df(-iCellSize * 1.1, -iCellSize * 1.1, iCellSize * 1.1, iCellSize * 1.1);

    m_Cells.clear();
    m_Graph.m_CellGraph.clear();
    m_Graph.m_Edges.clear();
    m_Graph.m_Vertices.clear();
    m_Neigh.clear();
    m_Index.clear();

    Polygoni poly(AABB2Di(Vector2i::ZERO, Vector2i(Mathf::Ceil(m_Size.X()), Mathf::Ceil(m_Size.Y()))));
    PoissonDiskSampling sampler(poly, iRand);

    sampler.Sample(iCellSize, iCellSize);

    Vector<Vector2d> samples;
    Vector<Vector2f> cellCenter;
    sampler.GetLayer(0, samples);
    //sampler.GetLayer(1, samples[1]);
    //sampler.GetLayer(2, samples[2]);

    unsigned int const totSamples = samples.size() /*+ samples[1].size() + samples[2].size()*/;

    VoronoiGraph voroGr;

    for(unsigned int j = 0; j<samples.size(); ++j)
    {
      voroGr.AddCircle(iCellSize, MathTools::ToFVec(samples[j]));
      cellCenter.push_back(MathTools::ToFVec(samples[j]));

      CellProperties props;
      props.position = cellCenter.back();
      m_Cells.push_back(props);
    }

    voroGr.BuildGraph(m_Graph, true);

    {
      Vector<CellLoc> locs(cellCenter.size());
      for(unsigned int i = 0; i<cellCenter.size(); ++i)
      {
        Vector2f const& cellPos = cellCenter[i];
        locs.push_back(std::make_pair(AABB2Df(cellPos - Vector2f::ONE * iCellSize, Vector2f::ONE * 2 * iCellSize), i));
      }
      m_Index = CellIndex(locs);
    }

    unsigned int const numCells = m_Cells.size();
    Vector<unsigned int> sortedPoints;

    for (unsigned int i = 0; i < numCells; ++i)
    {
      m_Cells[i].neighCount = 0;
      sortedPoints.clear();
      Vector2f center = m_Cells[i].position;

      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);
      unsigned int numNeigh = std::distance(edgesBegin, edgesEnd);
      if(edgesBegin != edgesEnd)
      {
        unsigned int firstPoint = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        VoronoiGraph::Edge firstEdge = m_Graph.m_Edges.find(*edgesBegin)->second;
        ++edgesBegin;
        while(firstPoint == numCells)
        {
          --numNeigh;
          firstPoint = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
          firstEdge = m_Graph.m_Edges.find(*edgesBegin)->second;
          ++edgesBegin;
        }

        Segmentf::SortByAngle sortMeth(m_Cells[firstPoint].position);
        std::map<Vector2f, unsigned int, Segmentf::SortByAngle > sortedPointsMap(sortMeth);

        for (; edgesBegin != edgesEnd; ++edgesBegin)
        {
          unsigned int target = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
          if(target == numCells)
          {
            --numNeigh;
            continue;
          }
          sortedPointsMap.insert(std::make_pair(m_Cells[target].position, target));
        }
        sortedPoints.push_back(firstPoint);
        for(auto pair : sortedPointsMap)
        {
          sortedPoints.push_back(pair.second);
        }

        m_Cells[i].neighStart = m_Neigh.size();

        for(uint32_t curNeighIdx = 0; curNeighIdx < numNeigh; ++curNeighIdx)
        {
          unsigned int curNeigh = sortedPoints[curNeighIdx];

          m_Neigh.push_back(curNeigh);
        }

        m_Cells[i].neighCount = numNeigh;
      }
    }
  }

  bool Terrain::GetClosestCell(Vector2f const& iPos, uint32_t& oCellIdx)
  {
    AABB2Df queryBox = m_QueryBox;
    queryBox.m_Data[0] += iPos;
    queryBox.m_Data[1] += iPos;

    Vector<CellLoc> results;
    m_Index.query(boost::geometry::index::intersects(queryBox), std::back_inserter(results));

    if (results.empty())
    {
      return false;
    }

    float minDist = FLT_MAX;
    for (auto const& cellLoc : results)
    {
      CellProperties const& cell = m_Cells[cellLoc.second];
      float const distSq = (cell.position - iPos).SquaredLength();
      if (minDist > distSq)
      {
        oCellIdx = cellLoc.second;
        minDist = distSq;
      }
    }

    return true;
  }

  void Terrain::GetLineCells(Segmentf const& iSeg, float iRadius, Vector<unsigned int>& oCells)
  {
    oCells.clear();

    AABB2Df queryBox;
    queryBox.m_Data[0] = iSeg.m_Ext1;
    queryBox.m_Data[1] = iSeg.m_Ext1;
    queryBox.Absorb(iSeg.m_Ext2);
    queryBox.m_Data[0] -= Vector2f::ONE * iRadius;
    queryBox.m_Data[1] += Vector2f::ONE * iRadius;

    Vector<CellLoc> results;
    m_Index.query(boost::geometry::index::intersects(queryBox), std::back_inserter(results));

    for (auto const& cellLoc : results)
    {
      CellProperties const& cell = m_Cells[cellLoc.second];

      Vector2f dir;
      float dist = iSeg.NearestPointSeg(cell.position, dir);
      if (dist < iRadius)
      {
        oCells.push_back(cellLoc.second);
      }
    }
  }

  void Terrain::GetDiskCells(Vector2f const& iCenter, float iRadius, Vector<unsigned int>& oCells)
  {
    oCells.clear();

    AABB2Df queryBox(iCenter - iRadius * Vector2f::ONE, Vector2f::ONE * 2 * iRadius);

    Vector<CellLoc> results;
    m_Index.query(boost::geometry::index::intersects(queryBox), std::back_inserter(results));

    for(auto cell : results)
    {
      if((m_Cells[cell.second].position - iCenter).Length() < iRadius)
      {
        oCells.push_back(cell.second);
      }
    }
  }

  void Terrain::BuildLaplaceMatrix(ConductivityGetter& iConductivity, LaplaceMatrix*& oMat)
  {
    unsigned int const numCells = m_Cells.size();

    if (oMat != nullptr)
    {
      delete oMat;
    }

    ArmaLaplaceMatrix* matrix = eXl_NEW(ArmaLaplaceMatrix);

    oMat = matrix;

    arma::sp_mat& mat = matrix->m_Matrix;

    mat.set_size(numCells, numCells);

    //Vector<unsigned int> sortedPoints;
    //Vector<Vector2f> cellPoints;

    for (unsigned int i = 0; i < numCells; ++i)
    {
      //cellPoints.clear();
      //sortedPoints.clear();
      float sum = 0.0;

      Vector2f center = m_Cells[i].position;

      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);

      for (; edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int target = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if (target == numCells)
        {
          continue;
        }
        VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;

        float edgeLen = (m_Graph.m_Vertices[curEdge.m_Pt2].m_Position - m_Graph.m_Vertices[curEdge.m_Pt1].m_Position).Length();
        float cellDist = (center - m_Cells[target].position).Length();

        float curConnectionValue = iConductivity(i, target, edgeLen, cellDist);
        sum -= curConnectionValue;

        mat.at(i, target) = 0.5 * curConnectionValue;
      }

      mat.at(i, i) = 0.5 * sum;
    }
  }

  void Terrain::BuildSmoothingMatrix(LaplaceMatrix*& oMat)
  {
    unsigned int const numCells = m_Cells.size();

    if (oMat != nullptr)
    {
      delete oMat;
    }

    ArmaLaplaceMatrix* matrix = eXl_NEW(ArmaLaplaceMatrix);

    oMat = matrix;

    arma::sp_mat& mat = matrix->m_Matrix;

    mat.set_size(numCells, numCells);

    Vector<unsigned int> sortedPoints;
    Vector<Vector2f> cellPoints;

    for (unsigned int i = 0; i < numCells; ++i)
    {
      cellPoints.clear();
      sortedPoints.clear();
      Vector2f center = m_Cells[i].position;

      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);
      unsigned int numNeigh = std::distance(edgesBegin, edgesEnd);
      if(edgesBegin != edgesEnd)
      {
        unsigned int firstPoint = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        VoronoiGraph::Edge firstEdge = m_Graph.m_Edges.find(*edgesBegin)->second;
        ++edgesBegin;
        while(firstPoint == numCells)
        {
          --numNeigh;
          firstPoint = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
          firstEdge = m_Graph.m_Edges.find(*edgesBegin)->second;
          ++edgesBegin;
        }

        Segmentf::SortByAngle sortMeth(m_Cells[firstPoint].position);
        std::map<Vector2f, unsigned int, Segmentf::SortByAngle > sortedPointsMap(sortMeth);

        cellPoints.push_back(m_Graph.m_Vertices[firstEdge.m_Pt1].m_Position);
        cellPoints.push_back(m_Graph.m_Vertices[firstEdge.m_Pt2].m_Position);

        for (; edgesBegin != edgesEnd; ++edgesBegin)
        {
          unsigned int target = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
          if(target == numCells)
          {
            --numNeigh;
            continue;
          }
          VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;
          cellPoints.push_back(m_Graph.m_Vertices[curEdge.m_Pt1].m_Position);
          cellPoints.push_back(m_Graph.m_Vertices[curEdge.m_Pt2].m_Position);
          sortedPointsMap.insert(std::make_pair(m_Cells[target].position, target));
        }
        sortedPoints.push_back(firstPoint);
        for(auto pair : sortedPointsMap)
        {
          sortedPoints.push_back(pair.second);
        }

        Polygonf cellPoly;
        Polygonf::ConvexHull(cellPoints, cellPoly);

        float area = cellPoly.Area();
        float sum = 0.0;

        for(uint32_t curNeighIdx = 0; curNeighIdx < numNeigh; ++curNeighIdx)
        {
          float alphaI = 0.0;
          float betaI = 0.0;

          unsigned int curNeigh = sortedPoints[curNeighIdx];
          unsigned int prevNeigh = sortedPoints[curNeighIdx == 0 ? numNeigh - 1 : curNeighIdx - 1];
          unsigned int nextNeigh = sortedPoints[curNeighIdx == numNeigh - 1 ? 0 : curNeighIdx + 1];

          Vector2f next1 = m_Cells[curNeigh].position - m_Cells[nextNeigh].position;
          Vector2f next2 = center - m_Cells[nextNeigh].position;

          float crossA = Segmentf::Cross(next1, next2);
          if(Mathf::Abs(crossA) > Mathf::EPSILON)
          {
            alphaI = Mathf::Abs(next1.Dot(next2) / crossA);
          }

          Vector2f prev1 = center - m_Cells[prevNeigh].position;
          Vector2f prev2 = m_Cells[curNeigh].position - m_Cells[prevNeigh].position;

          float crossB = Segmentf::Cross(prev1, prev2);
          if(Mathf::Abs(crossB) > Mathf::EPSILON)
          {
            betaI = Mathf::Abs(prev1.Dot(prev2) / crossB);
          }
          float curConnectionValue = (alphaI + betaI / area);
          sum -= curConnectionValue;

          mat.at(i, curNeigh) = 0.5 * curConnectionValue;
        }

        mat.at(i, i) = 0.5 * sum;
      }
    }
  }

  void Terrain::Diffusion(LaplaceMatrix const* iMatrix, float iHeatCoefficient, Vector<float>& ioValues)
  {
    ArmaLaplaceMatrix const* matrixContainer = static_cast<ArmaLaplaceMatrix const*>(iMatrix);

    unsigned int const numCells = m_Cells.size();

    eXl_ASSERT(numCells == ioValues.size());

    arma::colvec inputTemp(numCells);
    for (unsigned int i = 0; i < numCells; ++i)
    {
      inputTemp.at(i) = ioValues[i];
    }

    arma::colvec tempRes(numCells, arma::fill::zeros);

    arma::sp_mat resEqn(numCells, numCells);
    arma::sp_mat idMat(numCells, numCells);
    for (unsigned int i = 0; i < numCells; ++i)
      idMat.at(i, i) = 1.0;

    resEqn = idMat - matrixContainer->m_Matrix * iHeatCoefficient;

    arma::spsolve(tempRes, resEqn, inputTemp);

    for (unsigned int i = 0; i < numCells; ++i)
    {
      ioValues[i] = tempRes.at(i);
    }
  }

  size_t Terrain::GetNumMeshVertices()
  {
    size_t numVtx = 2 * m_Graph.m_Edges.size();
    for (auto const& cell : m_Cells)
    {
      numVtx += 1 + cell.neighCount;
    }

    return numVtx;
  }

  size_t Terrain::GetNumMeshIndices()
  {
    return GetNumSmoothMeshIndices() /*+ 6 * m_Graph.m_Edges.size()*/;
  }

  size_t Terrain::GetNumSmoothMeshVertices()
  {
    return m_Cells.size() + m_Graph.m_Vertices.size();
  }

  size_t Terrain::GetNumSmoothMeshIndices()
  {
    size_t numIndices = 0;
    for (auto const& cell : m_Cells)
    {
      numIndices += 3 * cell.neighCount;
    }

    return numIndices;
  }

  void Terrain::BuildMesh(Vector3f const& iScale, const Vector<float>& iHeight, OutputBuffer oPositions, OutputBuffer oNormals, OutputBuffer oTexCoords, OutputBuffer oIdx)
  {
    uint32_t maxIndices = GetNumMeshIndices();
    uint32_t counterCheck = 0;

    Vector3f* positionsPtr = reinterpret_cast<Vector3f*>(oPositions.data);
    float* texCoordPtr = reinterpret_cast<float*>(oTexCoords.data);
    Vector3f* normalsPtr = reinterpret_cast<Vector3f*>(oNormals.data);
    uint32_t* indicesPtr = reinterpret_cast<uint32_t*>(oIdx.data);
    uint32_t addVtxCounter = 0;

    UnorderedMap<uint32_t, uint32_t> vtxAlloc;

    for (unsigned int i = 0; i < m_Cells.size(); ++i)
    {
      uint32_t cellCenterIdx = addVtxCounter;
      vtxAlloc.clear();
      Vector2f ptCenter = m_Cells[i].position;

      float height = iHeight[i];

      Vector3f pos(iScale.X() * ptCenter.X(), iScale.Y() * ptCenter.Y(), iScale.Z() * height);

      *positionsPtr = pos;
      texCoordPtr[0] = ptCenter.X() / m_Size.X();
      texCoordPtr[1] = ptCenter.Y() / m_Size.Y();
      *normalsPtr = Vector3f::UNIT_Z;

      oPositions.NextItem(positionsPtr);
      oTexCoords.NextItem(texCoordPtr);
      oNormals.NextItem(normalsPtr);
      ++addVtxCounter;

      VoronoiGraph::CellGraphImpl::vertex_descriptor curCell = i;
      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      unsigned int numPts = 0;
      for (boost::tie(edgesBegin, edgesEnd) = boost::out_edges(curCell, m_Graph.m_CellGraph); edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int target = curCell == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if (target >= m_Cells.size())
        {
          continue;
        }

        VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;

        auto insertRes = vtxAlloc.insert(std::make_pair(curEdge.m_Pt1, addVtxCounter));
        if (insertRes.second)
        {
          *positionsPtr = Vector3f(iScale.X() * m_Graph.m_Vertices[curEdge.m_Pt1].m_Position.X(),
            iScale.Y() * m_Graph.m_Vertices[curEdge.m_Pt1].m_Position.Y(),
            iScale.Z() * height);
          texCoordPtr[0] = m_Graph.m_Vertices[curEdge.m_Pt1].m_Position.X() / m_Size.X();
          texCoordPtr[1] = m_Graph.m_Vertices[curEdge.m_Pt1].m_Position.Y() / m_Size.Y();
          *normalsPtr = Vector3f::UNIT_Z;

          oPositions.NextItem(positionsPtr);
          oTexCoords.NextItem(texCoordPtr);
          oNormals.NextItem(normalsPtr);
          ++addVtxCounter;
        }
        insertRes = vtxAlloc.insert(std::make_pair(curEdge.m_Pt2, addVtxCounter));
        if (insertRes.second)
        {
          *positionsPtr = Vector3f(iScale.X() * m_Graph.m_Vertices[curEdge.m_Pt2].m_Position.X(),
            iScale.Y() * m_Graph.m_Vertices[curEdge.m_Pt2].m_Position.Y(),
            iScale.Z() * height);
          texCoordPtr[0] = m_Graph.m_Vertices[curEdge.m_Pt2].m_Position.X() / m_Size.X();
          texCoordPtr[1] = m_Graph.m_Vertices[curEdge.m_Pt2].m_Position.Y() / m_Size.Y();
          *normalsPtr = Vector3f::UNIT_Z;

          oPositions.NextItem(positionsPtr);
          oTexCoords.NextItem(texCoordPtr);
          oNormals.NextItem(normalsPtr);
          ++addVtxCounter;
        }
      }

      for (boost::tie(edgesBegin, edgesEnd) = boost::out_edges(curCell, m_Graph.m_CellGraph); edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int target = curCell == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if (target >= m_Cells.size())
        {
          continue;
        }

        VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;

        uint32_t pt1Idx = vtxAlloc[curEdge.m_Pt1];
        uint32_t pt2Idx = vtxAlloc[curEdge.m_Pt2];

        Vector3f const& otherPt1 = *oPositions.Item<Vector3f>(pt1Idx);
        Vector3f const& otherPt2 = *oPositions.Item<Vector3f>(pt2Idx);

        Vector3f locNormal = (pos - otherPt1).Cross(pos - otherPt2);

        if (locNormal.Dot(Vector3f::UNIT_Z) < -Mathf::EPSILON)
        {
          std::swap(pt1Idx, pt2Idx);
        }

        eXl_ASSERT(counterCheck < maxIndices);

        *indicesPtr = cellCenterIdx;
        oIdx.NextItem(indicesPtr);
        *indicesPtr = pt1Idx;
        oIdx.NextItem(indicesPtr);
        *indicesPtr = pt2Idx;
        oIdx.NextItem(indicesPtr);

        counterCheck += 3;
      }
    }

    for (unsigned int i = 0; i < m_Graph.m_Edges.size(); ++i)
    {
      //unsigned int numRef = 0;
      //float height = 0.0;
      //for (auto ref : edgePtRef[i])
      //{
      //  height += oPositions.Item<float>(ref)[2];
      //  ++numRef;
      //}
      //
      //if (numRef > 0)
      //  height /= numRef;
      //
      //
      //positionsPtr[0] = iScale.X() * m_Graph.m_Vertices[i].m_Position.X();
      //positionsPtr[1] = iScale.Y() * m_Graph.m_Vertices[i].m_Position.Y();
      //positionsPtr[2] = height;
      //texCoordPtr[0] = m_Graph.m_Vertices[i].m_Position.X() / m_Size.X();
      //texCoordPtr[1] = m_Graph.m_Vertices[i].m_Position.Y() / m_Size.Y();
      //*normalsPtr = Vector3f::ZERO;
      //
      //oPositions.NextItem(positionsPtr);
      //oTexCoords.NextItem(texCoordPtr);
      //oNormals.NextItem(normalsPtr);
    }
  }

  void Terrain::BuildSmoothMesh(Vector3f const& iScale, const Vector<float>& iHeight, OutputBuffer oPositions, OutputBuffer oNormals, OutputBuffer oTexCoords, OutputBuffer oIdx)
  {
    unsigned int const numCells = m_Cells.size();

    Vector<Set<unsigned int> > edgePtRef(m_Graph.m_Vertices.size());

    float* positionsPtr = reinterpret_cast<float*>(oPositions.data);
    float* texCoordPtr = reinterpret_cast<float*>(oTexCoords.data);
    uint32_t* indicesPtr = reinterpret_cast<uint32_t*>(oIdx.data);
    Vector3f* normalsPtr = reinterpret_cast<Vector3f*>(oNormals.data);

    for (unsigned int i = 0; i < numCells; ++i)
    {
      Vector2f ptCenter = m_Cells[i].position;

      float height = iHeight[i];

      positionsPtr[0] = iScale.X() * ptCenter.X();
      positionsPtr[1] = iScale.Y() * ptCenter.Y();
      positionsPtr[2] = iScale.Z() * height;
      texCoordPtr[0] = ptCenter.X() / m_Size.X();
      texCoordPtr[1] = ptCenter.Y() / m_Size.Y();

      VoronoiGraph::CellGraphImpl::vertex_descriptor curCell = i;
      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      unsigned int numPts = 0;
      for (boost::tie(edgesBegin, edgesEnd) = boost::out_edges(curCell, m_Graph.m_CellGraph); edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int target = curCell == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if (target >= m_Cells.size())
        {
          continue;
        }

        VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;

        uint32_t pt1Idx = numCells + curEdge.m_Pt1;
        uint32_t pt2Idx = numCells + curEdge.m_Pt2;

        *indicesPtr = i;
        oIdx.NextItem(indicesPtr);
        *indicesPtr = pt1Idx;
        oIdx.NextItem(indicesPtr);
        *indicesPtr = pt2Idx;
        oIdx.NextItem(indicesPtr);

        edgePtRef[curEdge.m_Pt1].insert(i);
        edgePtRef[curEdge.m_Pt2].insert(i);
      }

      *normalsPtr = Vector3f::ZERO;

      oPositions.NextItem(positionsPtr);
      oTexCoords.NextItem(texCoordPtr);
      oNormals.NextItem(normalsPtr);
    }

    for(unsigned int i = 0; i<edgePtRef.size(); ++i)
    {
      unsigned int numRef = 0;
      float height = 0.0;
      for(auto ref : edgePtRef[i])
      {
        height += oPositions.Item<float>(ref)[2];
        ++numRef;
      }

      if(numRef > 0)
        height /= numRef;

      positionsPtr[0] = iScale.X() * m_Graph.m_Vertices[i].m_Position.X();
      positionsPtr[1] = iScale.Y() * m_Graph.m_Vertices[i].m_Position.Y();
      positionsPtr[2] = height;
      texCoordPtr[0] = m_Graph.m_Vertices[i].m_Position.X() / m_Size.X();
      texCoordPtr[1] = m_Graph.m_Vertices[i].m_Position.Y() / m_Size.Y();
      *normalsPtr = Vector3f::ZERO;

      oPositions.NextItem(positionsPtr);
      oTexCoords.NextItem(texCoordPtr);
      oNormals.NextItem(normalsPtr);
    }

    indicesPtr = reinterpret_cast<uint32_t*>(oIdx.data);
    for (unsigned int i = 0; i < numCells; ++i)
    {
      Vector3f const& pos = *oPositions.Item<Vector3f>(i);

      VoronoiGraph::CellGraphImpl::vertex_descriptor curCell = i;
      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      unsigned int numPts = 0;
      Vector3f normal;
      Set<unsigned int> pts;
      for (boost::tie(edgesBegin, edgesEnd) = boost::out_edges(curCell, m_Graph.m_CellGraph); edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int target = curCell == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if (target >= m_Cells.size())
        {
          continue;
        }

        VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;

        Vector3f const& otherPt1 = *oPositions.Item<Vector3f>(curEdge.m_Pt1 + numCells);
        Vector3f const& otherPt2 = *oPositions.Item<Vector3f>(curEdge.m_Pt2 + numCells);

        Vector3f locNormal = (pos - otherPt1).Cross(pos - otherPt2);

        if (locNormal.Dot(Vector3f::UNIT_Z) < -Mathf::EPSILON)
        {
          locNormal *= -1.0;
          std::swap(indicesPtr[1], indicesPtr[2]);
        }

        *oNormals.Item<Vector3f>(i) += locNormal;
        *oNormals.Item<Vector3f>(curEdge.m_Pt1 + numCells) += locNormal;
        *oNormals.Item<Vector3f>(curEdge.m_Pt2 + numCells) += locNormal;

        oIdx.NextItem(indicesPtr);
        oIdx.NextItem(indicesPtr);
        oIdx.NextItem(indicesPtr);
      }
    }

    normalsPtr = reinterpret_cast<Vector3f*>(oNormals.data);
    for (unsigned int i = 0; i < numCells + m_Graph.m_Vertices.size(); ++i)
    {
      normalsPtr->Normalize();
      oNormals.NextItem(normalsPtr);
    }
  }

  void Terrain::ComputeFlowMap(const Vector<float>& iHeight, Vector<unsigned int>& oSummits, Vector<int>& oNext)
  {
    unsigned int const numCells = m_Cells.size();

    oSummits.clear();
    oNext.resize(numCells, -1);

    Vector<float> filledHeight(numCells, Mathf::MAX_REAL);

    for (unsigned int i = 0; i < numCells; ++i)
    {
      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);
      for(; edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int curNeigh = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;

        if(curNeigh == numCells)
        {
          filledHeight[i] = iHeight[i];
          break;
        }
      }
    }

    bool updated;
    do
    {
      updated = false;

      for (unsigned int i = 0; i < numCells; ++i)
      {
        if(iHeight[i] == filledHeight[i])
          continue;

        float curOrigH = iHeight[i];

        VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
        boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);
        for(; edgesBegin != edgesEnd; ++edgesBegin)
        {
          unsigned int curNeigh = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;

          eXl_ASSERT(curNeigh < numCells);

          float neighH = filledHeight[curNeigh];
          if(curOrigH >= neighH + Mathf::ZERO_TOLERANCE)
          {
            //C'est bon on a un voisin plus bas
            filledHeight[i] = curOrigH;
            updated = true;
          }
          else
          {
            float newH = neighH + Mathf::ZERO_TOLERANCE;
            if(filledHeight[i] > newH && (newH > curOrigH))
            {
              filledHeight[i] = newH;
              updated = true;
            }
          }
        }
      }
    }while(updated);

    for (unsigned int i = 0; i < numCells; ++i)
    {
      int lowerN = -1;
      float curLowerH = Mathf::MAX_REAL;
      float curH = filledHeight[i];
      bool summit = true;

      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);
      for(; edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int curNeigh = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if(curNeigh == numCells)
          continue;

        float neighH = filledHeight[curNeigh];
        if(curH > neighH)
        {
          if(neighH < curLowerH)
          {
            lowerN = curNeigh;
            curLowerH = neighH;
          }
        }
        else
          summit = false;
      }

      if(summit)
        oSummits.push_back(i);

      oNext[i] = lowerN;
    }
  }

  //void Terrain::ComputeNormals(Vector3f const& iScale, const Vector<float>& iHeight, Vector<Vector3f>& oNormals)
  //{
  //  unsigned int const numCells = m_Cells.size();
  //  oNormals.resize(numCells);
  //
  //  for (unsigned int i = 0; i < numCells; ++i)
  //  {
  //    Vector3f pos(m_Cells[i].position.X() * iScale.X(), iHeight[i] * iScale.Y(), m_Cells[i].position.Y() * iScale.Z());
  //
  //    unsigned int neighStart = m_Cells[i].neighStart;
  //    unsigned int neighEnd = m_Cells[i].neighCount + neighStart;
  //
  //    Vector3f normal;
  //    if(neighEnd != neighStart)
  //    {
  //      unsigned int prevNeigh = m_Neigh[neighEnd - 1];
  //      Vector3f prevPt(m_Cells[prevNeigh].position.X() * iScale.X(), iHeight[prevNeigh] * iScale.Y(), m_Cells[prevNeigh].position.Y() * iScale.Z());
  //
  //      for (unsigned int neighIdx = neighStart; neighIdx < neighEnd; ++neighIdx)
  //      {
  //        unsigned int curNeigh = m_Neigh[neighIdx];
  //        Vector3f curPt(m_Cells[curNeigh].position.X() * iScale.X(), iHeight[curNeigh] * iScale.Y(), m_Cells[curNeigh].position.Y() * iScale.Z());
  //        Vector3f locNormal = (pos - prevPt).Cross(pos - curPt);
  //
  //        if(locNormal.Dot(Vector3f::UNIT_Y) < -Mathf::EPSILON)
  //          locNormal *= -1.0;
  //
  //        normal += locNormal;
  //        prevPt = curPt;
  //      }
  //    }
  //    normal.Normalize();
  //    oNormals[i] = normal;
  //  }
  //}

}


// Copyright (C) 2018 Thanaphon Chavengsaksongkram <as12production@gmail.com>, He Sun <he.sun@ed.ac.uk>
// This file is subject to the license terms in the LICENSE file
// found in the top-level directory of this distribution.

#ifndef GSPARSE_ER_POLICY_APROXERSLMJACOBICG_HPP
#define GSPARSE_ER_POLICY_APROXERSLMJACOBICG_HPP

#include "../../Config.hpp"
#include "../../Util/JL.hpp"  // Building Random Projection

#include <Eigen/Dense>
#include <Eigen/Sparse>
namespace gSparse
{
    namespace ER
    {
        namespace Policy
        {
            /// \ingroup EffectiveResistance
            ///
            /// This class Approximate Effective Weight Resistance
            /// Adaptation from http://ccom.uprrp.edu/~ikoutis/SpectralAlgorithms.htm.
            /// The algorithm leverages Conjugated Graident with Jacobi preconditioner to solve linear system
            ///
            class AproxERSLMJacobiCG
            {
            protected:
                /// This function calculates Effective Resistance and return computation status.
                /// \param er A row matrix to receive the EffectiveResistance value
                /// \param graph A std::shared_ptr<IGraph> object representing the graph to calculate resistance
                /// \param eps Error tolerance for conjugated gradient. Default is 1.0f.
                /// \param JLTol Tolerance for JL projection Matrix. Default is 0.5f. (See http://ccom.uprrp.edu/~ikoutis/SpectralAlgorithms.htm.)
                /// \param maxIter  Maximum iteration for conjugated gradient. Default is 300 iterations.
                inline gSparse::COMPUTE_INFO _calculateER(
                    gSparse::PrecisionRowMatrix & er,
                    const gSparse::Graph & graph,
                    double eps = 1.0f,
                    double JLTol = 0.5,
                    int maxIter = 300
                    )
                {
                    er = gSparse::PrecisionRowMatrix::Zero(graph->GetEdgeCount(), 1);

                    std::size_t scale = static_cast<size_t>(
                                std::ceil(
                                std::log2(
                                static_cast<double>(graph->GetIncidentMatrix().cols()) / eps)));

                    for (int i = 1; i != scale + 1; ++i)
                    {
                        Eigen::VectorXd x;
                        gSparse::PrecisionMatrix Q =
                        gSparse::Util::randomProjectionMatrix(1,
                                                                graph->GetIncidentMatrix().rows(),
                                                                static_cast<double>(scale),
                                                                JLTol);

                        gSparse::PrecisionMatrix Y = (Q * graph->GetWeightMatrix().cwiseSqrt() * graph->GetIncidentMatrix());

                        // solve Linear system with 300 max iteration
                        Eigen::ConjugateGradient<gSparse::SparsePrecisionMatrix, Eigen::Lower | Eigen::Upper  > cg;
                        cg.setMaxIterations(maxIter);
                        x = cg.compute(graph->GetLaplacianMatrix()).solve(Y.transpose());

                        if (cg.info() != Eigen::Success)
                        {
                            // Does not converge this iteration. Keeps going.
                            continue;
                        }
                        for (std::size_t j = 0; j != graph->GetEdgeCount(); ++j)
                        {
                            er(j) += pow(std::abs(x(graph->GetEdgeList()(j, 0)) - x(graph->GetEdgeList()(j, 1))), 2.0f);
                        }
                    }
                    // Non finite element goes to zero
                    er = er.unaryExpr([](double v) { return std::isfinite(v)? v : 0.0; });
                    if (er.rows() != graph->GetEdgeCount())
                        return gSparse::NOT_CONVERGING;
                    return gSparse::SUCCESSFUL;
                }
            };
        }
    }
}
#endif


#include <ql/quantlib.hpp>

#include <boost/make_shared.hpp>

// example / tests for multicurrency lgm model

using namespace QuantLib;

void nodelete() {}

int main() {

    try {

        Date referenceDate(30, July, 2015);

        Settings::instance().evaluationDate() = referenceDate;

        // the single currency models
        // they can be calibrated in the usual way

        Handle<YieldTermStructure> eurYts(boost::make_shared<FlatForward>(
            referenceDate, 0.02, Actual365Fixed()));

        Handle<YieldTermStructure> usdYts(boost::make_shared<FlatForward>(
            referenceDate, 0.05, Actual365Fixed()));

        std::vector<Date> volstepdates;
        std::vector<Real> volsteptimes;
        Array volsteptimes_a(0);
        std::vector<Real> eurVols(1, atof(getenv("EURVOL")));
        std::vector<Real> usdVols(1, atof(getenv("USDVOL")));
        std::vector<Real> fxSigmas(1, atof(getenv("FXVOL")));
        Array fxSigmas_a(fxSigmas.begin(), fxSigmas.end());

        boost::shared_ptr<Lgm1> eurLgm = boost::make_shared<Lgm1>(
            eurYts, volstepdates, eurVols, atof(getenv("EURMR")));
        boost::shared_ptr<Lgm1> usdLgm = boost::make_shared<Lgm1>(
            usdYts, volstepdates, usdVols, atof(getenv("USDMR")));

        std::vector<boost::shared_ptr<Lgm1> > singleModels;
        singleModels.push_back(eurLgm);
        singleModels.push_back(usdLgm);

        std::vector<Handle<YieldTermStructure> > curves;
        curves.push_back(eurYts);
        curves.push_back(usdYts);

        // build cc parametrization from scratch

        // lgm parametrizations

        std::vector<boost::shared_ptr<detail::LgmParametrization<
            detail::LgmPiecewiseAlphaConstantKappa> > > lgmParametrizations;

        boost::shared_ptr<
            detail::LgmParametrization<detail::LgmPiecewiseAlphaConstantKappa> >
            eurParam = eurLgm->parametrization();
        boost::shared_ptr<
            detail::LgmParametrization<detail::LgmPiecewiseAlphaConstantKappa> >
            usdParam = usdLgm->parametrization();

        lgmParametrizations.push_back(eurParam);
        lgmParametrizations.push_back(usdParam);

        // fx parametrizations

        std::vector<boost::shared_ptr<detail::LgmFxParametrization<
            detail::LgmFxPiecewiseSigma> > > fxParametrizations;

        boost::shared_ptr<detail::LgmFxParametrization<
            detail::LgmFxPiecewiseSigma> > fxParam =
            boost::make_shared<detail::LgmFxPiecewiseSigma>(volsteptimes_a,
                                                            fxSigmas_a);

        fxParametrizations.push_back(fxParam);

        // the fx vols, correlations and the cclgmm parametrization / process /
        // model

        std::vector<Handle<Quote> > fxSpots;
        fxSpots.push_back(Handle<Quote>(boost::make_shared<SimpleQuote>(
            std::log(0.9090)))); // EUR-USD ~ 1.10

        Matrix c(3, 3);
        //  FX             EUR         USD
        c[0][0] = 1.0; c[0][1] = 0.99; c[0][2] = 0.99; // FX
        c[1][0] = 0.99; c[1][1] = 1.0; c[1][2] = 0.99; // EUR
        c[2][0] = 0.99; c[2][1] = 0.99; c[2][2] = 1.0; // USD

        boost::shared_ptr<detail::CcLgmPiecewise> ccParam =
            boost::make_shared<detail::CcLgmPiecewise>(fxParametrizations,
                                                       lgmParametrizations, c);

        ccParam->update();

        // test parametrization

        std::clog.precision(12);
        // std::clog << "H0(0.0) = " << ccParam->H_i(0,0.0) << std::endl;
        // std::clog << "H0(1.0) = " << ccParam->H_i(0,1.0) << std::endl;
        // std::clog << "H0(2.0) = " << ccParam->H_i(0,2.0) << std::endl;
        // std::clog << "H1(0.0) = " << ccParam->H_i(1,0.0) << std::endl;
        // std::clog << "H1(1.0) = " << ccParam->H_i(1,1.0) << std::endl;
        // std::clog << "H1(2.0) = " << ccParam->H_i(1,2.0) << std::endl;
        // std::clog << "zeta0(0.0) = " << ccParam->zeta_i(0,0.0) << std::endl;
        // std::clog << "zeta0(1.0) = " << ccParam->zeta_i(0,1.0) << std::endl;
        // std::clog << "zeta0(2.0) = " << ccParam->zeta_i(0,2.0) << std::endl;
        // std::clog << "zeta0(3.0) = " << ccParam->zeta_i(0,3.0) << std::endl;
        // std::clog << "zeta1(0.0) = " << ccParam->zeta_i(1,0.0) << std::endl;
        // std::clog << "zeta1(1.0) = " << ccParam->zeta_i(1,1.0) << std::endl;
        // std::clog << "zeta1(2.0) = " << ccParam->zeta_i(1,2.0) << std::endl;
        // std::clog << "zeta1(3.0) = " << ccParam->zeta_i(1,3.0) << std::endl;
        // std::clog << "alphaialphaj(0.0) = " <<
        // ccParam->alpha_i_alpha_j(0,0,0.0) << std::endl;
        // std::clog << "alphaialphaj(1.0) = " <<
        // ccParam->alpha_i_alpha_j(0,0,1.0) << std::endl;
        // std::clog << "alphaialphaj(2.0) = " <<
        // ccParam->alpha_i_alpha_j(0,0,2.0) << std::endl;
        // std::clog << "alphaialphaj(0.0) = " <<
        // ccParam->alpha_i_alpha_j(1,1,0.0) << std::endl;
        // std::clog << "alphaialphaj(1.0) = " <<
        // ccParam->alpha_i_alpha_j(1,1,1.0) << std::endl;
        // std::clog << "alphaialphaj(2.0) = " <<
        // ccParam->alpha_i_alpha_j(1,1,2.0) << std::endl;
        // std::clog << "alphaialphaj(0.0) = " <<
        // ccParam->alpha_i_alpha_j(0,1,0.0) << std::endl;
        // std::clog << "alphaialphaj(1.0) = " <<
        // ccParam->alpha_i_alpha_j(0,1,1.0) << std::endl;
        // std::clog << "alphaialphaj(2.0) = " <<
        // ccParam->alpha_i_alpha_j(0,1,2.0) << std::endl;
        // std::clog << "alphaialphaj(0.0) = " <<
        // ccParam->alpha_i_alpha_j(1,0,0.0) << std::endl;
        // std::clog << "alphaialphaj(1.0) = " <<
        // ccParam->alpha_i_alpha_j(1,0,1.0) << std::endl;
        // std::clog << "alphaialphaj(2.0) = " <<
        // ccParam->alpha_i_alpha_j(1,0,2.0) << std::endl;
        // std::clog << "sigmaisigmaj(0.0) = " <<
        // ccParam->sigma_i_sigma_j(0,0,0.0) << std::endl;
        // std::clog << "sigmaisigmaj(1.0) = " <<
        // ccParam->sigma_i_sigma_j(0,0,1.0) << std::endl;
        // std::clog << "sigmaisigmaj(2.0) = " <<
        // ccParam->sigma_i_sigma_j(0,0,2.0) << std::endl;
        // std::clog << "alphaisigmaj(0.0) = " <<
        // ccParam->alpha_i_sigma_j(0,0,0.0) << std::endl;
        // std::clog << "alphaisigmaj(1.0) = " <<
        // ccParam->alpha_i_sigma_j(0,0,1.0) << std::endl;
        // std::clog << "alphaisigmaj(2.0) = " <<
        // ccParam->alpha_i_sigma_j(0,0,2.0) << std::endl;
        // std::clog << "alphaisigmaj(0.0) = " <<
        // ccParam->alpha_i_sigma_j(1,0,0.0) << std::endl;
        // std::clog << "alphaisigmaj(1.0) = " <<
        // ccParam->alpha_i_sigma_j(1,0,1.0) << std::endl;
        // std::clog << "alphaisigmaj(2.0) = " <<
        // ccParam->alpha_i_sigma_j(1,0,2.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(0,0,0.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(0,0,1.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(0,0,2.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,1,0.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,1,1.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,1,2.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,0,0.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,0,1.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,0,2.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(0,0,0.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(0,0,1.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(0,0,2.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,1,0.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,1,1.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,1,2.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,0,0.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,0,1.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,0,2.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(0,0,0.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(0,0,1.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(0,0,2.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(1,0,0.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(1,0,1.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(1,0,2.0) << std::endl;
        // std::clog << "int_alphaialphaj(0.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_alphaialphaj(1.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_alphaialphaj(2.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_alphaialphaj(0.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,1,0.0,0.0) << std::endl;
        // std::clog << "int_alphaialphaj(1.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,1,0.0,1.0) << std::endl;
        // std::clog << "int_alphaialphaj(2.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,1,0.0,2.0) << std::endl;
        // std::clog << "int_alphaialphaj(0.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,1,0.0,0.0) << std::endl;
        // std::clog << "int_alphaialphaj(1.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,1,0.0,1.0) << std::endl;
        // std::clog << "int_alphaialphaj(2.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,1,0.0,2.0) << std::endl;
        // std::clog << "int_alphaialphaj(0.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,0,0.0,0.0) << std::endl;
        // std::clog << "int_alphaialphaj(1.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,0,0.0,1.0) << std::endl;
        // std::clog << "int_alphaialphaj(2.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,0,0.0,2.0) << std::endl;
        // std::clog << "int_sigmaisigmaj(0.0) = " <<
        // ccParam->int_sigma_i_sigma_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_sigmaisigmaj(1.0) = " <<
        // ccParam->int_sigma_i_sigma_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_sigmaisigmaj(2.0) = " <<
        // ccParam->int_sigma_i_sigma_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_alphaisigmaj(0.0) = " <<
        // ccParam->int_alpha_i_sigma_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_alphaisigmaj(1.0) = " <<
        // ccParam->int_alpha_i_sigma_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_alphaisigmaj(2.0) = " <<
        // ccParam->int_alpha_i_sigma_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_alphaisigmaj(0.0) = " <<
        // ccParam->int_alpha_i_sigma_j(1,0,0.0,0.0) << std::endl;
        // std::clog << "int_alphaisigmaj(1.0) = " <<
        // ccParam->int_alpha_i_sigma_j(1,0,0.0,1.0) << std::endl;
        // std::clog << "int_alphaisigmaj(2.0) = " <<
        // ccParam->int_alpha_i_sigma_j(1,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,1,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,1,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,1,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,1,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,1,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,1,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,1,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,1,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,1,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,1,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,1,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,1,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(1,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(1,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(1,0,0.0,2.0) << std::endl;

        // std::clog << "rho alpha-alpha 00" << ccParam->rho_alpha_alpha(0,0) <<
        // std::endl;
        // std::clog << "rho alpha-alpha 01" << ccParam->rho_alpha_alpha(0,1) <<
        // std::endl;
        // std::clog << "rho alpha-alpha 10" << ccParam->rho_alpha_alpha(1,0) <<
        // std::endl;
        // std::clog << "rho alpha-alpha 11" << ccParam->rho_alpha_alpha(1,1) <<
        // std::endl;
        // std::clog << "rho alpha-sigma 00" << ccParam->rho_alpha_sigma(0,0) <<
        // std::endl;
        // std::clog << "rho alpha-sigma 10" << ccParam->rho_alpha_sigma(1,0) <<
        // std::endl;
        // std::clog << "rho sigma-sigma 00" << ccParam->rho_sigma_sigma(0,0) <<
        // std::endl;

        // end test parametrization

        boost::shared_ptr<
            CcLgmProcess<detail::CcLgmPiecewise, detail::LgmFxPiecewiseSigma,
                         detail::LgmPiecewiseAlphaConstantKappa> > process =
            boost::make_shared<CcLgmProcess<
                detail::CcLgmPiecewise, detail::LgmFxPiecewiseSigma,
                detail::LgmPiecewiseAlphaConstantKappa> >(ccParam, fxSpots,
                                                          curves);

        // generate paths

        Size n = atoi(getenv("N")); // N paths
        Time T = atof(getenv("T")); // cashflow time
        Size steps = static_cast<Size>(
            T * atof(getenv("STEPS")));   // STEPS steps per year
        Size seed = atoi(getenv("SEED")); // rng seed
        TimeGrid grid(T, steps);

        PseudoRandom::rsg_type sg =
            PseudoRandom::make_sequence_generator(steps * 3, seed);
        MultiPathGenerator<PseudoRandom::rsg_type> pg(process, grid, sg, false);

        PseudoRandom::rsg_type sg2 =
            PseudoRandom::make_sequence_generator(steps, seed);
        PathGenerator<PseudoRandom::rsg_type> pg2(usdLgm->stateProcess(), grid,
                                                  sg2, false);

        std::vector<Sample<MultiPath> > paths;
        for (Size j = 0; j < n; ++j) {
            paths.push_back(pg.next());
        }

        std::vector<Sample<Path> > paths2;
        for (Size j = 0; j < n; ++j) {
            paths2.push_back(pg2.next());
        }

        // output paths for visual inspection in gnuplot

        if (atoi(getenv("OUTPUT"))) {
            // cc model paths
            for (Size i = 0; i < paths[0].value[0].length(); ++i) {
                std::cout << grid[i] << " ";
                for (Size j = 0; j < n; ++j) {
                    std::cout << std::exp(paths[j].value[0][i]) << " "
                              << paths[j].value[1][i] << " "
                              << paths[j].value[2][i] << " "
                              << paths2[j].value[i] << " ";
                }
                std::cout << "\n";
            }
        }

        // test: 1 USD in 1y, priced in domestic measure

        Size l = paths[0].value[0].length() - 1;
        IncrementalStatistics stat, stat2;
        for (Size j = 0; j < n; ++j) {
            Real fx = std::exp(paths[j].value[0][l]);
            Real zeur = paths[j].value[1][l];
            Real zusd = paths[j].value[2][l];
            Real zusd2 = paths2[j].value[l];
            Real stddev = eurLgm->stateProcess()->stdDeviation(0.0, 0.0, T);
            Real stddev2 = usdLgm->stateProcess()->stdDeviation(0.0, 0.0, T);
            Real y = (zeur - eurLgm->stateProcess()->expectation(0.0, 0.0, T)) /
                     (!close_enough(stddev, 0.0) ? stddev : 1.0);
            Real y2 =
                (zusd2 - usdLgm->stateProcess()->expectation(0.0, 0.0, T)) /
                (!close_enough(stddev2, 0.0) ? stddev2 : 1.0);
            stat.add(1.0 * fx / eurLgm->numeraire(T, y));
            stat2.add(1.0 / usdLgm->numeraire(T, y2));
        }
        std::clog << "1 USD @ 1y  = " << stat.mean() << " EUR +/- "
                  << stat.errorEstimate() << std::endl;
        ;
        std::clog << "curve price = " << usdYts->discount(T) << " spot "
                  << std::exp(fxSpots[0]->value()) << " EUR price "
                  << usdYts->discount(T) * std::exp(fxSpots[0]->value())
                  << "\n";

        std::clog << "1 USD @ 1y = " << stat2.mean() << " USD +/-"
                  << stat2.errorEstimate() << std::endl;

        return 0;

    } catch (QuantLib::Error e) {
        std::clog << "ql exception : " << e.what() << "\n";
    } catch (std::exception e) {
        std::clog << "std exception: " << e.what() << "\n";
    }
}


//          Copyright W.P. McNeill 2010.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// This program uses the A-star search algorithm in the Boost Graph Library to
// solve a maze.  It is an example of how to apply Boost Graph Library
// algorithms to implicit graphs.
//
// This program generates a random maze and then tries to find the shortest
// path from the lower left-hand corner to the upper right-hand corner.  Mazes
// are represented by two-dimensional grids where a cell in the grid may
// contain a barrier.  You may move up, down, right, or left to any adjacent
// cell that does not contain a barrier.
//
// Once a maze solution has been attempted, the maze is printed.  If a
// solution was found it will be shown in the maze printout and its length
// will be returned.  Note that not all mazes have solutions.
//
// The default maze size is 20x10, though different dimensions may be
// specified on the command line.

#include <boost/graph/astar_search.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#include <ctime>
#include <iostream>

boost::mt19937 random_generator;

// Distance traveled in the maze
typedef double distance;

#define GRID_RANK 2
typedef boost::grid_graph<GRID_RANK> grid;
typedef boost::graph_traits<grid>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<grid>::vertices_size_type vertices_size_type;

// A hash function for vertices.
struct vertex_hash:std::unary_function<vertex_descriptor, std::size_t> {
  std::size_t operator()(vertex_descriptor const& u) const {
    std::size_t seed = 0;
    boost::hash_combine(seed, u[0]);
    boost::hash_combine(seed, u[1]);
    return seed;
  }
};

typedef boost::unordered_set<vertex_descriptor, vertex_hash> vertex_set;
typedef boost::vertex_subset_complement_filter<grid, vertex_set>::type
        filtered_grid;

// A searchable maze
//
// The maze is grid of locations which can either be empty or contain a
// barrier.  You can move to an adjacent location in the grid by going up,
// down, left and right.  Moving onto a barrier is not allowed.  The maze can
// be solved by finding a path from the lower-left-hand corner to the
// upper-right-hand corner.  If no open path exists between these two
// locations, the maze is unsolvable.
//
// The maze is implemented as a filtered grid graph where locations are
// vertices.  Barrier vertices are filtered out of the graph.
//
// A-star search is used to find a path through the maze. Each edge has a
// weight of one, so the total path length is equal to the number of edges
// traversed.
class maze {
public:
  friend std::ostream& operator<<(std::ostream&, const maze&);
  friend maze random_maze(std::size_t, std::size_t);

  maze():m_grid(create_grid(0, 0)),m_barrier_grid(create_barrier_grid()) {};
  maze(std::size_t x, std::size_t y):m_grid(create_grid(x, y)),
       m_barrier_grid(create_barrier_grid()) {};

  // The length of the maze along the specified dimension.
  vertices_size_type length(std::size_t d) const {return m_grid.length(d);}

  bool has_barrier(vertex_descriptor u) const {
    return m_barriers.find(u) != m_barriers.end();
  }

  // Try to find a path from the lower-left-hand corner source (0,0) to the
  // upper-right-hand corner goal (x-1, y-1).
  vertex_descriptor source() const {return vertex(0, m_grid);}
  vertex_descriptor goal() const {
    return vertex(num_vertices(m_grid)-1, m_grid);
  }

  bool solve();
  bool solved() const {return !m_solution.empty();}
  bool solution_contains(vertex_descriptor u) const {
    return m_solution.find(u) != m_solution.end();
  }

private:
  // Create the underlying rank-2 grid with the specified dimensions.
  grid create_grid(std::size_t x, std::size_t y) {
    boost::array<std::size_t, GRID_RANK> lengths = { {x, y} };
    return grid(lengths);
  }

  // Filter the barrier vertices out of the underlying grid.
  filtered_grid create_barrier_grid() {
    return boost::make_vertex_subset_complement_filter(m_grid, m_barriers);
  }

  // The grid underlying the maze
  grid m_grid;
  // The underlying maze grid with barrier vertices filtered out
  filtered_grid m_barrier_grid;
  // The barriers in the maze
  vertex_set m_barriers;
  // The vertices on a solution path through the maze
  vertex_set m_solution;
  // The length of the solution path
  distance m_solution_length;
};

// Euclidean heuristic for a grid
//
// This calculates the Euclidean distance between a vertex and a goal
// vertex.
class euclidean_heuristic:
      public boost::astar_heuristic<filtered_grid, double>
{
public:
  euclidean_heuristic(vertex_descriptor goal):m_goal(goal) {};

  double operator()(vertex_descriptor v) {
    return sqrt(pow(m_goal[0] - v[0], 2) + pow(m_goal[1] - v[1], 2));
  }

private:
  vertex_descriptor m_goal;
};

// Exception thrown when the goal vertex is found
struct found_goal {};

// Visitor that terminates when we find the goal vertex
struct astar_goal_visitor:public boost::default_astar_visitor {
  astar_goal_visitor(vertex_descriptor goal):m_goal(goal) {};

  void examine_vertex(vertex_descriptor u, const filtered_grid&) {
    if (u == m_goal)
      throw found_goal();
  }

private:
  vertex_descriptor m_goal;
};

// Solve the maze using A-star search.  Return true if a solution was found.
bool maze::solve() {
  boost::static_property_map<distance> weight(1);
  // The predecessor map is a vertex-to-vertex mapping.
  typedef boost::unordered_map<vertex_descriptor,
                               vertex_descriptor,
                               vertex_hash> pred_map;
  pred_map predecessor;
  boost::associative_property_map<pred_map> pred_pmap(predecessor);
  // The distance map is a vertex-to-distance mapping.
  typedef boost::unordered_map<vertex_descriptor,
                               distance,
                               vertex_hash> dist_map;
  dist_map distance;
  boost::associative_property_map<dist_map> dist_pmap(distance);

  vertex_descriptor s = source();
  vertex_descriptor g = goal();
  euclidean_heuristic heuristic(g);
  astar_goal_visitor visitor(g);

  try {
    astar_search(m_barrier_grid, s, heuristic,
                 boost::weight_map(weight).
                 predecessor_map(pred_pmap).
                 distance_map(dist_pmap).
                 visitor(visitor) );
  } catch(found_goal fg) {
    // Walk backwards from the goal through the predecessor chain adding
    // vertices to the solution path.
    for (vertex_descriptor u = g; u != s; u = predecessor[u])
      m_solution.insert(u);
    m_solution.insert(s);
    m_solution_length = distance[g];
    return true;
  }

  return false;
}

#define BARRIER "#"
// Print the maze as an ASCII map.
std::ostream& operator<<(std::ostream& output, const maze& m) {
  // Header
  for (vertices_size_type i = 0; i < m.length(0)+2; i++)
    output << BARRIER;
  output << std::endl;
  // Body
  for (int y = m.length(1)-1; y >= 0; y--) {
    // Enumerate rows in reverse order and columns in regular order so that
    // (0,0) appears in the lower left-hand corner.  This requires that y be
    // int and not the unsigned vertices_size_type because the loop exit
    // condition is y==-1.
    for (vertices_size_type x = 0; x < m.length(0); x++) {
      // Put a barrier on the left-hand side.
      if (x == 0)
        output << BARRIER;
      // Put the character representing this point in the maze grid.
      vertex_descriptor u = {{x, y}};
      if (m.solution_contains(u))
        output << ".";
      else if (m.has_barrier(u))
        output << BARRIER;
      else
        output << " ";
      // Put a barrier on the right-hand side.
      if (x == m.length(0)-1)
        output << BARRIER;
    }
    // Put a newline after every row except the last one.
    output << std::endl;
  }
  // Footer
  for (vertices_size_type i = 0; i < m.length(0)+2; i++)
    output << BARRIER;
  if (m.solved())
    output << std::endl << "Solution length " << m.m_solution_length;
  return output;
}

// Return a random integer in the interval [a, b].
std::size_t random_int(std::size_t a, std::size_t b) {
  if (b < a)
    b = a;
  boost::uniform_int<> dist(a, b);
  boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
  generate(random_generator, dist);
  return generate();
}

// Generate a maze with a random assignment of barriers.
maze random_maze(std::size_t x, std::size_t y) {
  maze m(x, y);
  vertices_size_type n = num_vertices(m.m_grid);
  vertex_descriptor s = m.source();
  vertex_descriptor g = m.goal();
  // One quarter of the cells in the maze should be barriers.
  int barriers = n/4;
  while (barriers > 0) {
    // Choose horizontal or vertical direction.
    std::size_t direction = random_int(0, 1);
    // Walls range up to one quarter the dimension length in this direction.
    vertices_size_type wall = random_int(1, m.length(direction)/4);
    // Create the wall while decrementing the total barrier count.
    vertex_descriptor u = vertex(random_int(0, n-1), m.m_grid);
    while (wall) {
      // Start and goal spaces should never be barriers.
      if (u != s && u != g) {
        wall--;
        if (!m.has_barrier(u)) {
          m.m_barriers.insert(u);
          barriers--;
        }
      }
      vertex_descriptor v = m.m_grid.next(u, direction);
      // Stop creating this wall if we reached the maze's edge.
      if (u == v)
        break;
      u = v;
    }
  }
  return m;
}

int main (int argc, char const *argv[]) {
  // The default maze size is 20x10.  A different size may be specified on
  // the command line.
  std::size_t x = 20;
  std::size_t y = 10;

  if (argc == 3) {
    x = boost::lexical_cast<std::size_t>(argv[1]);
    y = boost::lexical_cast<std::size_t>(argv[2]);
  }

  random_generator.seed(std::time(0));
  maze m = random_maze(x, y);

  if (m.solve())
    std::cout << "Solved the maze." << std::endl;
  else
    std::cout << "The maze is not solvable." << std::endl;
  std::cout << m << std::endl;
  return 0;
}


// The MIT License (MIT)

// Copyright (c) 2018 li chunpeng, Xidian university

// Permission is hereby granted, free of charge, to any person obtaining sum copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to
// use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
// the Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
// FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
// IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#ifndef _URANUS_UN_CONSTRAINED_HPP_
#define _URANUS_UN_CONSTRAINED_HPP_

// 常见无约束优化算法
// 直接方法
// 间接方法

#include <iostream>
#include <Eigen/Dense>
#include "Matrix.hpp"

using namespace std;

template<int Dim> struct problem
{
	uranus::SquareMatrix<Dim> matirx_Jacobian_;  // 一阶求导矩阵

	uranus::Vector<Dim>& gradient(uranus::Vector<Dim>& var_x)
	{
		var_x = matirx_Jacobian_ * var_x;
		return var_x;
	}
};

template<int Dim> double myval(uranus::Vector<Dim> s_k)
{
	double sum = 0;
	for(int i=0;i<Dim;++i)
	{
		sum = sum + pow(s_k(i),2);
	}
	return sqrt(sum);
}
/**
 * @brief 使用秩为2的模拟牛顿法, BFGS
 * @param 求解的问题f
 * @param 初始点 var_x,是一个向量，可包含多维
 * @param 终止条件 delta, 默认 0.001
 * @param 是否打印中间结果
 * @return 求解得到的最优点
 */
template<int Dim>
uranus::Vector<Dim> BFGS(problem<Dim> f,
						 uranus::Vector<Dim> var_x,
						 const double delta = 0.001,
						 bool visual = false)
{
	uranus::Vector<Dim> s_k;
	uranus::Vector<Dim> y_k;
	uranus::SquareMatrix<Dim> H_k;
	uranus::SquareMatrix<Dim> E_k1;
	uranus::SquareMatrix<Dim> E_k2;

	// init 单位矩阵
	for (int i = 0;i < Dim; ++i)
		for (int j = 0;j < Dim; ++j)
			if ( i == j) H_k(i,j) = 1;
			else H_k(i,j) = 0;

	while(myval<Dim>(s_k) < delta)
	{
		s_k = -H_k * f.gradient(var_x);           // s_k = var_x2 - var_x1
		y_k = f.gradient(var_x);
		var_x = var_x - H_k * f.gradient(var_x);
		y_k = f.gradient(var_x) - y_k;            // y_k = \delta f_{k} - \delta f_{k-1}

		E_k1  = (s_k * s_k.transpose() - H_k * y_k * s_k.transpose() - s_k * y_k.transpose() * H_k)
		      / (s_k.transpose() * y_k);
		E_k2 = s_k * s_k.transpose();

		double k21 = y_k.transpose() * H_k * y_k;
		double k22 = pow((s_k.transpose() * y_k),2);

		H_k = H_k + E_k1 + E_k2 * k21/k22;    // update H_k

		if(visual)
		{
			cout << "s_k:\n" << s_k << "\n"
				 << "y_k:\n" << y_k << "\n"
				 << "H_k:\n" << H_k << "\n";
 		}
	}
	return var_x; // result
}
/**
 * @brief 使用BFGS的外点法
 * @param 初始点 var_x,是一个向量，可包含多维
 * @param 终止条件 delta, 默认 0.001
 * @param 是否打印中间结果
 * @return 求解得到的最优点
 */
template<int Dim>
void External_point_method(uranus::Vector<Dim> var_x,
						   const double delta = 0.001,
						   const double C = 10
						   bool visual = false)
{
	double M_k = 0.1;                          // init M_k > 0
	do{
		M_k = C * M_k;                         // update M_k
		f.Jacobian_Matrix(1,1) = 2 + M_k;      // update J mat

		var_x = BFGS(f, var_x, delta, visual); // min f
	}
	while(M_k * pow(var_x(1)-1), 2) > delta);

}

#endif


// Copyright (c) 2018 James Pritts
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

// Modified for HomLib to accept Eigen matrices by Marcus Valtonen Örnhag

#include <Eigen/Dense>
#include "gj.hpp"

namespace HomLib {
    void gj(Eigen::MatrixXd *M) {
        int rcnt = M->rows();
        int ccnt = M->cols();

        M->transposeInPlace();
        double* A = M->data();

        double tol = 1e-15;
        int r = 0;      // row
        int c = 0;      // col
        int k;
        int l;
        int dstofs;
        int srcofs;
        int ofs = 0;
        int pofs = 0;
        double b;

        // gj
        ofs = 0;
        pofs = 0;
        while (r < rcnt && c < ccnt) {
            // find pivot
            double apivot = 0;
            double pivot = 0;
            int pivot_r = -1;

            pofs = ofs;
            for (k = r; k < rcnt; k++) {
                // pivot selection criteria here !
                if (fabs(*(A+pofs)) > apivot) {
                    pivot = *(A+pofs);
                    apivot = fabs(pivot);
                    pivot_r = k;
                }
                pofs += ccnt;
            }

            if (apivot < tol) {
                // empty col - shift to next col (or jump)
                c++;
                ofs++;

            } else {
                // process rows

                // exchange pivot and selected rows
                // + divide row
                if (pivot_r == r) {
                    srcofs = ofs;
                    for (l = c; l < ccnt; l++) {
                        *(A+srcofs) = *(A+srcofs)/pivot;
                        srcofs++;
                    }

                } else {
                    srcofs = ofs;
                    dstofs = ccnt*pivot_r+c;
                    for (l = c; l < ccnt; l++) {
                        b = *(A+srcofs);
                        *(A+srcofs) = *(A+dstofs)/pivot;
                        *(A+dstofs) = b;

                        srcofs++;
                        dstofs++;
                    }
                }

                // zero bottom
                pofs = ofs + ccnt;
                for (k = r + 1; k < rcnt; k++) {
                        // nonzero row
                        b = *(A+pofs);
                        dstofs = pofs + 1;
                        srcofs = ofs + 1;
                        for (l = c + 1; l < ccnt; l++) {
                            *(A+dstofs) = (*(A+dstofs) - *(A+srcofs) * b);
                            dstofs++;
                            srcofs++;
                        }
                        *(A+pofs) = 0;

                    pofs += ccnt;
                }

                // zero top
                pofs = c;
                for (k = 0; k < r; k++) {
                        // nonzero row
                        b = *(A+pofs);
                        dstofs = pofs + 1;
                        srcofs = ofs + 1;
                        for (l = c + 1; l < ccnt; l++) {
                            *(A+dstofs) = (*(A+dstofs) - *(A+srcofs) * b);
                            dstofs++;
                            srcofs++;
                        }
                        *(A+pofs) = 0;

                    pofs += ccnt;
                }

                r++;
                c++;
                ofs += ccnt + 1;
            }
        }

        (*M) << Eigen::Map<Eigen::MatrixXd>(A, ccnt, rcnt);
        M->transposeInPlace();
    }
}  // namespace HomLib


#pragma once
#include <cstdint>
#include <cassert>
#include <algorithm>
#include <map>
#include <boost/dynamic_bitset.hpp>
//#include "BitOperations.h"
#include "../Basis/AbstractBasis2D.hpp"

template<typename UINT>
class TIJ1J2_2D
{
private:
	const edlib::AbstractBasis2D<UINT>& basis_;
	double J1_;
	double J2_;
	int sign_;

public:
	TIJ1J2_2D(const edlib::AbstractBasis2D<UINT>& basis, double J1, double J2,
			bool signRule = false)
		: basis_(basis), J1_(J1), J2_(J2)
	{
		if(signRule)
			sign_ = -1;
		else
			sign_ = 1;
	}

	std::map<int,double> getCol(UINT n) const
	{
		uint32_t Lx = basis_.getLx();
		uint32_t Ly = basis_.getLy();

		UINT a = basis_.getNthRep(n);
		const boost::dynamic_bitset<> bs(Lx*Ly, a);

		std::map<int, double> m;
		for(uint32_t nx = 0; nx < Lx; nx++)
		{
			for(uint32_t ny = 0; ny < Ly; ny++)
			{
				auto i = basis_.toIdx(nx, ny);
				//Nearest neighbor x
				{
				auto j = basis_.toIdx((nx+1)%Lx, ny);
				int zz = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J1_*zz;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += J1_*(1-zz)*coeff*sign_;
				}
				//Nearest neighbor y
				{
				auto j = basis_.toIdx(nx, (ny+1)%Ly);
				int zz = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J1_*zz;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += J1_*(1-zz)*coeff*sign_;
				}
				//Next-nearest neighbor right up
				{
				auto j = basis_.toIdx((nx+1)%Lx, (ny+1)%Ly);
				int zz = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J2_*zz;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += J2_*(1-zz)*coeff;
				}
				//Next-nearest neighbor right down
				{
				auto j = basis_.toIdx((nx+1)%Lx, (ny-1+Ly)%Ly);
				int zz = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J2_*zz;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += J2_*(1-zz)*coeff;
				}
			}
		}
		return m;
	}
};


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_EXPM1_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_EXPM1_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-exponential
    Function object implementing expm1 capabilities

    exponential function minus one:\f$e^{x}-1\f$

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = expm1(x);
    @endcode

    is similar to:

    @code
    T r = minusone(exp(x));
    @endcode

    @par Notes:

    - result is accurate even for x of small modulus

    @par Decorators

    std_ for floating entries

    @see exp

  **/
  const boost::dispatch::functor<tag::expm1_> expm1 = {};
} }
#endif

#include <boost/simd/function/scalar/expm1.hpp>
#include <boost/simd/function/simd/expm1.hpp>

#endif


#include "GaussianMixture.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <cmath>

#include "Util.h"

namespace ark {
    void GaussianMixture::load(const std::string & path)
    {
        std::ifstream ifs(path);
        if (!ifs) {
            std::cerr << "Warning: pose prior file at " << path << " does not exist or cannot be read\n";
            nComps = -1;
            return;
        }
        ifs >> nComps >> nDims;

        // compute constants
        double sqrt_2_pi_n = std::pow(2 * M_PI, nDims * 0.5 );
        double log_sqrt_2_pi_n = nDims * 0.5 * std::log(2 * M_PI);
        weight.resize(nComps);
        consts.resize(nComps);
        consts_log.resize(nComps);
        for (int i = 0; i < nComps; ++i) {
            // load weights
            ifs >> weight[i];
            consts_log[i] = log(weight[i]) - log_sqrt_2_pi_n;
            consts[i] = weight[i] / sqrt_2_pi_n;
        }

        mean.resize(nComps, nDims);
        for (int i = 0; i < nComps; ++i) {
            for (int j = 0; j < nDims; ++j) {
                // load mean vectors
                ifs >> mean(i, j);
            }
        }

        /** Cholesky decomposition */
        typedef Eigen::LLT<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>> Cholesky;

        cov.resize(nComps);
        cov_cho.resize(nComps);
        prec_cho.resize(nComps);
        double minDet = std::numeric_limits<double>::max();
        for (int i = 0; i < nComps; ++i) {
            auto & m = cov[i];
            m.resize(nDims, nDims);
            for (int j = 0; j < nDims; ++j) {
                for (int k = 0; k < nDims; ++k) {
                    // load covariance matrices
                    ifs >> m(j, k);
                }
            }
            Cholesky chol(cov[i]);
            if (chol.info() != Eigen::Success) throw "Decomposition failed!";
            cov_cho[i] = chol.matrixL();
            Cholesky chol_prec(cov[i].inverse());
            prec_cho[i] = chol_prec.matrixL();
            double det = cov_cho[i].determinant();
            minDet = std::min(det, minDet);

            // update constants
            consts[i] /= det;
            consts_log[i] -= log(det);
        }

        for (int i = 0; i < nComps; ++i) {
            // normalize constants
            consts[i] *= minDet;
            consts_log[i] += log(minDet);
        }
    }

    int GaussianMixture::numComponents() const {
        return nComps;
    };

    /** Compute PDF at 'input' */
    double GaussianMixture::pdf(const Eigen::VectorXd & x) const {
        double prob(0.0);
        typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Mattype;
        for (int i = 0; i < nComps; ++i) {
            Eigen::TriangularView<Eigen::MatrixXd, Eigen::Lower> L(prec_cho[i]);
            auto residual = (L * (x - mean.row(i).transpose()));
            prob += consts[i] * std::exp(-0.5 * residual.squaredNorm());
        }
        return prob;
    }

    Eigen::VectorXd GaussianMixture::residual(const Eigen::VectorXd & x, int* comp_idx) const {
        double bestProb = std::numeric_limits<double>::max();
        Eigen::VectorXd ans;
        for (int i = 0; i < nComps; ++i) {
            Eigen::TriangularView<Eigen::MatrixXd, Eigen::Lower> L(prec_cho[i]);
            Eigen::VectorXd residual(nDims + 1);
            residual[nDims] = 0.;
            residual.head(nDims) = L.transpose() * (x - mean.row(i).transpose()) * sqrt(0.5);
            double p = residual.squaredNorm() - consts_log[i];
            if (p < bestProb) {
                bestProb = p;
                residual[nDims] = sqrt(-consts_log[i]);
                ans = residual;
                if (comp_idx != nullptr) {
                    *comp_idx = i;
                }
            }
        }
        return ans;
    }

    Eigen::VectorXd GaussianMixture::sample() const {
        // Pick random GMM component
        double randf = random_util::uniform(0.0f, 1.0f);
        int component;
        for (size_t i = 0 ; i < nComps; ++i) {
            randf -= weight[i];
            if (randf <= 0) component = i;
        }
        Eigen::VectorXd r(nDims);
        // Sample from Gaussian
        for (int i = 0; i < nDims; ++i) {
            r(i) = random_util::randn();
        }
        r *= cov_cho[component];
        r += mean.row(component);
        return r;
    }

}  // namespace ark


#ifndef CWT1D_WAVELETS
#define CWT1D_WAVELETS
#include "cwt1d.hpp"
#include <boost/math/special_functions/gamma.hpp>

namespace cwt1d
{
  template <typename T>
  class dog
    :public wavelet_func<T>
  {
  private:
    int m;
    T norm;
    T coi_factor;
    bool coi_calculated;
  public:
    dog()
      :m(2),coi_calculated(false)
    {
      norm=boost::math::tgamma(m+1/2.);
    }

    dog(int m1)
      :m(m1),coi_calculated(false)
    {
      norm=boost::math::tgamma(m+1/2.);
    }

    T do_cone_of_influence_factor()const
    {
      if(!coi_calculated)
	{
	  blitz::Array<T,1> signal(1024);
	  signal=0;
	  signal(signal.extent(0)/2)=1;
	  blitz::Array<T,1> scales(1);
	  scales(0)=1;
	  blitz::Array<std::complex<T>,2> signal_cwt(cwt(signal,scales,*this));
	  int max_idx=0;
	  for(int i=0;i<signal.extent(0)/2;++i)
	    {
	      if(abs(signal_cwt(0,i))>abs(signal_cwt(0,signal.extent(0)/2))/5)
		{
		  max_idx=i;
		  break;
		}
	    }
	  const_cast<T&>(coi_factor)=signal.extent(0)/2-max_idx;
	  const_cast<bool&>(coi_calculated)=true;
	}
      return coi_factor*.85;
    }

  private:
    std::complex<T> do_wavelet_f(T w,T s)const
    {
      const T ws=w*s;
      const T pi=4*std::atan(1);
      return -pow(std::complex<T>(0,1),T(m))*pow(ws,T(m))*exp(-(ws*ws)/2)/norm;
    }
  };

  template <typename T>
  class morlet
    :public wavelet_func<T>
  {
  private:
    T omega_0;
    T coi_factor;
    bool coi_calculated;
  public:
    morlet()
      :omega_0(1),coi_calculated(false)
    {}

    morlet(T w1)
      :omega_0(w1),coi_calculated(false)
    {}

    T do_cone_of_influence_factor()const
    {
      if(!coi_calculated)
	{
	  blitz::Array<T,1> signal(1024);
	  signal=0;
	  signal(signal.extent(0)/2)=1;
	  blitz::Array<T,1> scales(1);
	  scales(0)=8;
	  blitz::Array<std::complex<T>,2> signal_cwt(cwt(signal,scales,*this));
	  int max_idx=0;
	  for(int i=0;i<signal.extent(0)/2;++i)
	    {
	      if(abs(signal_cwt(0,i))>abs(signal_cwt(0,signal.extent(0)/2))/1e2)
		{
		  max_idx=i;
		  break;
		}
	    }
	  const_cast<T&>(coi_factor)=(signal.extent(0)/2-max_idx)/8.;
	  const_cast<bool&>(coi_calculated)=true;
	}
      return coi_factor*.85;
    }

  private:
    std::complex<T> do_wavelet_f(T w,T s)const
    {
      static const T pi=atan(1)*4;
      if(w<=0)
	{
	  return 0;
	}
      else
	{
	  return pow(T(pi),T(-.25))*exp(-pow(s*w-omega_0,2)/2);
	}
    }
  };

  template <typename T>
  class paul
    :public wavelet_func<T>
  {
  private:
    int m;
    T norm;
    T coi_factor;
    bool coi_calculated;

  public:
    paul()
      :m(4),coi_calculated(false)
    {
      norm=calc_norm();
    }

    paul(T m1)
      :m(m1),coi_calculated(false)
    {
      norm=calc_norm();
    }

    T do_cone_of_influence_factor()const
    {
      if(!coi_calculated)
	{
	  blitz::Array<T,1> signal(1024);
	  signal=0;
	  signal(signal.extent(0)/2)=1;
	  blitz::Array<T,1> scales(1);
	  scales(0)=1;
	  blitz::Array<std::complex<T>,2> signal_cwt(cwt(signal,scales,*this));
	  int max_idx=0;
	  for(int i=0;i<signal.extent(0)/2;++i)
	    {
	      if(abs(signal_cwt(0,i))>abs(signal_cwt(0,signal.extent(0)/2))/25)
		{
		  max_idx=i;
		  break;
		}
	    }
	  const_cast<T&>(coi_factor)=(signal.extent(0)/2-max_idx)/4.;
	  const_cast<bool&>(coi_calculated)=true;
	}
      return coi_factor;
    }

    T calc_norm()
    {
      return pow(T(2.),(T)m)/sqrt(m*factorial(2*m-1));
    }

    int factorial(int n)
    {
      if(n==0)
	{
	  return 1;
	}
      else
	{
	  return n*factorial(n-1);
	}
    }

  private:
    std::complex<T> do_wavelet_f(T w,T s)const
    {
      if(w<=0)
	{
	  return 0;
	}
      else
	{
	  return norm*pow(s*w,(T)m)*exp(-s*w);
	}
    }
  };
}

#endif
//EOF


// Petter Strandmark 2012.

#include <cstdio>
#include <iostream>
#include <limits>
#include <stdexcept>

#include <Eigen/Dense>

#include <spii/spii.h>
#include <spii/solver.h>

namespace spii {

void PatternSolver::solve(const Function& function,
                          SolverResults* results) const
{
	double global_start_time = wall_time();

	// Dimension of problem.
	size_t n = function.get_number_of_scalars();

	if (n == 0) {
		results->exit_condition = SolverResults::FUNCTION_TOLERANCE;
		return;
	}

	// Current point, gradient and Hessian.
	double fval   = std::numeric_limits<double>::quiet_NaN();
	double fprev  = std::numeric_limits<double>::quiet_NaN();

	Eigen::VectorXd x;
	// Copy the user state to the current point.
	function.copy_user_to_global(&x);
	Eigen::VectorXd dx(n);

	// Size of the pattern.
	double pattern_size = 1.0;
	double x_inf = std::max(x.maxCoeff(), -x.minCoeff());
	if (x_inf > 1e-6) {
		pattern_size = x_inf /= 2.0;
	}
	double pattern_size0 = pattern_size;

	// Sufficient decrease.
	auto rho = [](double t) -> double { return 1e-4 * std::pow(t ,1.5); };

	//
	// START MAIN ITERATION
	//
	results->startup_time   += wall_time() - global_start_time;
	results->exit_condition = SolverResults::INTERNAL_ERROR;
	int iter = 0;
	while (true) {

		//
		// Search along all coordinate directions
		//
		double start_time = wall_time();

		if (iter == 0) {
			fval = function.evaluate(x);
		}

		bool success = false;
		for (size_t i = 0; i < n; ++i) {
			dx.setZero();
			//for d in {-1, 1}
			for (double d = -1; d <= 1; d += 2) {
				// Search along this coordinate axis in
				// direction d.
				dx[i] = d * pattern_size;
				// Evaluate function in new point.
				double fval_new = function.evaluate(x + dx);

				// If we have sufficient decrease.
				if (fval_new < fval - rho(pattern_size)) {
					success = true;
					x = x + dx;
					fval = fval_new;
					break;
				}
			}

			if (success) {
				break;
			}
		}

		// If no point was found, decrease pattern size.
		if (! success) {
			pattern_size /= 2.0;
		}

		results->function_evaluation_time += wall_time() - start_time;

		//
		// Test stopping criteriea
		//
		start_time = wall_time();
		if (pattern_size / pattern_size0 < this->area_tolerance) {
			results->exit_condition = SolverResults::GRADIENT_TOLERANCE;
			break;
		}
		if (success &&
			std::fabs(fval - fprev) / (std::fabs(fval) + this->function_improvement_tolerance) <
			                                             this->function_improvement_tolerance) {
			results->exit_condition = SolverResults::FUNCTION_TOLERANCE;
			break;
		}
		if (iter >= this->maximum_iterations) {
			results->exit_condition = SolverResults::NO_CONVERGENCE;
			break;
		}
		results->stopping_criteria_time += wall_time() - start_time;

		//
		// Log the results of this iteration.
		//
		start_time = wall_time();

		int log_interval = 1;
		if (iter > 30) {
			log_interval = 10;
		}
		if (iter > 200) {
			log_interval = 100;
		}
		if (iter > 2000) {
			log_interval = 1000;
		}
		if (iter > 20000) {
			log_interval = 10000;
		}
		if (iter > 200000) {
			log_interval = 100000;
		}
		if (this->log_function && iter % log_interval == 0) {
			char str[1024];
				if (iter == 0) {
					this->log_function("Itr         f       deltaf   gamma   success");
				}
				std::sprintf(str, "%7d %+10.3e %9.3e %9.3e %s",
					iter, fval, std::fabs(fval - fprev), pattern_size, success ? "yes" : "no");
			this->log_function(str);
		}
		results->log_time += wall_time() - start_time;

		fprev = fval;
		iter++;
	}

	function.copy_global_to_user(x);
	results->total_time += wall_time() - global_start_time;

	if (this->log_function) {
		char str[1024];
		std::sprintf(str, "    end %+10.3e %9.3e %9.3e", fval, std::fabs(fval - fprev), pattern_size);
		this->log_function(str);
	}
}

}  // namespace spii


#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
#include <vector>

using namespace std;
using namespace boost::multiprecision;

void printvector(vector<int> n); // imprime un vector
void dec2bin(cpp_int n); // transforma a binario
vector<int> twoscomplement(vector<int> &negative); // complemento del 2 del numero

int main() {
  cpp_int n;
  while (cin >> n) {
    dec2bin(n);
  }
  return 0;
}

void printvector(vector<int> n) {
  int bitcount = 0;
  for (size_t i = 0; i < n.size(); ++i) {
    cout << n[i];
    bitcount += n[i] == 1 ? 1 : 0;
  }
  cout << endl << "Bitcount: " << bitcount << endl;
}

void dec2bin(cpp_int n) {
  vector<int> resultado;
  resultado.clear();
  int counter = 31;
  bool isNegative = 0;
  if (n < 1) { // si es negativo, se calcula el valor absoluto
    n = -n;
    isNegative = 1;
  }
  for (int i = 0; i < 32; ++i) { // transforma el numero a binario al reves
    resultado.push_back(n % 2 == 1 ? 1 : 0);
	n /= 2;
  }
  for (int i = 0; i < 16; ++i) { // reordena el numero al reves
    swap(resultado[i], resultado[counter]);
    --counter;
  }
  printvector((isNegative == 1) ? twoscomplement(resultado) : resultado);
}

vector<int> twoscomplement(vector<int> &negative) {
  for (int i = 0; i < 32; ++i) { // cambia 1 por 0 y vice-versa
    negative[i] = (negative[i] == 0) ? 1 : 0;
  }
  for (int i = 31; i >= 0; --i) { // suma 1 al numero
    if (negative[i] != 0) {
      negative[i] = 0;
    } else {
      negative[i] = 1;
      break;
    }
  }
  return negative;
}


/*
// Copyright 2018 University of Liege
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Authors:
// - Adrien Crovato
*/

//// Surface variables computation
// Compute flow physical properties such as body velocity, mach number, pressure and forces coefficients.
//
// I/O:
// - symY: defines symmetry about Y axis
// - sRef: reference surface of the full wing
// - alpha: freestream angle of attack
// - Minf: freestream Mach number
// - vInf: freestream velocity vector
// - vSigma: field source induced body velocity
// - bPan: (network of) body panels (structure)
// - cL: lift coefficient
// - cD: drag coefficient

#include <iostream>
#include <Eigen/Dense>

#include "compute_sVars.h"

#define NDIM 3
#define GAMMA 1.4

using namespace std;
using namespace Eigen;

void compute_sVars(bool symY, double sRef, double alpha, double Minf, Vector3d &vInf,
               MatrixX3d &vSigma, Network &bPan, double &cL, double &cD) {

    // Temporary variables
    int panIdx = 0;
    double deltaL1, deltaL2, deltaT1, deltaT2;
    double u = 0, v = 0, w = 0;
    double cX = 0, cZ = 0;

    //// Begin
    cout << "Computing surface variables... ";

    // Resize matrices
    bPan.U = MatrixX3d::Zero(bPan.nP, NDIM);
    bPan.cP.resize(bPan.nP);

    //// Velocity computation
    // Surface induced velocity
    for (int j = 0; j < bPan.nS_; ++j) {
        for (int i = 0; i < bPan.nC_; ++i) {

            // Longitudinal
            if (i == 0) { // Forward differencing
                deltaL2 = (bPan.CG.row(panIdx + 1) - bPan.CG.row(panIdx)).norm();
                u = (bPan.mu(panIdx + 1) - bPan.mu(panIdx)) / deltaL2;
            }
            else if (i == bPan.nC_ - 1) { // Backward differencing
                deltaL1 = (bPan.CG.row(panIdx) - bPan.CG.row(panIdx - 1)).norm();
                u = (bPan.mu(panIdx) - bPan.mu(panIdx - 1)) / deltaL1;
            }
            else { // Central differencing
                deltaL1 = (bPan.CG.row(panIdx) - bPan.CG.row(panIdx - 1)).norm();
                deltaL2 = (bPan.CG.row(panIdx + 1) - bPan.CG.row(panIdx)).norm();
                u = 0.5 * ((bPan.mu(panIdx + 1) - bPan.mu(panIdx)) / deltaL2 +
                           (bPan.mu(panIdx) - bPan.mu(panIdx - 1)) / deltaL1);
            }
            // Transverse
            if (j == 0) { // Forward differencing
                deltaT2 = (bPan.CG.row(panIdx + bPan.nC_) - bPan.CG.row(panIdx)).norm();
                v = (bPan.mu(panIdx + bPan.nC_) - bPan.mu(panIdx)) / deltaT2;
            }
            else if (j == bPan.nS_ - 1) { // Backward differencing
                deltaT1 = (bPan.CG.row(panIdx) - bPan.CG.row(panIdx - bPan.nC_)).norm();
                v = (bPan.mu(panIdx) - bPan.mu(panIdx - bPan.nC_)) / deltaT1;
            }
            else { // Central differencing
                deltaT1 = (bPan.CG.row(panIdx) - bPan.CG.row(panIdx - bPan.nC_)).norm();
                deltaT2 = (bPan.CG.row(panIdx + bPan.nC_) - bPan.CG.row(panIdx)).norm();
                v = 0.5 * ((bPan.mu(panIdx + bPan.nC_) - bPan.mu(panIdx)) / deltaT2 +
                           (bPan.mu(panIdx) - bPan.mu(panIdx - bPan.nC_)) / deltaT1);
            }

            // Transverse to perpendicular velocity
            v = (bPan.t(panIdx, 0) * bPan.p(panIdx, 0)
                 + bPan.t(panIdx, 1) * bPan.p(panIdx, 1)
                 + bPan.t(panIdx, 2) * bPan.p(panIdx, 2)) * v;
            // Normal velocity
            w = bPan.tau(panIdx);

            // Transformation to global axis
            bPan.U(panIdx, 0) = u * bPan.l(panIdx, 0) - v * bPan.p(panIdx, 0) + w * bPan.n(panIdx, 0);
            bPan.U(panIdx, 1) = u * bPan.l(panIdx, 1) - v * bPan.p(panIdx, 1) + w * bPan.n(panIdx, 1);
            bPan.U(panIdx, 2) = u * bPan.l(panIdx, 2) - v * bPan.p(panIdx, 2) + w * bPan.n(panIdx, 2);

            panIdx++;
        }
    }
    // Freestream velocity
    for (int i = 0; i < bPan.nP; ++i) {
        bPan.U(i, 0) += vInf(0);
        bPan.U(i, 1) += vInf(1);
        bPan.U(i, 2) += vInf(2);
    }
    // Source induced velocity
    if (Minf != 0)
        bPan.U += vSigma;
    // Mach number
    if (Minf == 0)
        bPan.M = VectorXd::Zero(bPan.nP);
    else {
        bPan.M.resize(bPan.nP);
        for (int i = 0; i < bPan.nP; ++i)
            bPan.M(i) = bPan.U.row(i).norm()
                        / (1 / (Minf * Minf)
                           + (GAMMA - 1) / 2 - (GAMMA - 1) / 2 * bPan.U.row(i).dot(bPan.U.row(i)));
    }
    //// Aerodynamic forces
    // Pressure coefficient
    if (Minf == 0) {
        for (int i = 0; i < bPan.nP; ++i)
            bPan.cP(i) = 1 - bPan.U.row(i).dot(bPan.U.row(i)) / vInf.dot(vInf);
    }
    else {
        for (int i = 0; i < bPan.nP; ++i)
            bPan.cP(i) = 2 / (GAMMA * Minf * Minf) *
                (pow(1 + (GAMMA - 1) / 2 * Minf * Minf *
                                 (1 - bPan.U.row(i).dot(bPan.U.row(i))), GAMMA / (GAMMA - 1)) - 1);
    }
    // Forces (x,y,z)
    for (int i = 0; i < bPan.nP; ++i) {
        cX += -bPan.cP(i) * bPan.S(i) / sRef * bPan.n(i,0);
        cZ += -bPan.cP(i) * bPan.S(i) / sRef * bPan.n(i,2);
    }
    if (symY) {
        cX = 2*cX;
        cZ = 2*cZ;
    }
    // Forces (l,d,y)
    cD = cos(alpha)*cX + sin(alpha)*cZ;
    cL = -sin(alpha)*cX + cos(alpha)*cZ;

    //// Control display
    cout << "Done!" << endl;
    #ifdef VERBOSE
        cout << "Surface velocities: " << bPan.U.rows() << "X" << bPan.U.cols() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.U(i,0) << ' ' << bPan.U(i,1) << ' ' << bPan.U(i,2) << endl;
        cout << "Pressure coefficients: " << bPan.cP.rows() << "X" << bPan.cP.cols() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.cP(i) << endl;
    #endif
    cout << "Aerodynamic forces:" << endl;
    cout << "Cx = " << cX << endl;
    cout << "Cz = " << cZ << endl;
    cout << "Cl = " << cL << endl;
    cout << "Cd = " << cD << endl;
    cout << "L/D = " << cL/cD << endl << endl;
}