/**********************************************************************
*  Copyright (c) 2008-2015, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include "Geometry.hpp"
#include "Intersection.hpp"
#include "Transformation.hpp"

#include "../core/Assert.hpp"

#include <boost/math/constants/constants.hpp>

#include <polypartition/polypartition.h>

#include <list>

namespace openstudio{
  /// convert degrees to radians
  double degToRad(double degrees)
  {
    return degrees*boost::math::constants::pi<double>()/180.0;
  }

  /// convert radians to degrees
  double radToDeg(double radians)
  {
    return radians*180.0/boost::math::constants::pi<double>();
  }

  /// compute area from surface as Point3dVector
  boost::optional<double> getArea(const Point3dVector& points)
  {
    boost::optional<double> result;
    OptionalVector3d newall = getNewallVector(points);
    if (newall){
      result = newall->length() / 2.0;
    }
    return result;
  }

  // compute Newall vector from Point3dVector, direction is same as outward normal
  // magnitude is twice the area
  OptionalVector3d getNewallVector(const Point3dVector& points)
  {
    OptionalVector3d result;
    unsigned N = points.size();
    if (N >= 3){
      Vector3d vec;
      for (unsigned i = 1; i < N-1; ++i){
        Vector3d v1 = points[i] - points[0];
        Vector3d v2 = points[i+1] - points[0];
        vec += v1.cross(v2);
      }
     result = vec;
   }
   return result;
  }

  // compute outward normal from Point3dVector
  OptionalVector3d getOutwardNormal(const Point3dVector& points)
  {
    OptionalVector3d result = getNewallVector(points);
    if (result){
      if (!result->normalize()){
        result.reset();
      }
    }
    return result;
  }

  /// compute centroid from surface as Point3dVector
  OptionalPoint3d getCentroid(const Point3dVector& points)
  {
    OptionalPoint3d result;

    if (points.size() >= 3){
      // convert to face coordinates
      Transformation alignFace = Transformation::alignFace(points);
      Point3dVector surfacePoints = alignFace.inverse()*points;

      unsigned N = surfacePoints.size();
      double A = 0;
      double cx = 0;
      double cy = 0;
      for (unsigned i = 0; i < N; ++i){
        double x1, x2, y1, y2;
        if (i == N-1){
          x1 = surfacePoints[i].x();
          x2 = surfacePoints[0].x();
          y1 = surfacePoints[i].y();
          y2 = surfacePoints[0].y();
        }else{
          x1 = surfacePoints[i].x();
          x2 = surfacePoints[i+1].x();
          y1 = surfacePoints[i].y();
          y2 = surfacePoints[i+1].y();
        }

        double dA = (x1*y2-x2*y1);
        A += 0.5*dA;
        cx += (x1+x2)*dA;
        cy += (y1+y2)*dA;
      }

      if (A > 0){
        // centroid in face coordinates
        Point3d surfaceCentroid(cx/(6.0*A), cy/(6.0*A), 0.0);

        // centroid
        result = alignFace*surfaceCentroid;
      }
    }
    return result;
  }

  /// reorder points to upper-left-corner convention
  Point3dVector reorderULC(const Point3dVector& points)
  {
    unsigned N = points.size();
    if (N < 3){
      return Point3dVector();
    }

    // transformation to align face
    Transformation t = Transformation::alignFace(points);
    Point3dVector facePoints = t.inverse()*points;

    // find ulc index in face coordinates
    double maxY = std::numeric_limits<double>::min();
    double minX = std::numeric_limits<double>::max();
    unsigned ulcIndex = 0;
    for(unsigned i = 0; i < N; ++i){
      OS_ASSERT(std::abs(facePoints[i].z()) < 0.001);
      if ((maxY < facePoints[i].y()) || ((maxY < facePoints[i].y() + 0.00001) && (minX > facePoints[i].x()))){
        ulcIndex = i;
        maxY = facePoints[i].y();
        minX = facePoints[i].x();
      }
    }

    // no-op
    if (ulcIndex == 0){
      return points;
    }

    // create result
    Point3dVector result;
    std::copy (points.begin() + ulcIndex, points.end(), std::back_inserter(result));
    std::copy (points.begin(), points.begin() + ulcIndex, std::back_inserter(result));
    OS_ASSERT(result.size() == N);
    return result;
  }

  std::vector<Point3d> removeCollinear(const Point3dVector& points, double tol)
  {
    unsigned N = points.size();
    if (N < 3){
      return points;
    }

    std::vector<Point3d> result;
    Point3d lastPoint = points[0];
    result.push_back(lastPoint);

    for (unsigned i = 1; i < N; ++i){
      Point3d currentPoint = points[i];
      Point3d nextPoint = points[0];
      if (i < N-1){
        nextPoint = points[i+1];
      }

      Vector3d a = (currentPoint - lastPoint);
      Vector3d b = (nextPoint - currentPoint);

      // if these fail to normalize we have zero length vectors (e.g. adjacent points)
      if (a.normalize()){
        if (b.normalize()){

          Vector3d c = a.cross(b);
          if (c.length() >= tol){
            // cross product is significant
            result.push_back(currentPoint);
            lastPoint = currentPoint;
          }else{
            // see if dot product is near -1
            double d = a.dot(b);
            if (d <= -1.0 + tol){
              // this is a line reversal
              result.push_back(currentPoint);
              lastPoint = currentPoint;
            }
          }
        }
      }
    }

    return result;
  }

  double getDistance(const Point3d& point1, const Point3d& point2) {
    double dx = point1.x() - point2.x();
    double dy = point1.y() - point2.y();
    double dz = point1.z() - point2.z();
    double result = std::sqrt(dx*dx + dy*dy + dz*dz);
    return result;
  }

  double getDistancePointToLineSegment(const Point3d& point, const std::vector<Point3d>& lineSegment)
  {
    if (lineSegment.size() != 2){
      return 0;
    }

    // http://paulbourke.net/geometry/pointlineplane/

    Point3d point1 = lineSegment[0];
    Point3d point2 = lineSegment[1];

    Vector3d p2p1 = point2-point1;
    Vector3d p3p1 = point-point1;

    double d12 = p2p1.length();
    if (d12 < 1.0e-12){
      return p3p1.length();
    }

    Point3d closestPoint;
    double u = p3p1.dot(p2p1) / (d12*d12);
    if (u < 0){
      closestPoint = point1;
    }else if (u > 1){
      closestPoint = point2;
    }else{
      closestPoint = point1 + u*p2p1;
    }

    Vector3d diff = point - closestPoint;

    return diff.length();
  }

  double getDistancePointToTriangle(const Point3d& point, const std::vector<Point3d>& triangle)
  {
    if (triangle.size() != 3){
      return 0;
    }

    //Distance Between Point and Triangle in 3D
    //David Eberly
    //Geometric Tools, LLC
    //http://www.geometrictools.com/

    //T(s; t) = B+sE0+tE1

    Point3d B = triangle[0];
    Vector3d E0 = triangle[1] - triangle[0];
    Vector3d E1 = triangle[2] - triangle[0];
    Vector3d BminusP = B - point;

    double b = E0.dot(E1);

    if (std::abs(b) > 1.0-1.0E-12){
      // triangle is collinear
      return 0;
    }

    double a = E0.dot(E0);
    double c = E1.dot(E1);
    double d = E0.dot(BminusP);
    double e = E1.dot(BminusP);
    // double f = BminusP.dot(BminusP); // unused

    double det = a*c-b*b;
    double s = b*e-c*d;
    double t = b*d-a*e;

    Point3d closestPoint;

    if ( s+t <= det ) {
      if ( s < 0 ) {
        if ( t < 0 ) {
          //region 4, closest to point triangle[0]
          return getDistance(point, triangle[0]);
        } else {
          //region 3, closest to line triangle[0] to triangle[2]
          std::vector<Point3d> line;
          line.push_back(triangle[0]);
          line.push_back(triangle[2]);
          return getDistancePointToLineSegment(point, line);
        }
      } else if ( t < 0 ) {
        //region 5, closest to line triangle[0] to triangle[1]
        std::vector<Point3d> line;
        line.push_back(triangle[0]);
        line.push_back(triangle[1]);
        return getDistancePointToLineSegment(point, line);
      } else {
        //region 0, closest point is inside triangle
        double invDet = 1.0/det;
        closestPoint = B + invDet*s*E0 + invDet*t*E1;
      }
    } else {
      if ( s < 0 ) {
        //region 2, closest to point triangle[2]
        return getDistance(point, triangle[2]);
      } else if ( t < 0 ) {
        //region 6, closest to point triangle[1]
        return getDistance(point, triangle[1]);
      } else {
        //region 1, closest to line triangle[1] to triangle[2]
        std::vector<Point3d> line;
        line.push_back(triangle[1]);
        line.push_back(triangle[2]);
        return getDistancePointToLineSegment(point, line);
      }
    }

    Vector3d diff = point-closestPoint;
    return diff.length();
  }

  double getAngle(const Vector3d& vector1, const Vector3d& vector2) {
    Vector3d working1(vector1);
    working1.normalize();
    Vector3d working2(vector2);
    working2.normalize();
    return acos(working1.dot(working2));
  }

  /// compute distance in meters between two points on the Earth's surface
  /// lat and lon are specified in degrees
  double getDistanceLatLon(double lat1, double lon1, double lat2, double lon2)
  {

    // for more accuracy would want to use WGS-84 ellipsoid params and Vincenty formula

    // Haversine formula
    double R = 6371000; // Earth radius meters
    double deltaLat = degToRad(lat2-lat1);
    double deltaLon = degToRad(lon2-lon1);
    double a = sin(deltaLat/2) * sin(deltaLat/2) +
               cos(degToRad(lat1)) * cos(degToRad(lat2)) *
               sin(deltaLon/2) * sin(deltaLon/2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    double d = R * c;

    return d;
  }

  bool circularEqual(const Point3dVector& points1, const Point3dVector& points2, double tol)
  {
    unsigned N = points1.size();
    if (N != points2.size()){
      return false;
    }

    if (N == 0){
      return true;
    }

    bool result = false;

    // look for a common starting point
    for (unsigned i = 0; i < N; ++i){
      if (getDistance(points1[0], points2[i]) <= tol){

        result = true;

        // check all other points
        for (unsigned j = 0; j < N; ++j){
          if (getDistance(points1[j], points2[(i + j) % N]) > tol){
            result = false;
            break;
          }
        }
      }

      if (result){
        return result;
      }
    }

    return result;
  }

  Point3d getCombinedPoint(const Point3d& point3d, std::vector<Point3d>& allPoints, double tol)
  {
    for (const Point3d& otherPoint : allPoints){
      if (std::sqrt(std::pow(point3d.x()-otherPoint.x(), 2) + std::pow(point3d.y()-otherPoint.y(), 2) + std::pow(point3d.z()-otherPoint.z(), 2)) < tol){
        return otherPoint;
      }
    }
    allPoints.push_back(point3d);
    return point3d;
  }

  std::vector<std::vector<Point3d> > computeTriangulation(const Point3dVector& vertices, const std::vector<std::vector<Point3d> >& holes, double tol)
  {
    std::vector<std::vector<Point3d> > result;

    // check input
    if (vertices.size () < 3){
      return result;
    }

    boost::optional<Vector3d> normal = getOutwardNormal(vertices);
    if (!normal || normal->z() > -0.999){
      return result;
    }

    for (const auto& hole : holes){
      normal = getOutwardNormal(hole);
      if (!normal || normal->z() > -0.999){
        return result;
      }
    }

    std::vector<Point3d> allPoints;

    // PolyPartition does not support holes which intersect the polygon or share an edge
    // if any hole is not fully contained we will use boost to remove all the holes
    bool polyPartitionHoles = true;
    for (const std::vector<Point3d>& hole : holes){
      if (!within(hole, vertices, tol)){
        // PolyPartition can't handle this
        polyPartitionHoles = false;
        break;
      }
    }

    if (!polyPartitionHoles){
      // use boost to do all the intersections
      std::vector<std::vector<Point3d> > allFaces = subtract(vertices, holes, tol);
      std::vector<std::vector<Point3d> > noHoles;
      for (const std::vector<Point3d>& face : allFaces){
        std::vector<std::vector<Point3d> > temp = computeTriangulation(face, noHoles);
        result.insert(result.end(), temp.begin(), temp.end());
      }
      return result;
    }

    // convert input to vector of TPPLPoly
    std::list<TPPLPoly> polys;

    TPPLPoly outerPoly; // must be counter-clockwise, input vertices are clockwise
    outerPoly.Init(vertices.size());
    outerPoly.SetHole(false);
    unsigned n = vertices.size();
    for(unsigned i = 0; i < n; ++i){

      // should all have zero z coordinate now
      double z = vertices[n-i-1].z();
      if (abs(z) > tol){
        LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
        return result;
      }

      Point3d point = getCombinedPoint(vertices[n-i-1], allPoints, tol);
      outerPoly[i].x = point.x();
      outerPoly[i].y = point.y();
    }
    outerPoly.SetOrientation(TPPL_CCW);
    polys.push_back(outerPoly);

    for (const std::vector<Point3d>& holeVertices : holes){

      if (holeVertices.size () < 3){
        LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Hole has fewer than 3 points, ignoring");
        continue;
      }

      TPPLPoly innerPoly; // must be clockwise, input vertices are clockwise
      innerPoly.Init(holeVertices.size());
      innerPoly.SetHole(true);
      //std::cout << "inner :";
      for(unsigned i = 0; i < holeVertices.size(); ++i){

        // should all have zero z coordinate now
        double z = holeVertices[i].z();
        if (abs(z) > tol){
          LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
          return result;
        }

        Point3d point = getCombinedPoint(holeVertices[i], allPoints, tol);
        innerPoly[i].x = point.x();
        innerPoly[i].y = point.y();
      }
      innerPoly.SetOrientation(TPPL_CW);
      polys.push_back(innerPoly);
    }

    // do partitioning
    TPPLPartition pp;
    std::list<TPPLPoly> resultPolys;
    int test = pp.Triangulate_EC(&polys,&resultPolys);
    if (test == 0){
      test = pp.Triangulate_MONO(&polys, &resultPolys);
    }
    if (test == 0){
      LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Failed to partition polygon");
      return result;
    }

    // convert back to vertices
    std::list<TPPLPoly>::iterator it, itend;
    //std::cout << "Start" << std::endl;
    for(it = resultPolys.begin(), itend = resultPolys.end(); it != itend; ++it){

      it->SetOrientation(TPPL_CW);

      std::vector<Point3d> triangle;
      for (long i = 0; i < it->GetNumPoints(); ++i){
        TPPLPoint point = it->GetPoint(i);
        triangle.push_back(Point3d(point.x, point.y, 0));
      }
      //std::cout << triangle << std::endl;
      result.push_back(triangle);
    }
    //std::cout << "End" << std::endl;

    return result;
  }

  std::vector<Point3d> moveVerticesTowardsPoint(const Point3dVector& vertices, const Point3d& point, double distance)
  {
    Point3dVector result;
    for (const Point3d& vertex : vertices){
      Vector3d vector = point-vertex;
      vector.setLength(distance);
      result.push_back(vertex+vector);
    }
    return result;
  }

  std::vector<Point3d> reverse(const Point3dVector& vertices)
  {
    std::vector<Point3d> result(vertices);
    std::reverse(result.begin(), result.end());
    return result;
  }

} // openstudio


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

#include "precomp.hpp"

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/core/eigen.hpp>
#include <opencv2/sfm/numeric.hpp>
#include <opencv2/sfm/projection.hpp>

// libmv headers
#include "libmv/multiview/projection.h"

#include <iostream>

namespace cv
{
namespace sfm
{

template<typename T>
void
homogeneousToEuclidean(const Mat & X_, Mat & x_)
{
  int d = X_.rows - 1;

  const Mat_<T> & X_rows = X_.rowRange(0,d);
  const Mat_<T> h = X_.row(d);

  const T * h_ptr = h[0], *h_ptr_end = h_ptr + h.cols;
  const T * X_ptr = X_rows[0];
  T * x_ptr = x_.ptr<T>(0);
  for (; h_ptr != h_ptr_end; ++h_ptr, ++X_ptr, ++x_ptr)
  {
    const T * X_col_ptr = X_ptr;
    T * x_col_ptr = x_ptr, *x_col_ptr_end = x_col_ptr + d * x_.step1();
    for (; x_col_ptr != x_col_ptr_end; X_col_ptr+=X_rows.step1(), x_col_ptr+=x_.step1() )
      *x_col_ptr = (*X_col_ptr) / (*h_ptr);
  }
}

void
homogeneousToEuclidean(InputArray X_, OutputArray x_)
{
  // src
  const Mat X = X_.getMat();

  // dst
   x_.create(X.rows-1, X.cols, X.type());
  Mat x = x_.getMat();

  // type
  if( X.depth() == CV_32F )
  {
    homogeneousToEuclidean<float>(X,x);
  }
  else
  {
    homogeneousToEuclidean<double>(X,x);
  }
}

void
euclideanToHomogeneous(InputArray x_, OutputArray X_)
{
  const Mat x = x_.getMat();
  const Mat last_row = Mat::ones(1, x.cols, x.type());
  vconcat(x, last_row, X_);
}

template<typename T>
void
projectionFromKRt(const Mat_<T> &K, const Mat_<T> &R, const Mat_<T> &t, Mat_<T> P)
{
  hconcat( K*R, K*t, P );
}

void
projectionFromKRt(InputArray K_, InputArray R_, InputArray t_, OutputArray P_)
{
  const Mat K = K_.getMat(), R = R_.getMat(), t = t_.getMat();
  const int depth = K.depth();
  CV_Assert((K.cols == 3 && K.rows == 3) && (t.cols == 1 && t.rows == 3) && (K.size() == R.size()));
  CV_Assert((depth == CV_32F || depth == CV_64F) && depth == R.depth() && depth == t.depth());

  P_.create(3, 4, depth);

  Mat P = P_.getMat();

  // type
  if( depth == CV_32F )
  {
    projectionFromKRt<float>(K, R, t, P);
  }
  else
  {
    projectionFromKRt<double>(K, R, t, P);
  }

}

template<typename T>
void
KRtFromProjection( const Mat_<T> &P_, Mat_<T> K_, Mat_<T> R_, Mat_<T> t_ )
{
  libmv::Mat34 P;
  libmv::Mat3 K, R;
  libmv::Vec3 t;

  cv2eigen( P_, P );

  libmv::KRt_From_P( P, &K, &R, &t );

  eigen2cv( K, K_ );
  eigen2cv( R, R_ );
  eigen2cv( t, t_ );
}

void
KRtFromProjection( InputArray P_, OutputArray K_, OutputArray R_, OutputArray t_ )
{
  const Mat P = P_.getMat();
  const int depth = P.depth();
  CV_Assert((P.cols == 4 && P.rows == 3) && (depth == CV_32F || depth == CV_64F));

  K_.create(3, 3, depth);
  R_.create(3, 3, depth);
  t_.create(3, 1, depth);

  Mat K = K_.getMat(), R = R_.getMat(), t = t_.getMat();

  // type
  if( depth == CV_32F )
  {
    KRtFromProjection<float>(P, K, R, t);
  }
  else
  {
    KRtFromProjection<double>(P, K, R, t);
  }
}

template<typename T>
T
depthValue( const Mat_<T> &R_, const Mat_<T> &t_, const Mat_<T> &X_ )
{
  Matx<T,3,3> R(R_);
  Vec<T,3> t(t_);

  if ( X_.rows == 3)
  {
    Vec<T,3> X(X_);
    return (R*X)(2) + t(2);
  }
  else
  {
    Vec<T,4> X(X_);
    Vec<T,3> Xe;
    homogeneousToEuclidean(X,Xe);
    return depthValue<T>( Mat(R), Mat(t), Mat(Xe) );
  }
}

double
depth( InputArray R_, InputArray t_, InputArray X_)
{
  const Mat R = R_.getMat(), t = t_.getMat(), X = X_.getMat();
  const int depth = R.depth();
  CV_Assert( R.rows == 3 && R.cols == 3 && t.rows == 3 && t.cols == 1 );
  CV_Assert( (X.rows == 3 && X.cols == 1) || (X.rows == 4 && X.cols == 1) );
  CV_Assert( depth == CV_32F || depth == CV_64F );

  double depth_value = 0.0;

  if ( depth == CV_32F )
  {
    depth_value = static_cast<double>(depthValue<float>(R, t, X));
  }
  else
  {
    depth_value = depthValue<double>(R, t, X);
  }

  return depth_value;
}

} /* namespace sfm */
} /* namespace cv */


#include <iostream>
#include <vector>
#include <fstream>
#include <list>
#include <set>
#include <stack>
#include <algorithm>
#include <boost/date_time.hpp>

typedef std::vector<std::vector<unsigned long>> Graph;

std::stack<unsigned long> topological(Graph vertices, unsigned long vertice_count){

    std::cout << ("finding topological") << std::endl;
    std::set<unsigned long> notfound;
    for(unsigned long i=0;i<vertices.size();i++) notfound.emplace(i);
    std::set<unsigned long> found;
    std::stack<unsigned long> topo;
    std::stack<std::pair<unsigned long, bool>> stack;

    while (topo.size() < vertices.size()){
        if (stack.empty()){
            auto p = notfound.begin();
            stack.emplace(*p, false);
        }
        std::pair<unsigned long,bool> holder = stack.top();
        stack.pop();
        unsigned long i = holder.first;
        bool backtracking = holder.second;
        auto pos = found.find(i);
        if (not backtracking and pos == found.end()){
            notfound.erase(notfound.find(i));
            found.emplace(i);
            std::vector<unsigned long> children = vertices[i];
            bool pisfound = false;
            for(auto p = children.begin(); p != children.end(); pisfound ? p = children.erase(p) : p++ ){ // remove found children
                pisfound = false;
                if (found.find(*p) != found.end()){
                    pisfound = true;
                }
            }
            if(not children.empty()){
                stack.emplace(i, true);
                for (unsigned long v : children){
                    stack.emplace(v, false);
                }
            } else {
                topo.emplace(i);
            }
        } else if (backtracking){
            topo.emplace(i);
        }

    }
    return topo;
}

void load_vertices(const std::string &filename, bool get_r,
        Graph &vertices, Graph &vertices_r
                ,unsigned long &vertice_count, unsigned long &edge_count){

    std::cout << "reading file" << std::endl;
    std::fstream file(filename, std::ios::in);

    file >> vertice_count >> edge_count;

    vertices.reserve(static_cast<unsigned long>(vertice_count));
    vertices_r.reserve(static_cast<unsigned long>(vertice_count));
    for(unsigned long _ = 0; _ < vertice_count; _++){
        vertices.emplace_back(std::vector<unsigned long>());
        if (get_r) vertices_r.emplace_back(std::vector<unsigned long>());
    }

    unsigned long to, fro;
    while (file >> to >> fro) {
        vertices[to].emplace_back(fro);
        if (get_r) vertices_r[fro].emplace_back(to);
    }
    file.close();
}

std::vector<std::vector<unsigned long>> findComponents(Graph vertices, unsigned long vertice_count, std::stack<unsigned long> order){
    std::cout << "finding components" << std::endl;
    std::vector<unsigned long> nodecomp;
    nodecomp.reserve(static_cast<unsigned long>(vertice_count));
    for(unsigned long _ = 0; _ < vertice_count; _++) nodecomp.emplace_back(ULONG_MAX);
    unsigned long comp = 0;

    while (not order.empty()){
        unsigned long v = order.top();
        order.pop();

        if (nodecomp[v] == ULONG_MAX){
            std::vector<unsigned long> children = vertices[v];
            nodecomp[v] = comp;
            while (not children.empty()){
                auto p = children.begin();
                unsigned long c = *p;
                children.erase(p);
                if (nodecomp[c] == ULONG_MAX){
                    nodecomp[c] = comp;
                    std::vector<unsigned long> subch = vertices[c];
                    for (unsigned long i : subch) children.emplace_back(i);

                }
            }
            comp++;
        }

    }
    std::vector<std::vector<unsigned long>> result;
    result.reserve(static_cast<unsigned long>(comp));
    for(unsigned long _ = 0; _ < comp; _++) result.emplace_back(std::vector<unsigned long>());
    for(unsigned long i = 0; i < nodecomp.size(); i++){
        unsigned long v = nodecomp[i];
        result[v].emplace_back(i);
    }
    return result;

}

int main(int argc, char* argv[]) {

    std::string filename = "L7Skandinavia";
    if (argc > 1)
        filename = argv[1];

    Graph vertices;
    Graph vertices_r;
    unsigned long vertice_count = 0;
    unsigned long edge_count = 0;

    boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time();
    load_vertices(filename, true, vertices, vertices_r, vertice_count, edge_count);
     boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time();
    std::cout << "loaded " << vertices.size() << " vertices in " << (end-start).total_microseconds()/1000000. << " seconds" << std::endl;

    start = boost::posix_time::microsec_clock::local_time();
    std::stack<unsigned long> topo = topological(vertices, vertice_count);
    end = boost::posix_time::microsec_clock::local_time();
    auto ostart = start;
    std::cout << "found topological order in " << (end-start).total_microseconds()/1000000. << " seconds" << std::endl;

    start = boost::posix_time::microsec_clock::local_time();
    auto strongs = findComponents(vertices_r, vertice_count, topo);
    end = boost::posix_time::microsec_clock::local_time();
    std::cout << "found components in " << (end-start).total_microseconds()/1000000. << " seconds" << std::endl;
    std::cout << "total time: " << (end-ostart).total_microseconds()/1000000 << " seconds" << std::endl;

    std::cout << "grafen " << filename << " har " << strongs.size() << " sterkt sammenhengende komponenter." << std::endl;
    if (strongs.size() < 100){
        std::cout << "Komponent\t\tNoder i komponenten" << std::endl;
        for(unsigned long i = 0; i < strongs.size(); i++){
            std::vector<unsigned long> v = strongs[i];
            std::cout << i << "\t\t\t\t";
            for(unsigned long j = 0; j < v.size(); j++){
                std::cout << v[j];
                if (j+1 < v.size()) std::cout << ", ";
            }
            std::cout << std::endl;
        }
    }

    return EXIT_SUCCESS;

}


#include "NumberMpq.h"

#include "NumberClI.h"
#include "NumberClRA.h"
#include "NumberMpz.h"

#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

namespace carl {

#ifdef USE_CLN_NUMBERS
		Number<mpq_class>::Number(const Number<cln::cl_RA>& n) : Number(n.toString()) {}
		Number<mpq_class>::Number(const Number<cln::cl_I>& n) : Number(n.toString()) {}
#endif

	//TODO: probably also add possibility to construct from fraction strings; is there a reason for this complicated method? instead of using mpq_class constructor??
	Number<mpq_class>::Number(const std::string& s) {
		std::vector<std::string> strs;
		boost::split(strs, s, boost::is_any_of("."));

		if(strs.size() > 2)
		{
		    throw std::invalid_argument("More than one delimiter in the string.");
		}
		mpq_class result;
		if(!strs.front().empty())
		{
		    result += mpq_class(strs.front());
		}
		if(strs.size() > 1)
		{
		    //if(strs.back().size() > )
		    result += (mpq_class(strs.back())/carl::pow(mpz_class(10),static_cast<unsigned>(strs.back().size())));
		}
		mData = result;
	}

	//constructs a/b:
	Number<mpq_class>::Number(const Number<mpz_class>& a,const Number<mpz_class>& b) { mData = mpq_class(a.getValue(),b.getValue()); }

	Number<mpq_class>::Number(const Number<mpz_class>& n) { mData = mpq_class(n.getValue()); }
	Number<mpq_class>::Number(const mpz_class& n) { mData = mpq_class(n); }

	//TODO: doesn't mpq_class have a standard "output" as well?
	std::string Number<mpq_class>::toString(bool _infix) const
	{
		std::stringstream s;
		bool negative = (mData < mpq_class(0));
		if(negative) s << "(-" << (_infix ? "" : " ");
		if(_infix) s << this->abs();
		else
		{
		    mpz_class d = mData.get_den();
		    if(constant_one<mpz_class>::get() != mData) {
			mpz_class abs1, abs2;
			mpz_abs(abs1.get_mpz_t(), mData.get_num().get_mpz_t());
			mpz_abs(abs2.get_mpz_t(), d.get_mpz_t());
			s << "(/ " << abs1 << " " << abs2 << ")";
		    } else {
			s << this->abs().mData;
			}
		}
		if(negative)
		    s << ")";
		return s.str();
	}

	 bool Number<mpq_class>::sqrt_exact(Number<mpq_class>& b) const
	    {
		if( mpq_sgn(mData.__get_mp()) < 0 ) return false;
		mpz_class den = mData.get_den();
		mpz_class num = mData.get_num();
		mpz_class root_den;
		mpz_class root_den_rem;
		mpz_sqrtrem(root_den.__get_mp(), root_den_rem.__get_mp(), den.__get_mp());
		if( !Number( root_den_rem ).isZero() )
		    return false;

		mpz_class root_num;
		mpz_class root_num_rem;
		mpz_sqrtrem(root_num.__get_mp(), root_num_rem.__get_mp(), num.__get_mp());
		if( !Number( root_num_rem ).isZero() )
		    return false;

		mpq_class resNum;
		mpq_set_z(resNum.get_mpq_t(), root_num.get_mpz_t());
		mpq_class resDen;
		mpq_set_z(resDen.get_mpq_t(), root_den.get_mpz_t());

		mpq_class fraction;
		mpq_div(fraction.get_mpq_t(), resNum.get_mpq_t(), resDen.get_mpq_t());

		b = Number<mpq_class>(fraction);
		return true;
	    }

	    Number<mpq_class> Number<mpq_class>::sqrt() const {
		std::pair<Number<mpq_class>,Number<mpq_class>> r = this->sqrt_safe();
		return (r.first + r.second) / 2;
	    }

	    std::pair<Number<mpq_class>,Number<mpq_class>> Number<mpq_class>::sqrt_safe() const
	    {
		assert( mpq_sgn(mData.__get_mp()) > 0 );
		mpz_class den = mData.get_den();
		mpz_class num = mData.get_num();
		mpz_class root_den;
		mpz_class root_den_rem;
		mpz_sqrtrem(root_den.__get_mp(), root_den_rem.__get_mp(), den.__get_mp());

		mpz_class root_num;
		mpz_class root_num_rem;
		mpz_sqrtrem(root_num.__get_mp(), root_num_rem.__get_mp(), num.__get_mp());

		mpq_class lower;
		mpq_class upper;

		lower = root_num;
		if(root_den_rem == 0)
		    lower /= root_den;
		else
		    lower /= root_den+1;

		if(root_num_rem == 0)
		    upper = root_num;
		else
		    upper = root_num+1;

		upper /= root_den;

		return std::make_pair(Number(lower),Number(upper));
	    }

	    std::pair<Number<mpq_class>, Number<mpq_class>> Number<mpq_class>::sqrt_fast() const
	    {
		assert(mData >= 0);
	#if 1
		return sqrt_safe(); //NOTE: there was something (probably) equivalent to sqrt_safe() here, so this replacement should be ok
	#else
		mpq_class exact_root;
		if (carl::sqrtp(mData, exact_root)) {
		    // root can be computed exactly.
		    return std::make_pair(exact_root, exact_root);
		} else {
		    // compute an approximation with sqrt(). we can assume that the surrounding integers contain the actual root.
		    mpf_class af = sqrt(mpf_class(mData));
		    mpq_class lower(af - carl::constant_one<mpf_class>::get());
		    mpq_class upper(af + carl::constant_one<mpf_class>::get());
		    assert(lower * lower < mData);
		    assert(upper * upper > mData);
		    return std::make_pair(lower, upper);
		}
	#endif
	    }

}


#ifndef MIA_SPACIAL_HPP
#define MIA_SPACIAL_HPP

#include <array>
#include <cstddef>
#include <functional>
#include <tuple>
#include <type_traits>
#include <utility>
#include <gsl/gsl_assert>
#include <gsl/gsl_util>
#include <boost/hana/equal.hpp>
#include <boost/hana/ext/std/array.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/unpack.hpp>
#include <range/v3/utility/common_type.hpp>
#include <range/v3/utility/concepts.hpp>
#include <jegp/utility.hpp>
#include <mia/concepts.hpp>
#include <mia/units.hpp>

namespace mia {

enum class Axis { x, y, z };

template <Axis A, class Qty>
class Coordinate;

namespace detail {

    template <Axis A>
    struct Mp_coordinate {
        template <class Qty>
        using fn = Coordinate<A, Qty>;
    };

} // namespace detail

template <Axis A, class Qty>
class Coordinate
  : public Unit_alias<detail::Mp_coordinate<A>::template fn, Qty> {
public:
    static_assert(A >= Axis::x);

    static constexpr Axis axis{A};

    using Unit_alias<detail::Mp_coordinate<A>::template fn, Qty>::Unit_alias;
};

template <class Qty>
using Abscissa = Coordinate<Axis::x, Qty>;
template <class Qty>
using Ordinate = Coordinate<Axis::y, Qty>;
template <class Qty>
using Applicate = Coordinate<Axis::z, Qty>;

} // namespace mia

namespace ranges {

template <mia::Axis A, class Qty1, class Qty2>
struct common_type<mia::Coordinate<A, Qty1>, mia::Coordinate<A, Qty2>> {
    using type = mia::Coordinate<A, common_type_t<Qty1, Qty2>>;
};

} // namespace ranges

namespace std {

template <mia::Axis A, class Qty>
struct hash<mia::Coordinate<A, Qty>>
  : mia::detail::Unit_alias_hash<
        mia::detail::Mp_coordinate<A>::template fn, Qty> {
};

} // namespace std

namespace mia {

enum class Coordinates;

template <Coordinates C, class Qty>
class Point {
public:
    static_assert(C >= Coordinates{1});
    static_assert(Quantity<Qty>{});

    static constexpr Coordinates coordinates{C};
    using quantity = Qty;

private:
    static constexpr boost::hana::range<int, 0, jegp::underlying(coordinates)>
        ints{};

    static constexpr std::array axes{boost::hana::unpack(
        ints, [](auto... int_c) { return std::array{Axis{int_c()}...}; })};

public:
    Point()             = default;
    Point(const Point&) = default;
    Point(Point&&)      = default;
    ~Point()            = default;
    Point& operator=(const Point&) = default;
    Point& operator=(Point&&) = default;

    template <
        Axis... A, class... Qty2,
        CONCEPT_REQUIRES_(
            std::conjunction_v<
                std::bool_constant<boost::hana::equal(axes, std::array{A...})>,
                ranges::Constructible<
                    Coordinate<A, quantity>, const Coordinate<A, Qty2>&>...>)>
    explicit constexpr Point(const Coordinate<A, Qty2>&... coord) noexcept(
        std::conjunction_v<std::is_nothrow_constructible<
            Coordinate<A, quantity>, const Coordinate<A, Qty2>&>...>)
      : coords{coord...}
    {
    }

    template <
        class Qty2, CONCEPT_REQUIRES_(ranges::Constructible<quantity, Qty2>{})>
    constexpr Point(const Point<coordinates, Qty2>& pt) noexcept(
        std::is_nothrow_copy_constructible_v<Qty2>&&
            std::is_nothrow_constructible_v<quantity, Qty2>)
      : coords{boost::hana::unpack(ints, [&](auto... int_c) {
            return Coords{pt.template get<int_c>()...};
        })}
    {
    }

    constexpr Abscissa<quantity>& x() noexcept
    {
        return std::get<jegp::underlying(Axis::x)>(coords);
    }
    constexpr const Abscissa<quantity>& x() const noexcept
    {
        return std::get<jegp::underlying(Axis::x)>(coords);
    }

    template <Axis A = Axis::y, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr Ordinate<quantity>& y() noexcept
    {
        return std::get<jegp::underlying(A)>(coords);
    }
    template <Axis A = Axis::y, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr const Ordinate<quantity>& y() const noexcept
    {
        return std::get<jegp::underlying(A)>(coords);
    }

    template <Axis A = Axis::z, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr Applicate<quantity>& z() noexcept
    {
        return std::get<jegp::underlying(A)>(coords);
    }
    template <Axis A = Axis::z, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr const Applicate<quantity>& z() const noexcept
    {
        return std::get<jegp::underlying(A)>(coords);
    }

    template <std::size_t I>
    constexpr Coordinate<Axis{I}, quantity>& get() noexcept
    {
        return std::get<I>(coords);
    }
    template <std::size_t I>
    constexpr const Coordinate<Axis{I}, quantity>& get() const noexcept
    {
        return std::get<I>(coords);
    }

private:
    static constexpr auto coords_tuple = [](auto... int_c) {
        return std::tuple<Coordinate<axes[int_c], quantity>...>{};
    };

    using Coords = decltype(boost::hana::unpack(ints, coords_tuple));

    Coords coords;
};

template <Axis... A, class Qty>
explicit Point(Coordinate<A, Qty>...)->Point<Coordinates{sizeof...(A)}, Qty>;

template <class Qty>
using Point2d = Point<Coordinates{2}, Qty>;
template <class Qty>
using Point3d = Point<Coordinates{3}, Qty>;

template <
    std::size_t I, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(I < jegp::underlying(C))>
constexpr Coordinate<Axis{I}, Qty>& get(Point<C, Qty>& pt) noexcept
{
    return pt.template get<I>();
}
template <
    std::size_t I, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(I < jegp::underlying(C))>
constexpr const Coordinate<Axis{I}, Qty>& get(const Point<C, Qty>& pt) noexcept
{
    return pt.template get<I>();
}

template <
    Axis A, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(Axis::x <= A && A < static_cast<Axis>(C))>
constexpr Coordinate<A, Qty>& get(Point<C, Qty>& pt) noexcept
{
    return pt.template get<jegp::underlying(A)>();
}
template <
    Axis A, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(Axis::x <= A && A < static_cast<Axis>(C))>
constexpr const Coordinate<A, Qty>& get(const Point<C, Qty>& pt) noexcept
{
    return pt.template get<jegp::underlying(A)>();
}

template <
    class Coord, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(
        std::is_same_v<Coord, Coordinate<Coord::axis, Qty>> &&
        (Axis::x <= Coord::axis && Coord::axis < static_cast<Axis>(C)))>
constexpr Coord& get(Point<C, Qty>& pt) noexcept
{
    return pt.template get<jegp::underlying(Coord::axis)>();
}
template <
    class Coord, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(
        std::is_same_v<Coord, Coordinate<Coord::axis, Qty>> &&
        (Axis::x <= Coord::axis && Coord::axis < static_cast<Axis>(C)))>
constexpr const Coord& get(const Point<C, Qty>& pt) noexcept
{
    return pt.template get<jegp::underlying(Coord::axis)>();
}

template <
    Coordinates C, class Qty1, class Qty2,
    CONCEPT_REQUIRES_(ranges::EqualityComparable<Qty1, Qty2>{})>
constexpr auto operator==(
    const Point<C, Qty1>& l,
    const Point<C, Qty2>&
        r) noexcept(noexcept(std::declval<Qty1>() == std::declval<Qty2>()))
{
    return boost::hana::unpack(
        boost::hana::range_c<int, 0, jegp::underlying(C)>, [&](auto... int_c) {
            return (... && (get<int_c>(l) == get<int_c>(r)));
        });
}
template <Coordinates C, class Qty1, class Qty2>
constexpr auto operator!=(
    const Point<C, Qty1>& l,
    const Point<C, Qty2>& r) noexcept(noexcept(!(l == r)))
    -> decltype(!(l == r))
{
    return !(l == r);
}

} // namespace mia

namespace std {

template <mia::Coordinates C, class Qty>
struct tuple_size<mia::Point<C, Qty>>
  : integral_constant<size_t, jegp::underlying(C)> {
};

template <size_t I, mia::Coordinates C, class Qty>
struct tuple_element<I, mia::Point<C, Qty>> {
    static_assert(I < jegp::underlying(C));

    using type = mia::Coordinate<mia::Axis{I}, Qty>;
};

} // namespace std

namespace ranges {

template <mia::Coordinates C, class Qty1, class Qty2>
struct common_type<mia::Point<C, Qty1>, mia::Point<C, Qty2>> {
    using type = mia::Point<C, common_type_t<Qty1, Qty2>>;
};

} // namespace ranges

namespace mia::detail {

template <class Tuple, class = void>
struct Tuple_hash
  : Tuple_hash<Tuple, std::make_index_sequence<std::tuple_size_v<Tuple>>> {
};

template <class Tuple, std::size_t... I>
struct Tuple_hash<Tuple, std::index_sequence<I...>>
  : private std::hash<std::tuple_element_t<I, Tuple>>... {
    constexpr auto operator()(const Tuple& t) const
        noexcept(noexcept(jegp::hash_combine(get<I>(t)...)))
            -> decltype(jegp::hash_combine(get<I>(t)...))
    {
        return jegp::hash_combine(get<I>(t)...);
    }
};

} // namespace mia::detail

namespace std {

template <mia::Coordinates C, class Qty>
struct hash<mia::Point<C, Qty>> : mia::detail::Tuple_hash<mia::Point<C, Qty>> {
};

} // namespace std

namespace mia {

template <Axis A, class Qty>
class Dimension;

namespace detail {

    template <Axis A>
    struct Mp_dimension {
        template <class Qty>
        using fn = Dimension<A, Qty>;
    };

} // namespace detail

template <Axis A, class Qty>
class Dimension : public Unit_alias<detail::Mp_dimension<A>::template fn, Qty> {
public:
    static_assert(A >= Axis::x);

    static constexpr Axis axis{A};

    using Unit_alias<detail::Mp_dimension<A>::template fn, Qty>::Unit_alias;
};

template <class Qty>
using Width = Dimension<Axis::x, Qty>;
template <class Qty>
using Height = Dimension<Axis::y, Qty>;
template <class Qty>
using Depth = Dimension<Axis::z, Qty>;

} // namespace mia

namespace ranges {

template <mia::Axis A, class Qty1, class Qty2>
struct common_type<mia::Dimension<A, Qty1>, mia::Dimension<A, Qty2>> {
    using type = mia::Dimension<A, common_type_t<Qty1, Qty2>>;
};

} // namespace ranges

namespace std {

template <mia::Axis A, class Qty>
struct hash<mia::Dimension<A, Qty>>
  : mia::detail::Unit_alias_hash<
        mia::detail::Mp_dimension<A>::template fn, Qty> {
};

} // namespace std

namespace mia {

enum class Dimensions;

inline namespace literals {
    inline namespace dimensions_literals { //

        constexpr Dimensions operator""_D(unsigned long long d)
        {
            return gsl::narrow_cast<Dimensions>(d);
        }

    } // namespace dimensions_literals
} // namespace literals

template <Dimensions D, class Qty>
class Size {
public:
    static_assert(D >= 1_D);
    static_assert(Quantity<Qty>{});

    static constexpr Dimensions dimensions{D};
    using quantity = Qty;

private:
    static constexpr boost::hana::range<int, 0, jegp::underlying(dimensions)>
        ints{};

    static constexpr std::array axes{boost::hana::unpack(
        ints, [](auto... int_c) { return std::array{Axis{int_c()}...}; })};

public:
    Size(const Size&) = default;
    Size(Size&&)      = default;
    ~Size()           = default;
    Size& operator=(const Size&) = default;
    Size& operator=(Size&&) = default;

    template <
        Axis... A, class... Qty2,
        CONCEPT_REQUIRES_(
            std::conjunction_v<
                std::bool_constant<boost::hana::equal(axes, std::array{A...})>,
                ranges::Constructible<
                    Dimension<A, quantity>, const Dimension<A, Qty2>&>...>)>
    explicit constexpr Size(const Dimension<A, Qty2>&... dim)
      : dims{[&] {
            (..., Expects((dim > Dimension<A, Qty2>{})));
            return Dims{dim...};
        }()}
    {
    }

    template <
        class Qty2, CONCEPT_REQUIRES_(ranges::Constructible<quantity, Qty2>{})>
    constexpr Size(const Size<dimensions, Qty2>& sz) noexcept(
        std::is_nothrow_copy_constructible_v<Qty2>&&
            std::is_nothrow_constructible_v<quantity, Qty2>)
      : dims{boost::hana::unpack(ints, [&](auto... int_c) {
            return Dims{sz.template get<int_c>()...};
        })}
    {
    }

    constexpr void w(const Width<quantity>& w)
    {
        Expects(w > Width<quantity>{});
        std::get<jegp::underlying(Axis::x)>(dims) = w;
    }
    template <Axis A = Axis::y, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr void h(const Height<quantity>& h)
    {
        Expects(h > Height<quantity>{});
        std::get<jegp::underlying(A)>(dims) = h;
    }
    template <Axis A = Axis::z, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr void d(const Depth<quantity>& d)
    {
        Expects(d > Depth<quantity>{});
        std::get<jegp::underlying(A)>(dims) = d;
    }

    template <Axis A, CONCEPT_REQUIRES_(Axis::x <= A && A <= axes.back())>
    constexpr void set(const Dimension<A, quantity>& dim)
    {
        Expects((dim > Dimension<A, quantity>{}));
        std::get<jegp::underlying(A)>(dims) = dim;
    }

    constexpr Width<quantity> w() const
        noexcept(std::is_nothrow_copy_constructible_v<Width<quantity>>)
    {
        return std::get<jegp::underlying(Axis::x)>(dims);
    }
    template <Axis A = Axis::y, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr Height<quantity> h() const
        noexcept(std::is_nothrow_copy_constructible_v<Height<quantity>>)
    {
        return std::get<jegp::underlying(A)>(dims);
    }
    template <Axis A = Axis::z, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr Depth<quantity> d() const
        noexcept(std::is_nothrow_copy_constructible_v<Depth<quantity>>)
    {
        return std::get<jegp::underlying(A)>(dims);
    }

    template <std::size_t I>
    constexpr Dimension<Axis{I}, quantity> get() const noexcept(
        std::is_nothrow_copy_constructible_v<Dimension<Axis{I}, quantity>>)
    {
        return std::get<I>(dims);
    }

private:
    static constexpr auto dims_tuple = [](auto... int_c) {
        return std::tuple<Dimension<axes[int_c], quantity>...>{};
    };

    using Dims = decltype(boost::hana::unpack(ints, dims_tuple));

    Dims dims;
};

template <Axis... A, class Qty>
explicit Size(Dimension<A, Qty>...)->Size<Dimensions{sizeof...(A)}, Qty>;

template <class Qty>
using Size2d = Size<2_D, Qty>;
template <class Qty>
using Size3d = Size<3_D, Qty>;

template <
    std::size_t I, Dimensions D, class Qty,
    CONCEPT_REQUIRES_(I < jegp::underlying(D))>
constexpr Dimension<Axis{I}, Qty> get(const Size<D, Qty>& sz) noexcept(
    std::is_nothrow_copy_constructible_v<Dimension<Axis{I}, Qty>>)
{
    return sz.template get<I>();
}
template <
    Axis A, Dimensions D, class Qty,
    CONCEPT_REQUIRES_(Axis::x <= A && A < static_cast<Axis>(D))>
constexpr Dimension<A, Qty> get(const Size<D, Qty>& sz) noexcept(
    std::is_nothrow_copy_constructible_v<Dimension<A, Qty>>)
{
    return sz.template get<jegp::underlying(A)>();
}
template <
    class Dim, Dimensions D, class Qty,
    CONCEPT_REQUIRES_(
        std::is_same_v<Dim, Dimension<Dim::axis, Qty>> &&
        (Axis::x <= Dim::axis && Dim::axis < static_cast<Axis>(D)))>
constexpr Dim get(const Size<D, Qty>& sz) noexcept(
    std::is_nothrow_copy_constructible_v<Dim>)
{
    return sz.template get<jegp::underlying(Dim::axis)>();
}

template <
    Dimensions D, class Qty1, class Qty2,
    CONCEPT_REQUIRES_(ranges::EqualityComparable<Qty1, Qty2>{})>
constexpr auto operator==(
    const Size<D, Qty1>& l,
    const Size<D, Qty2>&
        r) noexcept(noexcept(std::declval<Qty1>() == std::declval<Qty2>()))
{
    return boost::hana::unpack(
        boost::hana::range_c<int, 0, jegp::underlying(D)>, [&](auto... int_c) {
            return (... && (get<int_c>(l) == get<int_c>(r)));
        });
}
template <Dimensions D, class Qty1, class Qty2>
constexpr auto operator!=(
    const Size<D, Qty1>& l,
    const Size<D, Qty2>& r) noexcept(noexcept(!(l == r))) -> decltype(!(l == r))
{
    return !(l == r);
}

} // namespace mia

namespace std {

template <mia::Dimensions D, class Qty>
struct tuple_size<mia::Size<D, Qty>>
  : integral_constant<size_t, jegp::underlying(D)> {
};

template <size_t I, mia::Dimensions D, class Qty>
struct tuple_element<I, mia::Size<D, Qty>> {
    static_assert(I < jegp::underlying(D));

    using type = mia::Dimension<mia::Axis{I}, Qty>;
};

} // namespace std

namespace ranges {

template <mia::Dimensions D, class Qty1, class Qty2>
struct common_type<mia::Size<D, Qty1>, mia::Size<D, Qty2>> {
    using type = mia::Size<D, common_type_t<Qty1, Qty2>>;
};

} // namespace ranges

namespace std {

template <mia::Dimensions D, class Qty>
struct hash<mia::Size<D, Qty>> : mia::detail::Tuple_hash<mia::Size<D, Qty>> {
};

} // namespace std

#endif // MIA_SPACIAL_HPP


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2014 by Synge Todo <wistaria@comp-phys.org>
*
* 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)
*
*****************************************************************************/

// C++ version of TITPACK Ver.2 by H. Nishimori

#ifndef TITPACK_SMALL_HPP
#define TITPACK_SMALL_HPP

#include "common.hpp"
#include "hamiltonian.hpp"
#include <vector>
#include <boost/tuple/tuple.hpp>

//
// matrix elements
//

template<typename MATRIX>
void elm3(hamiltonian const& hop, MATRIX& elemnt) {
  elemnt.set_zeros();
  for (int k = 0; k < hop.num_bonds(); ++k) {
    int isite1, isite2;
    boost::tie(isite1, isite2) = hop.site_pair(k);
    int is = (1 << isite1) + (1 << isite2);
    double wght = hop.bond_weight(k);
    double diag = 0.5 * wght * hop.z_ratio(k);
    #pragma omp parallel for schedule(static)
    for (int i = 0; i < hop.dimension(); ++i) {
      if (elemnt.is_gindex_myrow(i)) {
        int ibit = hop.config(i) & is;
        if (ibit == 0 || ibit == is) {
          elemnt.update_global(i, i, -diag);
        } else {
          elemnt.update_global(i, i, +diag);
          int newcfg = hop.config2index(hop.config(i) ^ is);
          elemnt.update_global(i, newcfg, -wght);
        }
      }
    }
  }
}

template<typename MATRIX>
void elm3_sx(subspace const& ss, int i1, int i2, MATRIX& elemnt) {
  elemnt.set_zeros();
  if (i1 < 0 || i1 >= ss.num_sites() || i2 < 0 || i2 >= ss.num_sites() || i1 == i2) {
    std::cerr << " #(W01)# Wrong site number given to xcorr\n";
    return;
  }
  int is = (1 << i1) + (1 << i2);
  #pragma omp parallel for schedule(static)
  for (int i = 0; i < ss.dimension(); ++i) {
    if (elemnt.is_gindex_myrow(i)) {
      int ibit = ss.config(i) & is;
      if (ibit != 0 && ibit != is) {
        int newcfg = ss.config2index(ss.config(i) ^ is);
        elemnt.update_global(i, newcfg, 0.25);
      }
    }
  }
}

//
// check of the eigenvector and eigenvalue
//
// elemnt    @ nonzero elements
// x         @ eigenvector to be checked
// xindex
// return value: Hexpec <x*H*x>

template<typename MATRIX>
double check3(MATRIX const& elemnt, MATRIX const& x, int xindex, MATRIX& y) {
  int idim = elemnt.rows();
  double dnorm = 0;
  for (int j = 0; j < idim; ++j) {
    dnorm += x(j, xindex) * x(j, xindex);
  }
  if (dnorm < 1e-30) {
    std::cerr << " #(W18)# Null vector given to check3\n";
    return 0;
  }
  for (int i = 0; i < idim; ++i) {
    y(i, xindex) = 0;
    for (int j = 0; j < idim; ++j) y(i, xindex) += elemnt(i, j) * x(j, xindex);
  }
  double prd = 0;
  for (int i = 0; i < idim; ++i) prd += y(i, xindex) * x(i, xindex);
  std::cout << "---------------------------- Information from check3\n"
            << "<x*H*x> = "<< prd << std::endl
            << "H*x(j)/x(j) (j=min(idim/3,13)-1,idim,max(1,idim/20))";
  int count = 0;
  for (int i = std::min((int)(idim / 3), 13) - 1; i < idim; i += std::max(1,idim/20), ++count) {
    if (count % 4 == 0) std::cout << std::endl;
    std::cout << '\t' << y(i, xindex) / x(i, xindex);
  }
  std::cout << std::endl << "---------------------------------------------------\n";
  return prd;
}

template<typename MATRIX>
double check3_mpi(MATRIX const& elemnt, MATRIX const& x, int xindex, MATRIX& y) {
  int idim = elemnt.get_n_global();
  double dnorm = dot_product(x, false, xindex, x, false, xindex);
  product_v(1.0, elemnt, false, x, false, xindex, 0.0, y, false, xindex);
  double prd = dot_product(x, false, xindex, y, false, xindex) / dnorm;

  if (x.is_gindex(0, xindex)) {
    std::cout << "---------------------------- Information from check3\n"
              << "<x*H*x> = "<< prd << std::endl
              << "H*x(j)/x(j) (j=min(idim/3,13)-1,idim,max(1,idim/20))";
  }
  std::cout << std::flush;
  MPI_Barrier(elemnt.get_grid().get_comm());
  int count = 0;
  for (int i = std::min((int)(idim / 3), 13) - 1; i < idim; i += std::max(1,idim/20), ++count) {
    if (x.is_gindex(i, xindex)) {
      if (count % 4 == 0) std::cout << std::endl;
      std::cout << '\t' << y.get_global(i, xindex) / x.get_global(i, xindex);
    }
    std::cout << std::flush;
    MPI_Barrier(elemnt.get_grid().get_comm());
  }
  if (x.is_gindex(0, xindex)) {
    std::cout << std::endl << "---------------------------------------------------\n";
  }
  std::cout << std::flush;
  return prd;
}

template<typename MATRIX>
void xcorr3_mpi(subspace const& ss, std::vector<int> const& npair, MATRIX const& x, int xindex,
                std::vector<double>& sxx, MATRIX& sx, MATRIX& y) {
  double dnorm = dot_product(x, false, xindex, x, false, xindex);
  int nbond = npair.size() / 2;
  for (int k = 0; k < nbond; ++k) {
    int i1 = npair[k * 2];
    int i2 = npair[k * 2 + 1];
    elm3_sx(ss, i1, i2, sx);
    product_v(1.0, sx, false, x, false, xindex, 0.0, y, false, xindex);
    sxx[k] =dot_product(x, false, xindex, y, false, xindex) / dnorm;
  }
}

#endif


// Copyright 2014 Vinzenz Feenstra
//
// 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.
#ifndef GUARD_LSL_RUNTIME_QUATERNION_HH_INCLUDED
#define GUARD_LSL_RUNTIME_QUATERNION_HH_INCLUDED

#include <boost/math/constants/constants.hpp>
#include <lsl/runtime/vector.hh>

namespace lsl {
namespace runtime {
    struct Quaternion {
        double x, y, z, s;
    };

    inline Quaternion identity(Quaternion) {
        return {0., 0., 0., 1.};
    }

    inline Quaternion add(Quaternion l, Quaternion r) {
        return Quaternion{
            l.x + r.x,
            l.y + r.y,
            l.z + r.z,
            l.s + l.s
        };
    }

    inline Quaternion sub(Quaternion l, Quaternion r) {
        return Quaternion{
            l.x - r.x,
            l.y - r.y,
            l.z - r.z,
            l.s - r.s
        };
    }

    inline Quaternion mul(Quaternion l, Quaternion r) {
        return Quaternion{
            r.s * l.x + r.x * l.s + r.y * l.z - r.z * l.y,
            r.s * l.y + r.y * l.s + r.z * l.x - r.x * l.z,
            r.s * l.z + r.z * l.s + r.x * l.y - r.y * l.x,
            r.s * l.s - r.x * l.x - r.y * l.y - r.z * l.z
        };
    }

    inline Vector mul(Vector l, Quaternion r) {
        Quaternion vq{l.x, l.y, l.z, 0.};
        Quaternion nq{-r.x, -r.y, -r.z, r.s};
        Quaternion result = mul(nq, mul(vq, r));
        return{result.x, result.y, result.z};
    }

    inline Quaternion mul(Quaternion l, double r) {
        return {l.x * r, l.y * r, l.z * r, l.s * r};
    }

    inline Quaternion div(Quaternion l, Quaternion r) {
        r.s = -r.s;
        return mul(l, r);
    }

    inline Vector div(Vector l, Quaternion r) {
        r.s = -r.s;
        return mul(l, r);
    }

    inline Vector operator * (Vector const & a, Quaternion const & b) {
        return mul(a, b);
    }
    inline Quaternion operator * (Quaternion const & a, Quaternion const & b) {
        return mul(a, b);
    }
    inline Quaternion operator * (Quaternion const & a, double b) {
        return mul(a, b);
    }
    inline Quaternion operator / (Quaternion const & a, Quaternion const & b) {
        return div(a, b);
    }
    inline Vector operator / (Vector const & a, Quaternion const & b) {
        return div(a, b);
    }
    inline Quaternion operator + (Quaternion const & a, Quaternion const & b) {
        return add(a, b);
    }
    inline Quaternion operator - (Quaternion const & a, Quaternion const & b) {
        return sub(a, b);
    }

    inline bool operator==(Quaternion l, Quaternion r) {
        return l.x == r.x
            && l.y == r.y
            && l.z == r.z
            && l.s == r.s;
    }

    inline bool operator!=(Quaternion l, Quaternion r) {
        return !(l == r);
    }

    inline Quaternion from_euler(Vector euler) {
        double c1 = cos(euler.x * 0.5);
        double c2 = cos(euler.y * 0.5);
        double c3 = cos(euler.z * 0.5);

        double s1 = sin(euler.x * 0.5);
        double s2 = sin(euler.y * 0.5);
        double s3 = sin(euler.z * 0.5);

        Quaternion b{
            s1 * c2 * c3 + c1 * s2 * s3,
            c1 * s2 * c3 - s1 * c2 * s3,
            s1 * s2 * c3 + c1 * c2 * s3,
            c1 * c2 * c3 - s1 * s2 * s3,
        };

        auto a = Quaternion{0., 0., s3, c3}
                 * Quaternion{0., s2, 0., c2}
                 * Quaternion{s1, 0., 0., s1};
        auto c = a + b;
        auto d = a - b;
        static double const err = 0.00001;
        if(   (fabs(c.x) > err && fabs(d.x) > err)
           || (fabs(c.y) > err && fabs(d.y) > err)
           || (fabs(c.z) > err && fabs(d.z) > err)
           || (fabs(c.s) > err && fabs(d.s) > err)) {
            return b;
        }
        return a;
    }

    static double const FP_MAG_THRESHOLD        = 0.0000001;
    static double const GIMBAL_THRESHOLD        = 0.000436;
    static double const ONE_PART_IN_A_MILLION   = 0.000001;

    inline Vector to_euler(Quaternion r) {
        using namespace boost::math::double_constants;

        double sx = 2. * (r.x * r.s - r.y * r.z);
        double sy = 2. * (r.y * r.s + r.x * r.z);

        double ys = r.s * r.s - r.y * r.y;
        double xz = r.x * r.x - r.z * r.z;

        double cx = ys - xz;
        double cy = sqrt(sx * sx + cx * cx);

        if(cy > GIMBAL_THRESHOLD) {
            return Vector{
                atan2(sx, cx),
                atan2(sy, cy),
                atan2(2 * (r.z * r.s - r.x * r.y), ys + xz)
            };
        }

        if(sy > 0) {
            return Vector{
                0.,
                half_pi,
                2. * atan2(r.z + r.x, r.s + r.y)
            };
        }
        return Vector{
            0.,
            half_pi * -1.,
            atan2(r.z - r.x, r.s - r.y)
        };
    }

    inline double mag(Quaternion q) {
        return sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.s * q.s);
    }

    inline Quaternion normalize(Quaternion q) {

        double m = mag(q);
        if(m < FP_MAG_THRESHOLD) {
            return Quaternion{0., 0., 0., 1.};
        }
        if(fabs(1. - m) > ONE_PART_IN_A_MILLION) {
            double oomag = 1. / m;
            return Quaternion{
                q.x * oomag,
                q.y * oomag,
                q.z * oomag,
                q.s * oomag
            };
        }
        return q;
    }

    inline bool operator<(Quaternion l, Quaternion r) {
        return mag(l) < mag(r);
    }
}
}

#endif //GUARD_LSL_RUNTIME_QUATERNION_HH_INCLUDED


/* -*- c++ -*- */
/*
 * Copyright 2006,2010-2012 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "costas_loop_cc_impl.h"
#include <gnuradio/io_signature.h>
#include <gnuradio/expj.h>
#include <gnuradio/sincos.h>
#include <gnuradio/math.h>
#include <boost/format.hpp>

namespace gr {
  namespace digital {

    costas_loop_cc::sptr
    costas_loop_cc::make(float loop_bw, int order, bool use_snr)
    {
      return gnuradio::get_initial_sptr
	(new costas_loop_cc_impl(loop_bw, order, use_snr));
    }

    static int ios[] = { sizeof(gr_complex), sizeof(float), sizeof(float), sizeof(float) };
    static std::vector<int> iosig(ios, ios+sizeof(ios)/sizeof(int));

    costas_loop_cc_impl::costas_loop_cc_impl(float loop_bw, int order, bool use_snr)
      : sync_block("costas_loop_cc",
                   io_signature::make(1, 1, sizeof(gr_complex)),
                   io_signature::makev(1, 4, iosig)),
	blocks::control_loop(loop_bw, 1.0, -1.0),
	d_order(order), d_error(0), d_noise(1.0), d_phase_detector(NULL)
    {
      // Set up the phase detector to use based on the constellation order
      switch(d_order) {
      case 2:
        if(use_snr)
          d_phase_detector = &costas_loop_cc_impl::phase_detector_snr_2;
	else
          d_phase_detector = &costas_loop_cc_impl::phase_detector_2;
	break;

      case 4:
        if(use_snr)
          d_phase_detector = &costas_loop_cc_impl::phase_detector_snr_4;
	else
          d_phase_detector = &costas_loop_cc_impl::phase_detector_4;
	break;

      case 8:
        if(use_snr)
          d_phase_detector = &costas_loop_cc_impl::phase_detector_snr_8;
	else
          d_phase_detector = &costas_loop_cc_impl::phase_detector_8;
	break;

      default:
	throw std::invalid_argument("order must be 2, 4, or 8");
	break;
      }

      message_port_register_in(pmt::mp("noise"));
      set_msg_handler(
        pmt::mp("noise"),
        boost::bind(&costas_loop_cc_impl::handle_set_noise,
                    this, _1));
    }

    costas_loop_cc_impl::~costas_loop_cc_impl()
    {
    }

    float
    costas_loop_cc_impl::phase_detector_8(gr_complex sample) const
    {
      /* This technique splits the 8PSK constellation into 2 squashed
	 QPSK constellations, one when I is larger than Q and one
	 where Q is larger than I. The error is then calculated
	 proportionally to these squashed constellations by the const
	 K = sqrt(2)-1.

	 The signal magnitude must be > 1 or K will incorrectly bias
	 the error value.

	 Ref: Z. Huang, Z. Yi, M. Zhang, K. Wang, "8PSK demodulation for
	 new generation DVB-S2", IEEE Proc. Int. Conf. Communications,
	 Circuits and Systems, Vol. 2, pp. 1447 - 1450, 2004.
      */

      float K = (sqrt(2.0) - 1);
      if(fabsf(sample.real()) >= fabsf(sample.imag())) {
	return ((sample.real()>0 ? 1.0 : -1.0) * sample.imag() -
		(sample.imag()>0 ? 1.0 : -1.0) * sample.real() * K);
      }
      else {
	return ((sample.real()>0 ? 1.0 : -1.0) * sample.imag() * K -
		(sample.imag()>0 ? 1.0 : -1.0) * sample.real());
      }
    }

    float
    costas_loop_cc_impl::phase_detector_4(gr_complex sample) const
    {
      return ((sample.real()>0 ? 1.0 : -1.0) * sample.imag() -
	      (sample.imag()>0 ? 1.0 : -1.0) * sample.real());
    }

    float
    costas_loop_cc_impl::phase_detector_2(gr_complex sample) const
    {
      return (sample.real()*sample.imag());
    }

    float
    costas_loop_cc_impl::phase_detector_snr_8(gr_complex sample) const
    {
      float K = (sqrt(2.0) - 1);
      float snr = abs(sample)*abs(sample) / d_noise;
      if(fabsf(sample.real()) >= fabsf(sample.imag())) {
	return ((blocks::tanhf_lut(snr*sample.real()) * sample.imag()) -
          (blocks::tanhf_lut(snr*sample.imag()) * sample.real() * K));
      }
      else {
	return ((blocks::tanhf_lut(snr*sample.real()) * sample.imag() * K) -
          (blocks::tanhf_lut(snr*sample.imag()) * sample.real()));
      }
    }

    float
    costas_loop_cc_impl::phase_detector_snr_4(gr_complex sample) const
    {
      float snr = abs(sample)*abs(sample) / d_noise;
      return ((blocks::tanhf_lut(snr*sample.real()) * sample.imag()) -
              (blocks::tanhf_lut(snr*sample.imag()) * sample.real()));
    }

    float
    costas_loop_cc_impl::phase_detector_snr_2(gr_complex sample) const
    {
      float snr = abs(sample)*abs(sample) / d_noise;
      return blocks::tanhf_lut(snr*sample.real()) * sample.imag();
    }

    float
    costas_loop_cc_impl::error() const
    {
      return d_error;
    }

    void
    costas_loop_cc_impl::handle_set_noise(pmt::pmt_t msg)
    {
      if(pmt::is_real(msg)) {
        d_noise = pmt::to_double(msg);
        d_noise = powf(10.0f, d_noise/10.0f);
      }
    }

    int
    costas_loop_cc_impl::work(int noutput_items,
			      gr_vector_const_void_star &input_items,
			      gr_vector_void_star &output_items)
    {
      const gr_complex *iptr = (gr_complex *) input_items[0];
      gr_complex *optr = (gr_complex *) output_items[0];
      float *freq_optr  = output_items.size() >= 2 ? (float *) output_items[1] : NULL;
      float *phase_optr = output_items.size() >= 3 ? (float *) output_items[2] : NULL;
      float *error_optr = output_items.size() >= 4 ? (float *) output_items[3] : NULL;

      gr_complex nco_out;

      std::vector<tag_t> tags;
      get_tags_in_range(tags, 0, nitems_read(0),
                        nitems_read(0)+noutput_items,
                        pmt::intern("phase_est"));

      for(int i = 0; i < noutput_items; i++) {
        if(tags.size() > 0) {
          if(tags[0].offset-nitems_read(0) == (size_t)i) {
            d_phase = (float)pmt::to_double(tags[0].value);
            tags.erase(tags.begin());
          }
        }

        nco_out = gr_expj(-d_phase);
        optr[i] = iptr[i] * nco_out;

        d_error = (*this.*d_phase_detector)(optr[i]);
        d_error = gr::branchless_clip(d_error, 1.0);

        advance_loop(d_error);
        phase_wrap();
        frequency_limit();

        if (freq_optr != NULL)
          freq_optr[i] = d_freq;
        if (phase_optr != NULL)
          phase_optr[i] = d_phase;
        if (error_optr != NULL)
          error_optr[i] = d_error;
      }

      return noutput_items;
    }

    void
    costas_loop_cc_impl::setup_rpc()
    {
#ifdef GR_CTRLPORT
      // Getters
      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<costas_loop_cc, float>(
	      alias(), "error",
	      &costas_loop_cc::error,
	      pmt::mp(-2.0f), pmt::mp(2.0f), pmt::mp(0.0f),
	      "", "Error signal of loop", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<control_loop, float>(
	      alias(), "frequency",
	      &control_loop::get_frequency,
	      pmt::mp(0.0f), pmt::mp(2.0f), pmt::mp(0.0f),
	      "", "Frequency Est.", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<control_loop, float>(
	      alias(), "phase",
	      &control_loop::get_phase,
	      pmt::mp(0.0f), pmt::mp(2.0f), pmt::mp(0.0f),
	      "", "Phase Est.", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<control_loop, float>(
	      alias(), "loop_bw",
	      &control_loop::get_loop_bandwidth,
	      pmt::mp(0.0f), pmt::mp(2.0f), pmt::mp(0.0f),
	      "", "Loop bandwidth", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      // Setters
      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_set<control_loop, float>(
	      alias(), "loop_bw",
	      &control_loop::set_loop_bandwidth,
	      pmt::mp(0.0f), pmt::mp(1.0f), pmt::mp(0.0f),
	      "", "Loop bandwidth",
	      RPC_PRIVLVL_MIN, DISPNULL)));
#endif /* GR_CTRLPORT */
    }

  } /* namespace digital */
} /* namespace gr */


#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
//#include <boost/math/special_functions/binomial.hpp>
#include <cmath>
#include <Eigen/Dense>
#include <limits>
#include <vector>

#include <fstream>

#include "kalman_filter.hpp"
#include "kalman_filter_cache.hpp"
#include "model.hpp"
#include "partition.hpp"
#include "io.hpp"

//using boost::math::binomial_coefficient;

namespace biggles
{

namespace model
{

namespace detail
{

// log multivariate gamma function with p == 2
inline float lgamma_2(float x)
{
    // see http://en.wikipedia.org/wiki/Multivariate_gamma_function
    return 0.5f*logf(M_PI) + lgammaf(x) + lgammaf(x-0.5f);
}

// log of the inverse Wishart PDF.
inline float log_inverse_wishart_pdf(const Eigen::Matrix2f& B, const Eigen::Matrix2f& Phi, int m)
{
    // see http://en.wikipedia.org/wiki/Inverse-Wishart_distribution

    float log_det_B = logf(B.determinant());
    float log_det_Phi = logf(Phi.determinant());
    float trace_Phi_B_inv = (Phi * B.inverse()).trace();

    return 0.5f*m*log_det_Phi - 0.5f*(m+2+1)*log_det_B - 0.5f*trace_Phi_B_inv - 0.5f*(2*m)*logf(2.f) - lgamma_2(0.5f*m);
}

}

/// \brief calculates p(partition | clutter rate)
///
/// This function seems to be uncontroversial
float log_partition_given_clutter_rate_density(const partition& part_sample, const model::parameters& para_sample) {
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    /*
    std::vector<size_t> clutter_counts(part_duration);
    BOOST_FOREACH(const observation& obs, part_sample.clutter()) {
        ++clutter_counts[t(obs) - part_first_ts];
    }
    */
    float lambda_f = clutter_rate(para_sample);
    /*
    if (not std::isfinite(lambda_f)) {
        std::stringstream sstr;
        sstr << "clutter rate is not finite: " << lambda_f;
        throw std::runtime_error(sstr.str()) ;
    }
    */
    BOOST_ASSERT(std::isfinite(lambda_f));
    BOOST_ASSERT(lambda_f >= 0);
    float log_part_clutter = part_sample.clutter().size() * logf(lambda_f) - part_duration * lambda_f;
    for (clutter_t::const_iterator it = part_sample.clutter().begin(); it not_eq part_sample.clutter().end(); ++it) {
        log_part_clutter += -lgammaf(1.f + it->second.size());
    }
    return log_part_clutter;
}

float log_partition_given_survival_prob_density(const partition& part_sample, const model::parameters& para_sample) {
    binomial_coefficient& binom_coeff = binomial_coefficient::get();
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    const int min_surv = 0; // number of guaranteed survivals
    std::vector<size_t> survival_events(part_duration);
    std::vector<size_t> death_events(part_duration);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        BOOST_ASSERT(t_ptr->duration() > min_surv);
        time_stamp track_first_ts = t_ptr->first_time_stamp();
        time_stamp track_last_ts = t_ptr->last_time_stamp();
        for (time_stamp ts = track_first_ts - part_first_ts + 1 + min_surv; ts < track_last_ts - part_first_ts; ++ts)
            survival_events[ts]++;
        if (track_last_ts < part_last_ts)
            death_events[track_last_ts - part_first_ts]++;
    }
    float log_part_surv = 0.f;
    float log_p_s = logf(survival_probability(para_sample));
    float log_1m_p_s = logf(1.f - survival_probability(para_sample));
    for ( time_stamp i = 0; i < part_duration; ++i) {
        log_part_surv += logf(binom_coeff(survival_events[i] + death_events[i], survival_events[i])) +
            survival_events[i] * log_p_s + death_events[i] * log_1m_p_s;
    }
    return log_part_surv;
}

float log_partition_given_observation_prob_density(const partition& part_sample, const model::parameters& para_sample) {
    binomial_coefficient& binom_coeff = binomial_coefficient::get();
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    const size_t min_obs = 0; //number of guaranteed observations
    std::vector<size_t> track_events(part_duration);
    std::vector<size_t> observation_events(part_duration);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        time_stamp track_first_ts = t_ptr->first_time_stamp();
        time_stamp track_last_ts = t_ptr->last_time_stamp();
        for (time_stamp ts = track_first_ts - part_first_ts; ts < track_last_ts - part_first_ts; ++ts)
            track_events[ts]++;
        BOOST_ASSERT(t_ptr->observations().size() >= min_obs);
        observation_collection::const_iterator obs_iter = t_ptr->observations().begin();
        for (size_t i = 0; i < min_obs; ++i) {
            track_events[t(*obs_iter) - part_first_ts]--;
            obs_iter++;
        }
        for (; obs_iter != t_ptr->observations().end(); ++obs_iter)
            observation_events[t(*obs_iter) - part_first_ts]++;
    }
    float log_part_obs = 0.f;
    float log_p_o = logf(observation_probability(para_sample));
    float log_1m_p_o = logf(1.f - observation_probability(para_sample));
    for (time_stamp i = 0; i < part_duration; ++i) {
        log_part_obs += logf(binom_coeff(track_events[i], observation_events[i])) +
            observation_events[i] * log_p_o + (track_events[i] - observation_events[i]) * log_1m_p_o;
    }
    return log_part_obs;
}

float log_partition_given_birth_rate_density(const partition& part_sample, const model::parameters& para_sample) {
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    const int min_surv = 0; // number of guaranteed survivals
    std::vector<size_t> birth_events(part_duration);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        BOOST_ASSERT(t_ptr->duration() > min_surv);
        time_stamp track_first_ts = t_ptr->first_time_stamp();
        birth_events[track_first_ts - part_first_ts]++;
    }
    float lambda_b = birth_rate(para_sample);
    size_t n_total_born(part_sample.tracks().size());
    float log_part_birth = n_total_born * logf(lambda_b) - (part_duration - min_surv) * lambda_b;
    // TODO: the value for lgammaf may be stored and recalled rather than recalculated; could be faster.
    for (time_stamp i = 0; i < part_duration; ++i)
        log_part_birth += -lgammaf(1.f + birth_events[i]);
    return log_part_birth;
}

float log_partition_given_parameters_density_orig(const partition& partition, const model::parameters& parameters)
{
    float log_pdf = 0.f;

    off_t n_frames = partition.duration();

    // ensemble statistics over all time stamps
    off_t n_total_born(partition.tracks().size());
    off_t n_total_false(partition.clutter().size());
    off_t n_total_died(partition.tracks().size()); // everything that has a beginning has an end, Neo.
    off_t n_total_survived(0);
    off_t n_total_generated(0);

    // calculate n_total_survived and n_total_generated. record birth times for tracks
    std::vector<size_t> birth_counts(partition.last_time_stamp() - partition.first_time_stamp());
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, partition.tracks())
    {
        BOOST_ASSERT(t_ptr->duration() > 1);
        // a track 'survives' for one fewer ticks than it's duration
        n_total_survived += t_ptr->duration() - 1;

        // the track's size is the number of observations within it
        n_total_generated += t_ptr->size();

        // record birth time
        BOOST_ASSERT(t_ptr->first_time_stamp() >= partition.first_time_stamp());
        BOOST_ASSERT(t_ptr->first_time_stamp() - partition.first_time_stamp() < static_cast<time_stamp>(birth_counts.size()));
        ++birth_counts[t_ptr->first_time_stamp() - partition.first_time_stamp()];
    }

    // record clutter counts
    std::vector<size_t> clutter_counts(partition.last_time_stamp() - partition.first_time_stamp());
    for (clutter_t::const_iterator it = partition.clutter().begin(); it not_eq partition.clutter().end(); ++it) {
        clutter_counts[it->first - partition.first_time_stamp()] = it->second.size();
    }

    // p_s term
    float p_s = frame_to_frame_survival_probability(parameters);
    BOOST_ASSERT(std::isfinite(p_s) and p_s >= 0);
    log_pdf += n_total_survived * logf(p_s) + n_total_died * logf(1.f - p_s);

    // p_d term
    float p_d = generate_observation_probability(parameters);
    BOOST_ASSERT(std::isfinite(p_d) and p_d >= 0);
    log_pdf += n_total_generated * logf(p_d) + (n_total_survived + n_total_born - n_total_generated) * logf(1.f - p_d);

    // start of lambda_b term
    float lambda_b = mean_new_tracks_per_frame(parameters);
    BOOST_ASSERT(std::isfinite(lambda_b) and lambda_b >= 0);
    log_pdf += n_total_born * logf(lambda_b) - n_frames * lambda_b; // ... still to calculate log gamma terms

    // start of lambda_f term
    float lambda_f = mean_false_observations_per_frame(parameters);
    if (not std::isfinite(lambda_f)) {
        std::stringstream sstr;
        sstr << "clutter rate is not finite: " << lambda_f;
        throw std::runtime_error(sstr.str()) ; // TODO debug cleaning up
    }
    BOOST_ASSERT(std::isfinite(lambda_f));
    BOOST_ASSERT(lambda_f >= 0);
    log_pdf += n_total_false * logf(lambda_f) - n_frames * lambda_f; // ... still to calculate log gamma terms

    // finish lambda_b and lambda_f terms
    for(time_stamp ts = partition.first_time_stamp(); ts < partition.last_time_stamp(); ++ts)
    {
        // finish lambda_b term
        log_pdf += -lgammaf(1.f + birth_counts[ts - partition.first_time_stamp()]);

        // finish lambda_f term
        log_pdf += -lgammaf(1.f + clutter_counts[ts - partition.first_time_stamp()]);
    }

    return log_pdf; // the rest is just a repeat of P(T|clutter_rate)

}

float log_partition_given_parameters_density(
        const partition& partition_sample, const model::parameters& parameter_sample)
{
    return log_partition_given_parameters_density_orig(partition_sample, parameter_sample);
}

float log_clutter_given_parameters_density(const partition& part, const model::parameters& parameters) {
    return -logf(part.volume())*part.clutter().size();
}

float log_track_given_parameters_density(const boost::shared_ptr<const track>& track_p, const model::parameters& parameters)
{
    return track_p->log_posterior(parameters);
}

float log_parameters_prior_density(const model::parameters& parameters)
{
    // finite regions of support:
    if(mean_new_tracks_per_frame(parameters) <= 0.f)
        return -std::numeric_limits<float>::max();
    if(mean_false_observations_per_frame(parameters) <= 0.f)
        return -std::numeric_limits<float>::max();
    if(frame_to_frame_survival_probability(parameters) < 0.f)
        return -std::numeric_limits<float>::max();
    if(frame_to_frame_survival_probability(parameters) > 1.f)
        return -std::numeric_limits<float>::max();
    if(generate_observation_probability(parameters) < 0.f)
        return -std::numeric_limits<float>::max();
    if(generate_observation_probability(parameters) > 1.f)
        return -std::numeric_limits<float>::max();

    /*
    if (mean_false_observations_per_frame(parameters) != 0.1f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    if (mean_new_tracks_per_frame(parameters) != .5f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    if (generate_observation_probability(parameters) != .9f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    if (frame_to_frame_survival_probability(parameters) != .9f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    */
    // prior on R:
    // FIXME PRIOR calculation
    /*
    if ( observation_error_covariance(parameters) == Eigen::Matrix2f::Identity()* 0.09f ) {
        return 0.f;
    } else {
        return -std::numeric_limits<float>::max();
    }
    */
    return detail::log_inverse_wishart_pdf(observation_error_covariance(parameters), 2.f * Eigen::Matrix2f::Identity(), 5);
}

float log_likelihood(const partition& part_sample, const model::parameters& para_sample) {
    float log_pdf(0.f);

    // track PDFs
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks())
    {
        log_pdf += log_track_given_parameters_density(t_ptr, para_sample);
    }

    // clutter PDF
    log_pdf += log_clutter_given_parameters_density(part_sample, para_sample);
    return log_pdf;
}

float log_tracks_given_parameters_density(const partition& part, const model::parameters& parameters) {
    // track PDFs
    float log_pdf(0.f);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part.tracks())
    {
        float track_log_pdf = log_track_given_parameters_density(t_ptr, parameters);
        if (not std::isfinite(track_log_pdf)) {
            std::cerr << "track duration " << t_ptr->duration() << std::endl;
            std::cerr << "track size " << t_ptr->size() << std::endl;
        }
        log_pdf += track_log_pdf;
    }
    return log_pdf;
}

void dump_part_para(const partition& part, const model::parameters& para) {
    size_t ctr = 0;
    std::string fname = boost::str(boost::format("no_finite_log_pdf.%04d.json") % ctr);
    while (boost::filesystem::exists(fname)) {
        ctr++;
        if (ctr > 9999) {
            std::cerr << "nothing dumped :(" << std::endl;
            return;
        }
        fname = boost::str(boost::format("no_finite_log_pdf.%04d.json") % ctr);
    }
    std::ofstream fh(fname.c_str());
    fh << "{ \"parameters\": ";
    io::write_model_parameters_to_json_stream(fh, para);
    fh << ", \"partition\": " << std::endl;
    io::write_partition_to_json_stream(fh, part);
    fh << "}" << std::endl;
}

float log_partition_given_parameters_and_data_density(const partition& part, const model::parameters& parameters)
{
    float log_pdf(0.f);
    float temp(0.f);

    // clutter PDF
    temp = log_clutter_given_parameters_density(part, parameters);
    if (not std::isfinite(temp)) {
        std::cerr << " *** clutter log density not finite" << std::endl;
        std::cerr << "     " << part.volume() << std::endl;
        dump_part_para(part, parameters);
    }
    log_pdf += temp;

    temp = log_tracks_given_parameters_density(part, parameters);
    if (not std::isfinite(temp)) {
        std::cerr << " *** track log density not finite" << std::endl;
        std::cerr << temp << std::endl;
        std::cerr << model::observation_error_covariance(parameters) << std::endl;
        dump_part_para(part, parameters);
    }
    log_pdf += temp;

    //log_pdf = 0.f; // FIXME PRIOR test get rid of this line again

    // data-independent track PDF
    temp = log_partition_given_parameters_density(part, parameters);
    if (not std::isfinite(temp)) {
        std::cerr << " *** partition log density not finite" << std::endl;
        dump_part_para(part, parameters);
    }
    log_pdf += temp;

    // prior
    // the prior is only need to record the log pdf and not for the sampling itself.
    // log_pdf += log_parameters_prior_density(parameters);

    return log_pdf;
}

}

}


// Copyright (c) 2015 Zachary Kann
//
// 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.

// ---
// Author: Zachary Kann

#include <fstream>
#include <cmath>
#include <complex>
#include <armadillo>
#include "boost/program_options.hpp"
#include "z_constants.hpp"
#include "z_conversions.hpp"
#include "z_string.hpp"

namespace po = boost::program_options;

int main (int argc, char *argv[]) {

  enum Chromophore {kOH, kOD};

  std::string V_filename;
  po::options_description desc("Options");
  desc.add_options()
    ("help,h",  "Print help messages")
    ("V_file,c", po::value<std::string>(&V_filename),
     "Choice of chromophore (OH or OD)")
    ("chromophore,c", po::value<std::string>(),
     "Choice of chromophore (OH or OD)");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    exit(EXIT_SUCCESS);
  }

  Chromophore chromophore;
  const std::string& vm_chromophore =
      vm["chromophore"].as<std::string>();
  if (vm_chromophore == "OH")
    chromophore = kOH;
  else if (vm_chromophore == "OD")
    chromophore = kOD;
  else
    assert(false && "Unrecognized chromophore option");

  double red_mass = (chromophore == kOH) ? MASS_H*MASS_OD : MASS_D*MASS_OH;
  red_mass *= AMU_TO_KG/MASS_HOD;
  double delta_x = 0.02e-10;
  int points = 56;

  arma::mat T = arma::zeros<arma::mat>(points, points);
  arma::mat V = arma::zeros<arma::mat>(points, points);
  arma::mat H = arma::zeros<arma::mat>(points, points);

  std::ifstream V_file(V_filename.c_str());

  for (int i=0; i<points; i++)
    V_file >> V(i,i);

  for (int i_col = 0; i_col < points; ++i_col) {
    for (int i_row = 0; i_row < points; ++i_row) {
      T(i_row, i_col) =
          H_BAR*H_BAR/2.0/red_mass/delta_x/delta_x/C_SPEED_CGS/PLANCK;
      V(i_row, i_col) *= 220000;
      if ((i_col-i_row)%2)
        T(i_row, i_col) *= -1.0;
      if (i_row == i_col)
        T(i_row, i_col) *= M_PI*M_PI/3.0;
      else
        T(i_row, i_col) *= 2.0/(i_col-i_row)/(i_col-i_row);
    }
  }
  H = T+V;
  arma::rowvec eigenvalues = arma::eig_sym(H);

  std::ofstream output_file("freqs.dat");
  output_file << eigenvalues(1) - eigenvalues(0);
}