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

  @copyright 2016 NumScale SAS

  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_ARCH_COMMON_SCALAR_FUNCTION_LOG1P_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_LOG1P_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/detail/enforce_precision.hpp>
#include <boost/simd/function/log.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      detail::enforce_precision<A0> enforcer;

      if (Mone<A0>() > a0)   return Nan<A0>();
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (a0 == Inf<A0>())   return Inf<A0>();
      #endif
      if (a0 == Mone<A0>())   return Minf<A0>();
      A0 u = inc(a0);
      return log(u)+(a0-dec(u))/u;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      return std::log1p(a0);
    }
  };
} } }

#endif


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

  @copyright 2016 NumScale SAS

  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_ARCH_COMMON_SIMD_FUNCTION_AVERAGE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_AVERAGE_HPP_INCLUDED

#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD_IF ( average_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return bitwise_and(a0, a1)+shift_right(bitwise_xor(a0, a1),1);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( average_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::floating_<A0>, X>
                          , bs::pack_< bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return fma(a0,Half<A0>(),a1*Half<A0>());
    }
  };
} } }

#endif


/*--
    PrimitiveFitUtils.cpp

    This file is part of the Cornucopia curve sketching library.
    Copyright (C) 2010 Ilya Baran (baran37@gmail.com)

    This program 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 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "PrimitiveFitUtils.h"

#include "Line.h"
#include "Arc.h"
#include "Clothoid.h"
#include "Fresnel.h"

#include <Eigen/Eigenvalues>

using namespace std;
using namespace Eigen;
NAMESPACE_Cornu

void LineFitter::addPointW(const Vector2d &pt, double weight)
{
    ++_numPts;
    _totWeight += weight;

    _lastPoint = pt;
    if(_numPts == 1)
        _firstPoint = pt;

    _sum += weight * pt;
    _squaredSum[0] += weight * SQR(pt[0]);
    _squaredSum[1] += weight * SQR(pt[1]);
    _crossSum += weight * pt[0] * pt[1];
}

LinePtr LineFitter::getCurve() const
{
    if(_numPts < 2)
        return LinePtr();

    Matrix2d cov = Matrix2d::Zero();
    double factor = 1. / double(_totWeight);
    cov(0, 0) = _squaredSum[0] - SQR(_sum[0]) * factor;
    cov(1, 1) = _squaredSum[1] - SQR(_sum[1]) * factor;
    cov(0, 1) = cov(1, 0) = _crossSum - _sum[0] * _sum[1] * factor;
    cov *= factor;

    SelfAdjointEigenSolver<Matrix2d> eigenSolver(cov);

    Vector2d eigVs = eigenSolver.eigenvalues();
    Vector2d dir = eigenSolver.eigenvectors().col(1).normalized(); //1 is the index of the larger eigenvalue
    Vector2d pt = _sum * factor;
    Vector2d pt0 = pt + ((_firstPoint - pt).dot(dir)) * dir;
    Vector2d pt1 = pt + ((_lastPoint - pt).dot(dir)) * dir;

    return new Line(pt0, pt1);
}

void ArcFitter::addPointW(const Vector2d &pt, double weight)
{
    _pts.push_back(pt);

    Vector3d pt3(pt[0] - _pts[0][0], pt[1] - _pts[0][1], (pt - _pts[0]).squaredNorm());

    _totWeight += weight;
    _sum += weight * pt3;
    _squaredSum += weight * pt3 * pt3.transpose();
}

ArcPtr ArcFitter::getCurve() const
{
    if((int)_pts.size() < 2)
        return ArcPtr();

    double factor = 1. / _totWeight;
    Vector3d pt = _sum * factor;
    Matrix3d cov = factor * _squaredSum - pt * pt.transpose();

    SelfAdjointEigenSolver<Matrix3d> eigenSolver(cov);
    Vector3d eigVs = eigenSolver.eigenvalues();

    Vector3d dir = eigenSolver.eigenvectors().col(0); //0 is the index of the smallest eigenvalue
    dir /= (1e-16 + dir[2]);

    double dot = dir.dot(pt);
    //circle equation is:
    //dir[0] * x + dir[1] * y + (x^2+y^2) = dot
    Vector2d center = -0.5 * Vector2d(dir[0], dir[1]);
    double radius = sqrt(1e-16 + dot + center.squaredNorm());
    center += _pts[0];

    //TODO: convert code to use AngleUtils
    //Now get the arc
    Vector2d c[3] = { _pts[0], _pts[_pts.size() / 2], _pts.back() };
    double angle[3];
    for(int i = 0; i < 3; ++i) {
        c[i] = (c[i] - center).normalized() * radius;
        angle[i] = atan2(c[i][1], c[i][0]);
    }
    if(angle[2] < angle[0])
        angle[2] += PI * 2.;
    if(angle[1] < angle[0])
        angle[1] += PI * 2.;
    if(angle[1] <= angle[2]) { //OK--CCW arc
        double a = angle[2] - angle[0];
        return new Arc(c[0] + center, angle[0] + PI * 0.5, a * radius, 1. / radius);
    }
    else { //Backwards--CW arc
        for(int i = 0; i < 3; ++i)
            angle[i] = atan2(c[i][1], c[i][0]);
        if(angle[0] <= angle[2])
            angle[0] += PI * 2.;
        if(angle[1] < angle[2])
            angle[1] += PI * 2.;
        assert(angle[1] <= angle[0]);
        double a = angle[0] - angle[2];
        return new Arc(c[0] + center, angle[0] - PI * 0.5, a * radius, -1. / radius);
    }
}

void ClothoidFitter::addPoint(const Vector2d &pt)
{
    _pts.push_back(pt);

    if((int)_pts.size() < 2)
        return;

    const Vector2d &prevPt = _pts[_pts.size() - 2];
    double segmentLength = (pt - prevPt).norm();
    _centerOfMass += (pt + prevPt) * (0.5 * segmentLength);

    double angle = atan2(pt[1] - prevPt[1], pt[0] - prevPt[0]);
    if(angle < _prevAngle) //make sure it's not far from the previous angle
        angle += TWOPI * int(0.5 + (_prevAngle - angle) / TWOPI);
    else
        angle -= TWOPI * int(0.5 + (angle - _prevAngle) / TWOPI);
    _prevAngle = angle;

    double x0 = _totalLength;
    double x1 = (_totalLength += segmentLength);

    double y = angle;
    double z = _angleIntegral - y * x0;
    _rhs += _getRhs(x1, y, z) - _getRhs(x0, y, z);

    _angleIntegral += segmentLength * angle;
}

ClothoidPtr ClothoidFitter::getCurve() const
{
    Matrix4d lhs;

    lhs = _getLhs(_totalLength);

    Vector4d abcd = lhs.inverse() * _rhs;
    return getClothoidWithParams(abcd);
}

ClothoidPtr ClothoidFitter::getCurveWithZeroCurvature(double param) const
{
    Matrix<double, 5, 5> lhs;
    Matrix<double, 5, 1> rhs;

    Vector4d constraint;
    constraint << 6 * param, 2, 0, 0; // second derivative of ax^3+bx^2+cx+d is 6ax+2b

    //For constrained least squares,
    //lhs is now [A^T A    C]
    //           [ C^T     0]
    lhs << _getLhs(_totalLength),   constraint,
           constraint.transpose(),  0;
    rhs << _rhs, 0;

    Vector4d abcd = (lhs.inverse() * rhs).head<4>();
    return getClothoidWithParams(abcd);
}

ClothoidPtr ClothoidFitter::getClothoidWithParams(const Eigen::Vector4d &abcd) const
{
    Vector3d abc = Vector3d(abcd[0] * 3, abcd[1] * 2, abcd[2]);

    double startAngle = abc[2];
    double startCurvature = abc[1];
    double endCurvature = 2 * abc[0] * _totalLength + abc[1];

    //now compute the center of mass of the clothoid at 0
    double x, y;

    if(fabs(abc[0]) > 1e-8) //if it's a real clothoid
    {
        //The following comes from the expression that Mathematica generates with:
        //Integrate[Integrate[{Cos[a x^2 + b x + c], Sin[a x^2 + b x + c]}, {x, 0, t}], {t, 0, s}]/s
        bool negative = false;
        if(abc[0] < 0)
        {
            negative = true;
            abc = -abc;
        }
        double a = abc[0], b = abc[1], c = abc[2];
        double s = _totalLength;

        double invRtApi2 = 1. / sqrt(0.5 * PI * a);

        double f1s, f2s, f1c, f2c;
        fresnelApprox(b * invRtApi2 * 0.5, &f1s, &f1c);
        fresnelApprox((b + 2 * a * s) * invRtApi2 * 0.5, &f2s, &f2c);

        double disc = b * b / (4 * a) - c;
        double sind = sin(disc), cosd = cos(disc);

        y = (cos(c + s * (b + a * s)) - cos(c)) / (2. * a);
        y -= (b + 2 * a * s) * PI * 0.25 * (cosd * (f1s - f2s) + sind * (f2c - f1c)) * invRtApi2 / a;
        if(negative)
            y = -y;

        x = (sin(c) - sin(c + s * (b + a * s))) / (2. * a);
        x += (b + 2 * a * s) * PI * 0.25 * (-sind * (f1s - f2s) + cosd * (f2c - f1c)) * invRtApi2 / a;

        x /= s;
        y /= s;
    }
    else
    {
        double b = abc[1], c = abc[2];
        double s = _totalLength;

        if(fabs(b) < 1e-8) //line
        {
            x = cos(c) * s * 0.5;
            y = sin(c) * s * 0.5;
        }
        else //arc
        {
            double c1 = cos(c), s1 = sin(c);
            double c2 = cos(c + b * s), s2 = sin(c + b * s);

            x = (c1 - c2) / (b * b * s) - s1 / b;
            y = (s1 - s2) / (b * b * s) + c1 / b;
        }
    }

    return new Clothoid(Vector2d(_centerOfMass[0] / _totalLength - x, _centerOfMass[1] / _totalLength - y),
                        startAngle, _totalLength, startCurvature, endCurvature);
}

Matrix4d ClothoidFitter::_getLhs(double x)
{
    double xp[8] = {1, x, 0, 0, 0, 0, 0, 0 }; //powers of totalLength
    for(int i = 2; i < 8; ++i)
        xp[i] = xp[i - 1] * x;

    Matrix4d out;

    for(int i = 0; i < 4; ++i) for(int j = 0; j < 4; ++j)
    {
        int p = 7 - i - j;
        out(i, j) = (2. / double(p)) * xp[p];
    }

    return out;
}

Vector4d ClothoidFitter::_getRhs(double x, double y, double z)
{
    double xp[6] = {1, x, 0, 0, 0, 0 }; //powers of totalLength
    for(int i = 2; i < 6; ++i)
        xp[i] = xp[i - 1] * x;

    Vector4d out;

    out[0] = (2. / 5.) * y * xp[5] + 0.5 * z * xp[4];
    out[1] = 0.5 * y * xp[4] + (2. / 3.) * z * xp[3];
    out[2] = (2. / 3.) * y * xp[3] + z * xp[2];
    out[3] = y * xp[2] + 2. * z * xp[1];

    return out;
}

END_NAMESPACE_Cornu


/*
 * Copyright (c) 2015 Claus Christmann <hcc |ä| gatech.edu>.
 *
 * Licensed under the Apache Lice*nse, 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.
 *
 */

#include "gis.h"

#include <assert.h>
#include <cmath>
#include "units.hpp"
#include "commoncoordinatesystems.h"
#include <boost/concept_check.hpp>

// Anoymous namespace to hide the chosen implementation for ecef2geodetic()
namespace {

using namespace AMG::GIS;

void ecef2geodetic_iterativ(const double& x, const double& y, const double& z, double& lambda, double& phi, double& h)
{
  // NOTE: this function is a replication of ecef2geodetic.m from Matlab
  //
  //   function [phi, lambda, h] = ecef2geodetic(x, y, z, ellipsoid)
  //   %ECEF2GEODETIC Convert geocentric (ECEF) to geodetic coordinates
  //   %
  //   %   [PHI, LAMBDA, H] = ECEF2GEODETIC(X, Y, Z, ELLIPSOID) converts point
  //   %   locations in geocentric Cartesian coordinates, stored in the
  //   %   coordinate arrays X, Y, Z, to geodetic coordinates PHI (geodetic
  //   %   latitude in radians), LAMBDA (longitude in radians), and H (height
  //   %   above the ellipsoid). The geodetic coordinates refer to the
  //   %   reference ellipsoid specified by ELLIPSOID (a row vector with the
  //   %   form [semimajor axis, eccentricity]). X, Y, and Z must use the same
  //   %   units as the semimajor axis;  H will also be expressed in these
  //   %   units.  X, Y, and Z must have the same shape; PHI, LAMBDA, and H
  //   %   will have this shape also.
  //   %
  //   %   For a definition of the geocentric system, also known as
  //   %   Earth-Centered, Earth-Fixed (ECEF), see the help for GEODETIC2ECEF.
  //   %
  //   %   See also ECEF2LV, GEODETIC2ECEF, GEOCENTRIC2GEODETICLAT, LV2ECEF.
  //
  //   % Copyright 2005-2009 The MathWorks, Inc.
  //   % $Revision: 1.1.6.4 $  $Date: 2009/04/15 23:34:43 $
  //
  //   % Reference
  //   % ---------
  //   % Paul R. Wolf and Bon A. Dewitt, "Elements of Photogrammetry with
  //   % Applications in GIS," 3rd Ed., McGraw-Hill, 2000 (Appendix F-3).
  //
  //   % Implementation Notes from Rob Comer
  //   % -----------------------------------
  //   % The implementation below follows Wolf and DeWitt quite literally,
  //   % with a few important exceptions required to ensure good numerical
  //   % behavior:
  //   %
  //   % 1) I used ATAN2 rather than ATAN in the formulas for beta and phi.  This
  //   %    avoids division by zero (or a very small number) for points on (or
  //   %    near) the Z-axis.
  //   %
  //   % 2) Likewise, I used ATAN2 instead of ATAN when computing beta from phi
  //   %    (conversion from geodetic to parametric latitude), ensuring
  //   %    stability even for points at very high latitudes.
  //   %
  //   % 3) Finally, I avoided dividing by cos(phi) -- also problematic at high
  //   %    latitudes -- in the calculation of h, the height above the ellipsoid.
  //   %    Wold and Dewitt give
  //   %
  //   %                   h = sqrt(X^2 + Y^2)/cos(phi) - N.
  //   %
  //   %    The trick is to notice an alternative formula that involves division
  //   %    by sin(phi) instead of cos(phi), then take a linear combination of the
  //   %    two formulas weighted by cos(phi)^2 and sin(phi)^2, respectively. This
  //   %    eliminates all divisions and, because of the identity cos(phi)^2 +
  //   %    sin(phi)^2 = 1 and the fact that both formulas give the same h, the
  //   %    linear combination is also equal to h.
  //   %
  //   %    To obtain the alternative formula, we simply rearrange
  //   %
  //   %                   Z = [N(1 - e^2) + h]sin(phi)
  //   %    into
  //   %                   h = Z/sin(phi) - N(1 - e^2).
  //   %
  //   %    The linear combination is thus
  //   %
  //   %        h = (sqrt(X^2 + Y^2)/cos(phi) - N) cos^2(phi)
  //   %            + (Z/sin(phi) - N(1 - e^2))sin^2(phi)
  //   %
  //   %    which simplifies to
  //   %
  //   %      h = sqrt(X^2 + Y^2)cos(phi) + Zsin(phi) - N(1 - e^2sin^2(phi)).
  //   %
  //   %    From here it's not hard to verify that along the Z-axis we have
  //   %    h = Z - b and in the equatorial plane we have h = sqrt(X^2 + Y^2) - a.

  // % Ellipsoid constants
  // WGS84::a   % Semimajor axis
  // WGS84::e2  % Square of first eccentricity
  // WGS84::ep2 % Square of second eccentricity
  // WGS84::f   % Flattening
  // WGS84::b   % Semiminor axis

  // Longitude
  lambda = std::atan2(y,x);

  // Distance from the z-axis
  double rho = std::hypot(x,y);

  // Bowring's formula for initial parametric (beta) and geodetic (phi) latitudes
  double beta = std::atan2(z, (1 - WGS84::f) * rho);
  phi = std::atan2(z   + WGS84::b*WGS84::ep2 *std::pow(std::sin(beta),3) ,
                   rho - WGS84::a*WGS84::e2  *std::pow(std::cos(beta),3) );

  // Fixed-point iteration with Bowring's formula
  // (typically converges within two or three iterations)
  double betaNew = std::atan2((1 - WGS84::f)*std::sin(phi), std::cos(phi));
  int count = 0;
  while( beta != betaNew && count < 10 )
  {
    beta = betaNew;
    phi = std::atan2(z   + WGS84::b * WGS84::ep2 * std::pow(std::sin(beta),3) ,
                     rho - WGS84::a * WGS84::e2  * std::pow(std::cos(beta),3) );
    betaNew = std::atan2((1 - WGS84::f)*std::sin(phi), std::cos(phi));
    ++count;
  }

  // Calculate ellipsoidal height from the final value for latitude
  double sinphi = std::sin(phi);
  double N = WGS84::a / std::sqrt(1.0 - WGS84::e2 * sinphi*sinphi);
  h = rho * std::cos(phi) + (z + WGS84::e2 * N * sinphi) * sinphi - N;

}

void ecef2geodetic_closedForm(const double& x, const double& y, const double& z, double& lambda, double& phi, double& h)
{
  //NOTE: This method is taken from
  //
  // Markku Heikkinen, "Geschlossene Formeln zur Berechnung räumlicher
  // geodätischer Koordinaten aud rechtwinkeligen Koordinaten", Zeitschrift für
  // Vermessungswesen, Vol. 107, 5/1982, p. 207--2011, in German

  //NOTE: This method is only valid if the radius r3 = sqrt(x*x+y*y+z*z) is
  // larger than ~43 km. (See the above mentioned paper for the related details.)
  assert( std::sqrt(x*x+y*y+z*z) > 45e3 );

  //
  // Ellipsoidal constants
  //
  long double a = WGS84::a;
  long double b = WGS84::b;
  long double e2 = WGS84::e2; // pos. definite
  long double ep2 = WGS84::ep2;// pos. definite
  long double E2 = e2*a*a; // alternative E2 = a*a-b*b; // pos. definite

  //
  // intermediaries
  //

  long double r  = std::sqrt( (x*x)+(y*y) );

  if( 0.0L == r ) // on N-S axis.
  {
    lambda = 0.0;

    phi =  static_cast<double>(M_PI_2l);
    if( z < 0 )
    { phi *= -1.0; }

    h = std::abs(z)-b;
    return;
  }

  long double F  = 54.0L*(b*b)*(z*z); // pos. definite
  long double G  = (r*r)+(1.0L-e2)*(z*z)-e2*E2; // pos. definite
  long double c  = (e2*e2)*F*(r*r)/(G*G*G)  ;
  long double s  = std::pow( 1.0L+c+std::sqrt((c*c)+2.0L*c) ,(1.0L/3.0L)); // 3rd root
  long double P  = F/( 3.0L*(1.0L+s+1.0L/s)*(1.0L+s+1.0L/s)*(G*G) );
  long double Q  = std::sqrt( 1.0L+2.0L*(e2*e2)*P );
  long double r0 = ((P*e2*r)/(1.0L+Q))
    +std::sqrt( 0.5L*(a*a)*(1.0L+1.0L/Q)-(P*(1.0L-e2)*(z*z))/(Q*(1.0L+Q))-0.5L*P*(r*r) );
  long double U  = std::sqrt( (z*z)+(r-e2*r0)*(r-e2*r0) );
  long double V  = std::sqrt( (r-e2*r0)*(r-e2*r0)+(1.0L-e2)*(z*z) );
  long double z0 = ((b*b)*z)/(a*V);

  //NOTE: I introduced these 3 variables in order to save some typing
  long double W  = z+ep2*z0;
  long double sinphi = W/std::sqrt( (r*r)+(W*W) );
  long double cosphi = r/std::sqrt( (r*r)+(W*W) );

  //
  // final computations
  //
  h = double( U*(1.0L-(b*b)/(a*V)) );

  //NOTE: as both, sinphi and cosphi are given in the algorithm, I decided to
  // invert the lower value, hoping that the higher slope in that region gives
  // a better numerical result. Obviously, this is just a gutt feel, no real
  // scienece involved...
  if( sinphi<=cosphi )
  {
    phi = double(std::asin(sinphi));
  }
  else
  {
    phi = double(std::acos(cosphi));
  }

  //NOTE: same argument as above... Claus
  if( x<=y )
  {
    lambda = double(std::acos(x/r));
  }
  else
  {
    lambda = double(std::asin(y/r));
  }

}

}

namespace AMG { namespace GIS {

std::string convertToString ( const CompassPoint& cp )
{
  using CP=AMG::GIS::CompassPoint;
  switch( cp )
  {
    case CP::N    : return "N"   ; //"North";
    case CP::NbE  : return "NbE" ; //"North by East";
    case CP::NNE  : return "NNE" ; //"North-northeast";
    case CP::NEbN : return "NEbN"; //"Northeast by North";
    case CP::NE   : return "NE"  ; //"Northeast";
    case CP::NEbE : return "NEbE"; //"Northeast by East";
    case CP::ENE  : return "ENE" ; //"East-northeast";
    case CP::EbN  : return "EbN" ; //"East by North";
    case CP::E    : return "E"   ; //"East";
    case CP::EbS  : return "EbS" ; //"South";
    case CP::ESE  : return "ESE" ; //"East-southeast";
    case CP::SEbE : return "SEbE"; //"Southeast by East";
    case CP::SE   : return "SE"  ; //"Southeast";
    case CP::SEbS : return "SEbS"; //"Southeast by South";
    case CP::SSE  : return "SSE" ; //"South-southeast";
    case CP::SbE  : return "SbE" ; //"South by East";
    case CP::S    : return "S"   ; //"South";
    case CP::SbW  : return "SbW" ; //"South by West";
    case CP::SSW  : return "SSW" ; //"South-southwest";
    case CP::SWbS : return "SWbS"; //"Southwest by South";
    case CP::SW   : return "SW"  ; //"Southwest";
    case CP::SWbW : return "SWbW"; //"Southwest by West";
    case CP::WSW  : return "WSW" ; //"West-southwest";
    case CP::WbS  : return "WbS" ; //"West by South";
    case CP::W    : return "W"   ; //"West";
    case CP::WbN  : return "WbN" ; //"West by North";
    case CP::WNW  : return "WNW" ; //"West-northwest";
    case CP::NWbW : return "NWbW"; //"Northwest by West";
    case CP::NW   : return "NW"  ; //"Northwest";
    case CP::NWbN : return "NWbN"; //"Northwest by North";
    case CP::NNW  : return "NNW" ; //"North-norhtwest";
    case CP::NbW  : return "NbW" ; //"North by West";

    default: return std::string();
  }
  return std::string();
}

double ensureHeading ( const double& heading_deg )
{
 // check for circular multiplicity, i.e. reduce heading to within one circle
 double hdg_deg = std::fmod(heading_deg,360.0);

 if( hdg_deg > 180.0 )
 {hdg_deg -=360.0;}
 else if( hdg_deg <= -180.0 )
 {hdg_deg += 360; }

 return hdg_deg;
}

double opposingHeading ( const double& heading_deg )
{
  return ensureHeading(heading_deg+180.0);
}

double compassPointToHeading ( CompassPoint const & cp )
{
  double center_deg = static_cast<double>(cp)/100;
  return ensureHeading(center_deg);
}

bool isInLeftHalfCircle ( const double& reference_deg, const double& heading_deg )
{
  double ref_deg  = ensureHeading(reference_deg );
  double test_deg = ensureHeading(heading_deg);

  //NOTE: remember: left  is [ref-180,ref), i.e. the opposite heading is in
  // both, the left and the right half circle!

  if( ref_deg == test_deg )
  { return false; }

  if( ref_deg == 0.0 )
  { return test_deg < 0  or test_deg == 180.0; }

  if( ref_deg > 0.0 )
  { return  (ref_deg-180.0)<=test_deg && test_deg < ref_deg; }

  if( ref_deg < 0.0 )
  { return !( ref_deg<test_deg && test_deg <(ref_deg+180.0) ) ; }
  //NOTE: ---------------------------------^
  // This is not a "<=" as we defined left as [ref-180,ref) !

  if( test_deg == opposingHeading (ref_deg) )
  { return true; }

  // catch all that shouldn't be reached.
  return false;
}

bool isInRightHalfCircle ( const double& reference_deg, const double& heading_deg )
{
  double ref_deg  = ensureHeading(reference_deg );
  double test_deg = ensureHeading(heading_deg);

  //NOTE: remember: right  is (ref,ref+180], i.e. the opposite heading is in
  // both, the left and the right half circle!

  if( ref_deg == test_deg )
  { return false; }

  if( ref_deg == 0.0 )
  { return test_deg > 0  or test_deg == 180.0; }

  if( ref_deg > 0.0 )
  { return  !( (ref_deg-180.0)<test_deg && test_deg < ref_deg ); }
  //NOTE: --------------------^
  // This is not a "<=" as we defined right as (ref,ref+180] !

  if( ref_deg < 0.0 )
  { return ref_deg<test_deg && test_deg <=(ref_deg+180.0) ; }

  if( test_deg == opposingHeading (ref_deg) )
  { return true; }

  // catch all that shouldn't be reached.
  return false;
}

CompassPoint headingToCompassPoint ( const double& heading_deg, int resolution )
{
  if( resolution != 4 or resolution != 8 /*or resolution != 16*/ )
  { resolution = 8; }

  double hdg_deg = ensureHeading(heading_deg);

  using CP=AMG::GIS::CompassPoint;
  CP cp = CP::N;

  auto inbetween = [hdg_deg](CP left,CP right)
  { return compassPointToHeading(left)<hdg_deg && hdg_deg <= compassPointToHeading(right); };

  if( inbetween(CP::SW, CP ::NW ) )
  { cp = CP::W;

    if( resolution == 4 ){ /*break;*/ }
    else if( inbetween(CP::SSW, CP::WSW) )
    { cp = CP::SW; }
    else if( inbetween(CP::WNW,CP::NNW) )
    { cp = CP::NW; }

  }
  else if( inbetween(CP::NW, CP::NE) )
  { cp = CP::N;

    if( resolution == 4 ){ /*break;*/ }
    else if( inbetween(CP::WNW,CP::NNW) )
    { cp = CP::NW; }
    else if( inbetween(CP::NNE,CP::ENE) )
    { cp = CP::NE; }

  }
  else if( inbetween(CP::NE,CP::SE) )
  { cp = CP::E;

    if( resolution == 4 ){ /*break;*/ }
    else if( inbetween(CP::NNE,CP::ENE) )
    { cp = CP::NE; }
    else if( inbetween(CP::ESE,CP::SSE) )
    { cp = CP::SE; }

  }
  else
  { cp = CP::S;

    if( resolution == 4 ){ /*break;*/ }
    else if( inbetween(CP::ESE,CP::SSE) )
    { cp = CP::SE; }
    else if( inbetween(CP::SSW,CP::WSW) )
    { cp = CP::SW; }

  }

  return cp;
}

std::string geodeticPositionString ( const AMG::Vector& position )
{
  CoordinateTupel ecefCoords = position.absoluteCoordsIn( AMG::CoSy::getECEF() );

  double lat_rad,lon_rad,alt_m;
  GIS::ecef2geodetic(ecefCoords[0],ecefCoords[1],ecefCoords[2],
                          lon_rad,lat_rad,alt_m );

  CompassPoint northSouth = CompassPoint::N;
  if ( lat_rad < 0)
  { // Southern hemisphere
    northSouth = CompassPoint::S;
          lat_rad *= -1.0;
  }

  CompassPoint eastWest = CompassPoint::E;
  if ( lon_rad < 0)
  { // Western hemisphere
    eastWest = CompassPoint::W;
          lon_rad *= -1.0;
  }

  std::ostringstream stream;
  stream << AMG::Units::radian2degree( lat_rad ) << "°" << convertToString(northSouth) << ", "
         << AMG::Units::radian2degree( lon_rad ) << "°" << convertToString(eastWest)   << ", "
         << alt_m << " m WGS84";
  return stream.str();
}

void ecef2geodetic(const double& x, const double& y, const double& z, double& lambda, double& phi, double& h)
{
  ecef2geodetic_iterativ(x,y,z,lambda,phi,h);
//   ecef2geodetic_closedForm(x,y,z,lambda,phi,h); // forward call to closed form algorithm.
}

void geodetic2ecef(const double& lambda, const double& phi, const double& h, double& x, double& y, double& z)
{
  // NOTE: this function is a replication of ecef2geodetic.m from Matlab
  //
  //   function [x, y, z] = geodetic2ecef(phi, lambda, h, ellipsoid)
  //   %GEODETIC2ECEF Convert geodetic to geocentric (ECEF) coordinates
  //   %
  //   %   [X, Y, Z] = GEODETIC2ECEF(PHI, LAMBDA, H, ELLIPSOID) converts geodetic
  //   %   point locations specified by the coordinate arrays PHI (geodetic
  //   %   latitude in radians), LAMBDA (longitude in radians), and H (ellipsoidal
  //   %   height) to geocentric Cartesian coordinates X, Y, and Z.  The geodetic
  //   %   coordinates refer to the reference ellipsoid specified by ELLIPSOID (a
  //   %   row vector with the form [semimajor axis, eccentricity]).  H must use
  //   %   the same units as the semimajor axis;  X, Y, and Z will be expressed in
  //   %   these units also.
  //   %
  //   %   The geocentric Cartesian coordinate system is fixed with respect to the
  //   %   Earth, with its origin at the center of the ellipsoid and its X-, Y-,
  //   %   and Z-axes intersecting the surface at the following points:
  //   %
  //   %                PHI  LAMBDA
  //   %      X-axis:    0     0      (Equator at the Prime Meridian)
  //   %      Y-axis:    0   pi/2     (Equator at 90-degrees East)
  //   %      Z-axis:  pi/2    0      (North Pole)
  //   %
  //   %   A common synonym is Earth-Centered, Earth-Fixed coordinates, or ECEF.
  //   %
  //   %   See also ECEF2GEODETIC, ECEF2LV, GEODETIC2GEOCENTRICLAT, LV2ECEF.
  //
  //   % Copyright 2004-2009 The MathWorks, Inc.
  //   % $Revision: 1.1.6.4 $  $Date: 2009/04/15 23:34:46 $
  //
  //   % Reference
  //   % ---------
  //   % Paul R. Wolf and Bon A. Dewitt, "Elements of Photogrammetry with
  //   % Applications in GIS," 3rd Ed., McGraw-Hill, 2000 (Appendix F-3).

  // % Ellipsoid constants
  // WGS84::a   % Semimajor axis
  // WGS84::e2  % Square of first eccentricity
  // WGS84::ep2 % Square of second eccentricity
  // WGS84::f   % Flattening
  // WGS84::b   % Semiminor axis

  double sinphi = std::sin(phi);
  double cosphi = std::cos(phi);

  double N  = WGS84::a / std::sqrt(1.0 - WGS84::e2 * sinphi*sinphi);
//   double N  = WGS84::e2 ;
//   N *= sinphi;
//   N *= sinphi;
//   N = 1.0 - N;
//   N = std::sqrt(N);
//   N = WGS84::a / N;

  x = (N + h) * cosphi * std::cos(lambda);
  y = (N + h) * cosphi * std::sin(lambda);
  z = (N*(1 - WGS84::e2) + h) * sinphi;
}

void GudermannFunction(const double& yGudermann, double& phiGeocentric)
{
  phiGeocentric = std::asin( std::tanh( yGudermann ) );
}

void InverseGudermannFunction(const double& phiGeocentric, double& yGudermann)
{
  assert( (-M_PI_2l < phiGeocentric) && (phiGeocentric < M_PI_2l) );
  yGudermann = std::asinh( std::tan( phiGeocentric ) );
}

void geocentric2mercator(const double& lambdaGeocentric,
                                    const double& phiGeocentric,
                                    const double& hGeocentric,
                                          double& xGudermann,
                                          double& yGudermann,
                                          double& zGudermann)
{
  // longitude
  xGudermann = lambdaGeocentric;
  // latitude
  InverseGudermannFunction(phiGeocentric,yGudermann);
  // elevation
  zGudermann = hGeocentric;
}

void mercator2geocentric(const double& xGudermann,
                                    const double& yGudermann,
                                    const double& zGudermann,
                                          double& lambdaGeocentric,
                                          double& phiGeocentric,
                                          double& hGeocentric)
{
  // longitude
  lambdaGeocentric = xGudermann;
  // latitude
  GudermannFunction(yGudermann,phiGeocentric);
  // elevation
  hGeocentric=zGudermann;
}

void cartesian2spherical(const double& x,
                                   const double& y,
                                   const double& z,
                                         double& azimuth,
                                         double& elevation,
                                         double& distance)
{
  double xy = std::hypot(x,y); // length of the projection in the xy-plane
  distance  = std::hypot(z,xy);
  azimuth   = std::atan2(y,x);
  elevation = std::atan2(z,xy);
}

void spherical2cartesian(const double& azimuth,
                                   const double& elevation,
                                   const double& distance,
                                         double& x,
                                         double& y,
                                         double& z)
{
  assert(-M_PIl <= azimuth && azimuth <= M_PIl);
  assert(-M_PI_2l <= elevation && elevation <= M_PI_2l);
  assert( 0.0 <= distance );

  double xy = std::cos(elevation) * distance; // length of the projection in the xy-plane
  z = std::sin(elevation) * distance;
  y = std::sin(azimuth) * xy;
  x = std::cos(azimuth) * xy;
}

void ecef2geocentric(const double& x, const double& y, const double& z, double& lambdaGeocentric, double& phiGeocentric, double& hGeocentric)
{
  // treat it as a regular cartesian2spherical conversion
  double radialDistance(0.0);
  cartesian2spherical(x,y,z,lambdaGeocentric,phiGeocentric,radialDistance);

  // subtract the raddius of the referecen sphere from the distance
  hGeocentric = radialDistance-WGS84::a;
}

void geocentric2ecef(const double& lambdaGeocentric, const double& phiGeocentric, const double& hGeocentric, double& x, double& y, double& z)
{
  double radialDistance = hGeocentric+WGS84::a;
  spherical2cartesian(lambdaGeocentric,phiGeocentric,radialDistance,x,y,z);
}

void geocentric2geodetic(const double& lambdaGeocentric, const double& phiGeocentric, const double& hGeocentric, double& lambda, double& phi, double& h)
{
 double xTemp,yTemp,zTemp;

 geocentric2ecef(lambdaGeocentric,phiGeocentric,hGeocentric,xTemp,yTemp,zTemp);
 ecef2geodetic(xTemp,yTemp,zTemp,lambda,phi,h);
}

void geodetic2geocentric(const double& lambda, const double& phi, const double& h, double& lambdaGeocentric, double& phiGeocentric, double& hGeocentric)
{
  double xTemp,yTemp,zTemp;

  geodetic2ecef(lambda, phi, h, xTemp, yTemp, zTemp);
  ecef2geocentric(xTemp,yTemp,zTemp,lambdaGeocentric,phiGeocentric,hGeocentric);
}

void mercator2ecef(const double& xGudermann, const double& yGudermann, const double& zGudermann, double& x, double& y, double& z)
{
  double lambdaGeocentric, phiGeocentric, hGeocentric;

  mercator2geocentric(xGudermann,yGudermann,zGudermann,lambdaGeocentric,phiGeocentric,hGeocentric);
  geocentric2ecef(lambdaGeocentric,phiGeocentric,hGeocentric,x,y,z);

}

void ecef2mercator(const double& x, const double& y, const double& z, double& xGudermann, double& yGudermann, double& zGudermann)
{
  double lambdaGeocentric, phiGeocentric, hGeocentric;

  ecef2geocentric(x,y,z,lambdaGeocentric,phiGeocentric,hGeocentric);
  geocentric2mercator(lambdaGeocentric,phiGeocentric,hGeocentric,xGudermann,yGudermann,zGudermann);
}

void mercator2geodetic(const double& xGudermann, const double& yGudermann, const double& zGudermann, double& lambda, double& phi, double& h)
{
  double lambdaGeocentric, phiGeocentric, hGeocentric;

  mercator2geocentric(xGudermann,yGudermann,zGudermann,lambdaGeocentric,phiGeocentric,hGeocentric);
  geocentric2geodetic(lambdaGeocentric,phiGeocentric,hGeocentric,lambda,phi,h);

}

void geodetic2mercator(const double& lambda, const double& phi, const double& h, double& xGudermann, double& yGudermann, double& zGudermann)
{
  double lambdaGeocentric, phiGeocentric, hGeocentric;

  geodetic2geocentric(lambda,phi,h,lambdaGeocentric,phiGeocentric,hGeocentric);
  geocentric2mercator(lambdaGeocentric,phiGeocentric,hGeocentric,xGudermann,yGudermann,zGudermann);
}

} // end namespace GIS
} // end namespace AMG

std::ostream& operator<< ( std::ostream& out, const CompassPoint& cp )
{
  return out << AMG::GIS::convertToString(cp);
}


/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include <math.h>
#include <float.h>
#include <iomanip>

#include <niftkConversionUtils.h>
#include <niftkCommandLineParser.h>
#include <itkCommandLineHelper.h>

#include <itkCommand.h>
#include <itkSimpleFilterWatcher.h>
#include <itkImageRegionIterator.h>
#include <itkImageRegionConstIterator.h>
#include <itkBasicImageFeaturesImageFilter.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>
#include <itkNifTKImageIOFactory.h>
#include <itkRescaleIntensityImageFilter.h>
#include <itkImage.h>
#include <itkUnaryFunctorImageFilter.h>
#include <itkScalarToRGBBIFPixelFunctor.h>
#include <itkScalarToRGBOBIFPixelFunctor.h>
#if 0
#include <itkMaskImageFilter.h>
#endif
#include <itkResampleImageFilter.h>
#include <itkLinearInterpolateImageFunction.h>
#include <itkIdentityTransform.h>
#include <itkSliceBySliceImageFilterPatched.h>

#include <boost/filesystem.hpp>

struct niftk::CommandLineArgumentDescription clArgList[] = {

  {OPT_SWITCH, "st", NULL, "Perform single threaded execution [multi-threaded]."},
  {OPT_SWITCH, "resample", NULL, "Speed up the execution by resampling the image."},

  {OPT_SWITCH, "orientate", NULL, "Calculate orientated BIFs [no]."},
  {OPT_SWITCH, "n72", NULL, "Calculate orientations in one degree increments [45degs]."},

  {OPT_SWITCH, "vflip", NULL, "Flip the orientation vertically (e.g. for PA vs AP views)."},
  {OPT_SWITCH, "hflip", NULL, "Flip the orientation horizontally (e.g. for ML vs LM views)."},

  {OPT_SWITCH, "noSlope", NULL, "Ignore slopes, i.e. only classify as 2nd order."},

  {OPT_INT,    "slice", "dimension",   "The slice direction to process: 0:'x', 1:'y', 2:'z' [0]."},

  {OPT_DOUBLEx2, "origin", "ox,oy", "Orientate relative to this origin in mm (0,0 = corner of the image)."},

  {OPT_FLOAT, "sigma", "value", "The Guassian std. dev. in mm at which to compute the BIFs [1.0]."},
  {OPT_INT,   "nscales", "n",   "The number of scales to process [1]."},
  {OPT_FLOAT, "fscales", "value", "The multiplicative factor between scales [2.0]."},

  {OPT_FLOAT, "e", "epsilon", "The noise suppression parameter [1e-05]."},

  {OPT_STRING, "u2D", "filename", "Local reference orientation in 'x'."},
  {OPT_STRING, "v2D", "filename", "Local reference orientation in 'y'."},

#if 0
  {OPT_STRING, "mask", "filename", "Only compute BIFs where the mask image is non-zero."},
#endif

  {OPT_FLOAT, "t",    "threshold", "Only compute BIFs where the input image is greater than <threshold>."},
#if 0
  {OPT_STRING, "om", "filename", "Write the computed mask image to a file."},
#endif

  {OPT_STRING, "oS00", "filename", "Save the zero order smoothed image to a file."},
  {OPT_STRING, "oS10", "filename", "Save the first derivative in 'x' to a file."},
  {OPT_STRING, "oS01", "filename", "Save the first derivative in 'y' to a file."},
  {OPT_STRING, "oS11", "filename", "Save the second derivative in 'xy' to a file."},
  {OPT_STRING, "oS20", "filename", "Save the second derivative in 'xx' to a file."},
  {OPT_STRING, "oS02", "filename", "Save the second derivative in 'yy' to a file."},

  {OPT_STRING, "oFlat", "filename", "Save the flatness response to a file."},
  {OPT_STRING, "oSlope", "filename", "Save the slope-like response to a file."},
  {OPT_STRING, "oDarkBlob", "filename", "Save the dark blob response to a file."},
  {OPT_STRING, "oLightBlob", "filename", "Save the light blob response to a file."},
  {OPT_STRING, "oDarkLine", "filename", "Save the dark line response to a file."},
  {OPT_STRING, "oLightLine", "filename", "Save the light line response to a file."},
  {OPT_STRING, "oSaddle", "filename", "Save the saddlelike response to a file."},

  {OPT_STRING, "oOrient", "filename", "Save the continuous orientation image to a file."},
  {OPT_STRING, "oVar", "filename", "Save the BIF response variance image to a file."},

  {OPT_STRING, "oh",   "filename", "Write the histogram of BIFs to a file.."},
  {OPT_STRING, "opng", "filename", "Write the label image as a colour PNG file for display purposes."},
  {OPT_STRING, "o",    "filename", "The output label image."},

  {OPT_STRING|OPT_LONELY|OPT_REQ, NULL, "filename", "The input image."},

  {OPT_DONE, NULL, NULL,
   "Program to compute basic image features for a 3D image.\n"
  }
};

enum {
  O_SINGLE_THREADED,
  O_RESAMPLE_IMAGES,

  O_ORIENTATE,
  O_72_ORIENTATIONS,

  O_FLIP_VERTICALLY,
  O_FLIP_HORIZONTALLY,

  O_SECOND_ORDER_ONLY,

  O_SLICE_DIMENSION,

  O_ORIGIN,

  O_SIGMA_IN_MM,
  O_NUMBER_OF_SCALES,
  O_SCALE_FACTOR,

  O_EPSILON,

  O_ORIENTATION_INX,
  O_ORIENTATION_INY,

#if 0
  O_MASK,
#endif

  O_THRESHOLD,
#if 0
  O_OUTPUT_MASK,
#endif

  O_OUTPUT_S00,
  O_OUTPUT_S10,
  O_OUTPUT_S01,
  O_OUTPUT_S11,
  O_OUTPUT_S20,
  O_OUTPUT_S02,

  O_OUTPUT_FLAT,
  O_OUTPUT_SLOPE,
  O_OUTPUT_DARK_BLOB,
  O_OUTPUT_LIGHT_BLOB,
  O_OUTPUT_DARK_LINE,
  O_OUTPUT_LIGHT_LINE,
  O_OUTPUT_SADDLE,

  O_OUTPUT_ORIENTATION,
  O_OUTPUT_VARIANCE,

  O_OUTPUT_HISTOGRAM,
  O_OUTPUT_COLOUR_IMAGE,
  O_OUTPUT_IMAGE,

  O_INPUT_IMAGE
};

std::string AddScaleSuffix( std::string filename, float scale, int nScales )
{
  if ( nScales > 1 ) {

    char strScale[128];

    boost::filesystem::path pathname( filename );
    boost::filesystem::path ofilename;

    std::string extension = pathname.extension().string();
    std::string stem = pathname.stem().string();

    if ( extension == std::string( ".gz" ) ) {

      extension = pathname.stem().extension().string() + extension;
      stem = pathname.stem().stem().string();
    }

    sprintf(strScale, "_%03gmm", scale);

    ofilename = pathname.parent_path() /
      boost::filesystem::path( stem + std::string( strScale ) + extension );

    return ofilename.string();
  }
  else
    return filename;
}

std::string AddSuffix( std::string filename, std::string suffix )
{
  boost::filesystem::path pathname( filename );
  boost::filesystem::path ofilename;

  std::string extension = pathname.extension().string();
  std::string stem = pathname.stem().string();

  if ( extension == std::string( ".gz" ) ) {

    extension = pathname.stem().extension().string() + extension;
    stem = pathname.stem().stem().string();
  }

  ofilename = pathname.parent_path() /
    boost::filesystem::path( stem + suffix + extension );

  return ofilename.string();
}

void sliceCallBack(itk::Object* object, const itk::EventObject &, void*)
{
  // the same typedefs than in the main function - should be done in a nicer way
  const unsigned int Dimension = 3;
  const unsigned int SliceDimension = 2;
  typedef float PixelType;
  typedef itk::Image<PixelType, SliceDimension> InputSliceType;
  typedef itk::Image<PixelType, SliceDimension> OutputSliceType;

  typedef itk::Image< PixelType, Dimension >                   ImageType;
  typedef itk::SliceBySliceImageFilter< ImageType, ImageType > SliceBySliceFilterType;
  typedef itk::BasicImageFeaturesImageFilter< InputSliceType, OutputSliceType > BasicImageFeaturesFilterType;

  // real stuff begins here
  // get the slice by slice filter and the median filter
  SliceBySliceFilterType *sliceFilter = dynamic_cast< SliceBySliceFilterType * >( object );

#if 0
  BasicImageFeaturesFilterType *bifFilter = dynamic_cast< BasicImageFeaturesFilterType * >( sliceFilter->GetInputFilter() );
  bifFilter->Print( std::cout );
#endif

  std::cout << std::endl << "Processing slice: " << sliceFilter->GetSliceIndex() << std::endl;
}

int main( int argc, char *argv[] )
{
  itk::NifTKImageIOFactory::Initialize();

  bool flgSingleThreaded;
  bool flgOrientate;
  bool flgResampleImages;

  bool flgFlipVertically;
  bool flgFlipHorizontally;

  bool flgN72;

  bool flgSecondOrderOnly;

  unsigned int iDim;
  unsigned int sliceDimension = 0;

  int iScale;
  int nScales = 1;

  float sigmaInMM = 1;
  float scaleFactor = 2.;
  float scaleFactorRelativeToInput = 1.;

  float epsilon = 1.0e-05;

  float threshold = 0.;

  double *origin = 0;

  std::string fileOrientationInX;
  std::string fileOrientationInY;

#if 0
  std::string fileMask;
  std::string fileOutputMask;
#endif

  std::string fileOutputS00;
  std::string fileOutputS10;
  std::string fileOutputS01;
  std::string fileOutputS11;
  std::string fileOutputS20;
  std::string fileOutputS02;

  std::string fileOutputFlat;
  std::string fileOutputSlope;
  std::string fileOutputDarkBlob;
  std::string fileOutputLightBlob;
  std::string fileOutputDarkLine;
  std::string fileOutputLightLine;
  std::string fileOutputSaddle;

  std::string fileOutputOrientation;
  std::string fileOutputVariance;

  std::string fileOutputHistogram;
  std::string fileOutputImage;
  std::string fileOutputColourImage;

  std::string fileInputImage;

  // Create the command line parser, passing the
  // 'CommandLineArgumentDescription' structure. The final boolean
  // parameter indicates whether the command line options should be
  // printed out as they are parsed.

  niftk::CommandLineParser CommandLineOptions(argc, argv, clArgList, false);

  CommandLineOptions.GetArgument( O_SINGLE_THREADED, flgSingleThreaded );
  CommandLineOptions.GetArgument( O_RESAMPLE_IMAGES, flgResampleImages );

  CommandLineOptions.GetArgument( O_ORIENTATE, flgOrientate );

  CommandLineOptions.GetArgument( O_72_ORIENTATIONS, flgN72 );

  CommandLineOptions.GetArgument( O_FLIP_VERTICALLY,   flgFlipVertically );
  CommandLineOptions.GetArgument( O_FLIP_HORIZONTALLY, flgFlipHorizontally );

  CommandLineOptions.GetArgument( O_SECOND_ORDER_ONLY, flgSecondOrderOnly);

  CommandLineOptions.GetArgument( O_SLICE_DIMENSION, sliceDimension);

  CommandLineOptions.GetArgument( O_ORIGIN, origin );

  CommandLineOptions.GetArgument( O_SIGMA_IN_MM, sigmaInMM );
  CommandLineOptions.GetArgument( O_NUMBER_OF_SCALES, nScales );
  CommandLineOptions.GetArgument( O_SCALE_FACTOR, scaleFactor );

  CommandLineOptions.GetArgument( O_EPSILON, epsilon );

  CommandLineOptions.GetArgument( O_ORIENTATION_INX, fileOrientationInX );
  CommandLineOptions.GetArgument( O_ORIENTATION_INY, fileOrientationInY );

  if ( (fileOrientationInX.length() || fileOrientationInY.length()) && origin) {

    std::cerr <<"Command line options: -u2D and -v2D cannot be used with -origin";
    return EXIT_FAILURE;
  }

  if ( (fileOrientationInX.length() || fileOrientationInY.length())
       && ! (fileOrientationInX.length() && fileOrientationInY.length()) ) {

    std::cerr <<"Both command line options: -u2D and -v2D are required";
    return EXIT_FAILURE;
  }

#if 0
  CommandLineOptions.GetArgument( O_MASK, fileMask );
#endif

  CommandLineOptions.GetArgument( O_THRESHOLD, threshold );
#if 0
  CommandLineOptions.GetArgument( O_OUTPUT_MASK, fileOutputMask );
#endif

  CommandLineOptions.GetArgument( O_OUTPUT_S00, fileOutputS00 );
  CommandLineOptions.GetArgument( O_OUTPUT_S10, fileOutputS10 );
  CommandLineOptions.GetArgument( O_OUTPUT_S01, fileOutputS01 );
  CommandLineOptions.GetArgument( O_OUTPUT_S11, fileOutputS11 );
  CommandLineOptions.GetArgument( O_OUTPUT_S20, fileOutputS20 );
  CommandLineOptions.GetArgument( O_OUTPUT_S02, fileOutputS02 );

  CommandLineOptions.GetArgument( O_OUTPUT_FLAT,       fileOutputFlat );
  CommandLineOptions.GetArgument( O_OUTPUT_SLOPE,      fileOutputSlope );
  CommandLineOptions.GetArgument( O_OUTPUT_DARK_BLOB,  fileOutputDarkBlob );
  CommandLineOptions.GetArgument( O_OUTPUT_LIGHT_BLOB, fileOutputLightBlob );
  CommandLineOptions.GetArgument( O_OUTPUT_DARK_LINE,  fileOutputDarkLine );
  CommandLineOptions.GetArgument( O_OUTPUT_LIGHT_LINE, fileOutputLightLine );
  CommandLineOptions.GetArgument( O_OUTPUT_SADDLE,     fileOutputSaddle );

  CommandLineOptions.GetArgument( O_OUTPUT_ORIENTATION, fileOutputOrientation );
  CommandLineOptions.GetArgument( O_OUTPUT_VARIANCE, fileOutputVariance );

  CommandLineOptions.GetArgument( O_OUTPUT_HISTOGRAM, fileOutputHistogram );
  CommandLineOptions.GetArgument( O_OUTPUT_COLOUR_IMAGE, fileOutputColourImage );
  CommandLineOptions.GetArgument( O_OUTPUT_IMAGE, fileOutputImage );

  CommandLineOptions.GetArgument( O_INPUT_IMAGE, fileInputImage );

  // Read the input image
  // ~~~~~~~~~~~~~~~~~~~~

  // Define the dimension of the images
  const unsigned int ImageDimension = 3;

  int dims = itk::PeekAtImageDimension(fileInputImage);
  // Define the dimension of the images

  if (dims != ImageDimension)
  {
    std::cerr << "Unsupported image dimension." << std::endl;
    return EXIT_FAILURE;
  }

  const unsigned int SliceDimension = 2;

  typedef float InputPixelType;
  typedef float OutputPixelType;

  typedef itk::Image<InputPixelType, ImageDimension> InputImageType;
  typedef itk::Image<OutputPixelType, ImageDimension> OutputImageType;

  typedef itk::Image<InputPixelType, SliceDimension> InputSliceType;
  typedef itk::Image<OutputPixelType, SliceDimension> OutputSliceType;

  typedef itk::ImageFileReader< InputImageType > FileReaderType;
  typedef itk::ImageFileReader< InputSliceType > SliceReaderType;

  typedef itk::BasicImageFeaturesImageFilter< InputSliceType, OutputSliceType > BasicImageFeaturesFilterType;

#if 0
  typedef BasicImageFeaturesFilterType::MaskImageType MaskImageType;

  typedef itk::ImageFileReader< MaskImageType > MaskReaderType;
#endif

  typedef itk::SliceBySliceImageFilter< InputImageType, OutputImageType > SliceBySliceImageFilterType;

  FileReaderType::Pointer imageReader = FileReaderType::New();

  imageReader->SetFileName(fileInputImage);

  try
  {
    std::cout << "Reading the input image" << std::endl;
    imageReader->Update();
  }
  catch (itk::ExceptionObject &ex)
  {
    std::cout << ex << std::endl;
    return EXIT_FAILURE;
  }

  InputImageType::SizeType    nPixelsInput;
  InputImageType::SpacingType resnInput;
  InputImageType::PointType   originInput;

  nPixelsInput = imageReader->GetOutput()->GetLargestPossibleRegion().GetSize();
  resnInput    = imageReader->GetOutput()->GetSpacing();
  originInput  = imageReader->GetOutput()->GetOrigin();

  // Set up the image resampler
  // ~~~~~~~~~~~~~~~~~~~~~~~~~~

  InputImageType::SizeType    nPixelsResampled;
  InputImageType::SpacingType resnResampled;
  InputImageType::PointType   originResampled;

  InputImageType::Pointer pInputImage;

  typedef itk::ResampleImageFilter< InputImageType, InputImageType > ResampleFilterType;
  ResampleFilterType::Pointer resampleInputFilter = 0;

  typedef itk::IdentityTransform< double, ImageDimension > IdentityTransformType;
  IdentityTransformType::Pointer resampleIdentityTransform = 0;

  typedef itk::LinearInterpolateImageFunction< InputImageType, double > ResampleInterpolatorType;
  ResampleInterpolatorType::Pointer resampleInterpolator = 0;

  if ( flgResampleImages ) {

    for ( iDim=0; iDim<ImageDimension; iDim++) {

      nPixelsResampled[iDim] = nPixelsInput[iDim];
      resnResampled[iDim]    = resnInput[iDim];
      originResampled[iDim]  = originInput[iDim];
    }

    resampleInputFilter = ResampleFilterType::New();

    resampleIdentityTransform = IdentityTransformType::New();
    resampleInterpolator      = ResampleInterpolatorType::New();

    resampleInputFilter->SetInput( imageReader->GetOutput() );
    resampleInputFilter->SetOutputSpacing( resnResampled );
    resampleInputFilter->SetOutputOrigin( originResampled );
    resampleInputFilter->SetSize( nPixelsResampled );

    resampleInputFilter->SetTransform( resampleIdentityTransform );
    resampleInputFilter->SetInterpolator( resampleInterpolator );

    resampleInputFilter->SetDefaultPixelValue( 0 );

    InputImageType::DirectionType direction;
    direction.SetIdentity();
    resampleInputFilter->SetOutputDirection( direction );

    try
      {
	std::cout << "Resampling the input image" << std::endl;
	resampleInputFilter->Update();
      }
    catch (itk::ExceptionObject &ex)
      {
	std::cout << ex << std::endl;
	return EXIT_FAILURE;
      }

    pInputImage = resampleInputFilter->GetOutput();

  }
  else
    pInputImage = imageReader->GetOutput();

  // Read the local orientation images
  // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

  SliceReaderType::Pointer xOrientReader, yOrientReader;

  if ( (fileOrientationInX.length() > 0) && (fileOrientationInY.length() > 0) ) {

    flgOrientate = true;

    xOrientReader = SliceReaderType::New();

    xOrientReader->SetFileName( fileOrientationInX );

    try
      {
	std::cout << "Reading the local orientation in 'x'" << std::endl;
	xOrientReader->Update();
      }
    catch (itk::ExceptionObject &ex)
      {
	std::cout << ex << std::endl;
	return EXIT_FAILURE;
      }

    yOrientReader = SliceReaderType::New();

    yOrientReader->SetFileName( fileOrientationInY );

    try
      {
	std::cout << "Reading the local orientation in 'y'" << std::endl;
	yOrientReader->Update();
      }
    catch (itk::ExceptionObject &ex)
      {
	std::cout << ex << std::endl;
	return EXIT_FAILURE;
      }
  }

  // Read the mask
  // ~~~~~~~~~~~~~

#if 0
  MaskImageType::Pointer pMaskImage = 0;
  MaskReaderType::Pointer maskReader = 0;

  if ( fileMask.length() > 0 ) {

    maskReader = MaskReaderType::New();

    maskReader->SetFileName(fileMask);

    try
      {
	std::cout << "Reading the mask image" << std::endl;
	maskReader->Update();
      }
    catch (itk::ExceptionObject &ex)
      {
	std::cout << ex << std::endl;
	return EXIT_FAILURE;
      }

    pMaskImage = maskReader->GetOutput();
  }

  // Or create it by thresholding the input image

  if ( threshold ) {

    if ( ! pMaskImage ) {

      pMaskImage = MaskImageType::New();

      pMaskImage->SetRegions( pInputImage->GetLargestPossibleRegion() );
      pMaskImage->SetSpacing( pInputImage->GetSpacing() );
      pMaskImage->SetOrigin( pInputImage->GetOrigin() );

      pMaskImage->Allocate( );
      pMaskImage->FillBuffer( 1. );
    }

    typedef itk::ImageRegionConstIterator< InputImageType > InputIteratorType;

    InputIteratorType itInput( pInputImage, pInputImage->GetLargestPossibleRegion() );

    InputImageType::IndexType index;

    itInput.GoToBegin();

    while (! itInput.IsAtEnd() ) {

      index = itInput.GetIndex();

      if ( itInput.Get() < threshold )

	pMaskImage->SetPixel( index, 0.);

      ++itInput;
    }
  }

  if ( pMaskImage && ( fileOutputMask.length() > 0 ) ) {

    typedef itk::ImageFileWriter< MaskImageType > FileWriterType;

    FileWriterType::Pointer writer = FileWriterType::New();

    writer->SetFileName( fileOutputMask.c_str() );
    writer->SetInput( pMaskImage );

    try
    {
      std::cout << "Writing: " << fileOutputMask.c_str() << std::endl;
      writer->Update();
    }
    catch (itk::ExceptionObject &e)
    {
      std::cerr << e << std::endl;
    }
  }
#endif

  // Create the basic image features filter
  // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

  BasicImageFeaturesFilterType::Pointer BIFsFilter = BasicImageFeaturesFilterType::New();

  if (flgSingleThreaded)
    BIFsFilter->SetSingleThreadedExecution();

  BIFsFilter->SetEpsilon( epsilon );

  if (flgOrientate) {
    BIFsFilter->CalculateOrientatedBIFs();

    if ( flgN72 )
      BIFsFilter->SetNumberOfOrientations( 72 );
  }

  if ( flgFlipVertically )   BIFsFilter->SetFlipVertically();
  if ( flgFlipHorizontally ) BIFsFilter->SetFlipHorizontally();

  if ( flgSecondOrderOnly ) BIFsFilter->SecondOrderOnly();

  if (origin) {
    BasicImageFeaturesFilterType::OriginType bifOrigin;

    bifOrigin[0] = origin[0];
    bifOrigin[1] = origin[1];

    BIFsFilter->SetOrigin( bifOrigin );
  }

  if ( (fileOrientationInX.length() > 0) && (fileOrientationInY.length() > 0) )
    BIFsFilter->SetLocalOrientation( xOrientReader->GetOutput(),
				     yOrientReader->GetOutput() );

#if 0
  if ( pMaskImage )
    BIFsFilter->SetMask( pMaskImage );
#endif

  // Run the filter at each scale
  // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~

  for (iScale=0; iScale<nScales; iScale++) {

    BIFsFilter->SetSigma( sigmaInMM );

    // Create a slice by slice filter
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

    SliceBySliceImageFilterType::Pointer sliceBySliceFilter = SliceBySliceImageFilterType::New();

    sliceBySliceFilter->SetFilter( BIFsFilter );
    sliceBySliceFilter->SetDimension( sliceDimension );

    sliceBySliceFilter->SetInput( pInputImage );

    itk::CStyleCommand::Pointer command = itk::CStyleCommand::New();
    command->SetCallback( *sliceCallBack );

    sliceBySliceFilter->AddObserver( itk::IterationEvent(), command );

    itk::SimpleFilterWatcher watcher( sliceBySliceFilter, "filter" );

    try
      {
	std::cout << "Computing basic image features";
	sliceBySliceFilter->Update();
      }
    catch (itk::ExceptionObject &e)
      {
	std::cerr << e << std::endl;
      }

    // Compute a histogram of the BIFs
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

    if ( fileOutputHistogram.length() > 0 ) {

      unsigned int iBin;
      unsigned int nBins;

      float nPixels = 0;
      float *histogram;

      OutputPixelType pixel;

      OutputImageType::Pointer bifs = sliceBySliceFilter->GetOutput();

      if (flgOrientate) {
	if ( flgN72 )
	  nBins = 183;
	else
	  nBins = 23;
      }
      else
	nBins = 7;

      histogram = new float[nBins];

      for (iBin=0; iBin<nBins; iBin++)
	histogram[ iBin ] = 0.;

#if 0
      if ( pMaskImage )
      {

	typedef itk::ImageRegionConstIterator< MaskImageType > MaskIteratorType;

	MaskImageType::Pointer mask = pMaskImage;

	MaskIteratorType itMask( pMaskImage, pMaskImage->GetLargestPossibleRegion() );

	MaskImageType::IndexType index;

	itMask.GoToBegin();

	while (! itMask.IsAtEnd() ) {

	  index = itMask.GetIndex();

	  if ( itMask.Get() > 0 ) {	// if inside the mask

	    pixel = bifs->GetPixel( index );

	    if ( (pixel < 0) || (pixel >= nBins) )
	      std::cerr << "BIF value ("
			<< niftk::ConvertToString(pixel)
			<< ") exceeds histogram range (0 to "
			<< niftk::ConvertToString(nBins - 1) << ".";

	    else {
	      nPixels++;
	      histogram[ (unsigned int) pixel ]++;
	    }
	  }

	  ++itMask;
	}
      }
      else
#endif
      {
	typedef itk::ImageRegionConstIterator< OutputImageType > IteratorType;

	IteratorType itBIFs( bifs, bifs->GetLargestPossibleRegion() );

	itBIFs.GoToBegin();

	while (! itBIFs.IsAtEnd() ) {

	  pixel = itBIFs.Get();

	  if ( (pixel < 0) || (pixel >= nBins) )
	    std::cerr <<std::string("BIF value (")
		      << niftk::ConvertToString(pixel)
		      << ") exceeds histogram range (0 to "
		      << niftk::ConvertToString(nBins - 1) + ".";

	  else {
	    nPixels++;
	    histogram[ (unsigned int) pixel ]++;
	  }

	  ++itBIFs;
	}

      }

      std::fstream fout;
      fout.open( AddScaleSuffix( fileOutputHistogram, sigmaInMM, nScales ).c_str(), std::ios::out );

      if ((! fout) || fout.bad()) {
	std::cerr << "Failed to open file: "
		  << AddScaleSuffix( fileOutputHistogram, sigmaInMM, nScales ) << std::endl;
	exit(1);
      }

      std::cout << "Writing: "
		<< AddScaleSuffix( fileOutputHistogram, sigmaInMM, nScales ) << std::endl;

      for (iBin=0; iBin<nBins; iBin++)

	fout << std::setw(6) << iBin << " "
	     << histogram[ iBin ]/nPixels << std::endl;

      delete histogram;
      fout.close();
    }

    // Write the derivatives?
    // ~~~~~~~~~~~~~~~~~~~~~~

    if ( fileOutputS00.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 0, AddScaleSuffix( fileOutputS00,
							    sigmaInMM, nScales ) );
    if ( fileOutputS10.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 1, AddScaleSuffix( fileOutputS10,
							    sigmaInMM, nScales ) );
    if ( fileOutputS01.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 2, AddScaleSuffix( fileOutputS01,
							    sigmaInMM, nScales ) );
    if ( fileOutputS11.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 3, AddScaleSuffix( fileOutputS11,
							    sigmaInMM, nScales ) );
    if ( fileOutputS20.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 4, AddScaleSuffix( fileOutputS20,
							    sigmaInMM, nScales ) );
    if ( fileOutputS02.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 5, AddScaleSuffix( fileOutputS02,
							    sigmaInMM, nScales ) );

    // Write the filter responses?
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~

    if ( fileOutputFlat.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 0, AddScaleSuffix( fileOutputFlat,
								sigmaInMM, nScales ) );

    if ( fileOutputSlope.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 1, AddScaleSuffix( fileOutputSlope,
								sigmaInMM, nScales ) );

    if ( fileOutputDarkBlob.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 2, AddScaleSuffix( fileOutputDarkBlob,
								sigmaInMM, nScales ) );

    if ( fileOutputLightBlob.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 3, AddScaleSuffix( fileOutputLightBlob,
								sigmaInMM, nScales ) );

    if ( fileOutputLightLine.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 4, AddScaleSuffix( fileOutputLightLine,
								sigmaInMM, nScales ) );

    if ( fileOutputDarkLine.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 5, AddScaleSuffix( fileOutputDarkLine,
								sigmaInMM, nScales ) );

    if ( fileOutputSaddle.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 6, AddScaleSuffix( fileOutputSaddle,
								sigmaInMM, nScales ) );

    // Write the BIF image?
    // ~~~~~~~~~~~~~~~~~~~~

    if (fileOutputImage.length() != 0) {

      typedef itk::ImageFileWriter< OutputImageType > FileWriterType;

      FileWriterType::Pointer writer = FileWriterType::New();

      writer->SetFileName( AddScaleSuffix( fileOutputImage, sigmaInMM, nScales ) );
      writer->SetInput( sliceBySliceFilter->GetOutput() );

      try
	{
	  std::cout << "Writing: "
		    << AddScaleSuffix( fileOutputImage, sigmaInMM, nScales ) << std::endl;
	  writer->Update();
	}
      catch (itk::ExceptionObject &e)
	{
	  std::cerr << e << std::endl;
	}
    }

    // Increase the scale used
    // ~~~~~~~~~~~~~~~~~~~~~~~

    sigmaInMM *= scaleFactor;
    scaleFactorRelativeToInput *= scaleFactor;

    // Update the resampling?
    // ~~~~~~~~~~~~~~~~~~~~~~

    if ( flgResampleImages ) {
      float actualSamplingFactor;

      for ( iDim=0; iDim<ImageDimension; iDim++) {

        nPixelsResampled[iDim] = static_cast<InputImageType::SizeType::SizeValueType>(ceil( ((float) nPixelsInput[iDim])
               / scaleFactorRelativeToInput ));

	actualSamplingFactor = ((float) nPixelsInput[iDim]) / ((float) nPixelsResampled[iDim] );

	resnResampled[iDim]    = resnInput[iDim] * actualSamplingFactor;

	originResampled[iDim]  = originInput[iDim] + resnResampled[iDim]/2. - resnInput[iDim]/2.;
      }

      resampleInputFilter->SetOutputSpacing( resnResampled );
      resampleInputFilter->SetOutputOrigin( originResampled );
      resampleInputFilter->SetSize( nPixelsResampled );

      resampleInputFilter->SetInput( pInputImage );

      try
	{
	  std::cout << "Resampling the input image by: " << actualSamplingFactor << std::endl;
	  resampleInputFilter->UpdateLargestPossibleRegion();
	}
      catch (itk::ExceptionObject &ex)
	{
	  std::cout << ex << std::endl;
	  return EXIT_FAILURE;
	}
    }

  }
}


#include <boost/graph/spectrum.hpp>
#include "graph_types.hpp"
#include <boost/graph/python/graph.hpp>
#include <boost/python.hpp>
#include <vector>

namespace boost { namespace graph { namespace python {

template <typename Graph>
boost::python::tuple
spectrum
  (Graph& g,
   int first_eigenvector_index,
   int num_eigenvectors,
   double rel_tol = 100,
   double abs_tol = 1000)
{
  using std::vector;
  using boost::python::object;

  typedef typename property_map<Graph, vertex_index_t>::const_type VertexIndexMap;
  typedef vector_property_map<vertex_index_t, VertexIndexMap> IndexMap;
  typedef std::vector<double> Vector;

  int N = num_vertices(g);
  std::vector<Vector> evecs(num_eigenvectors);
  for (int i = 0; i < num_eigenvectors; i++)
    evecs[i] = *(new Vector(N));
  std::vector<double> evals(num_eigenvectors);

  boost::spectrum<Graph, std::vector<Vector> >(g, first_eigenvector_index, num_eigenvectors, evecs, evals, rel_tol, abs_tol);

  boost::python::list *evec;

  boost::python::list eigenvectors;
  for(int i = 0; i < num_eigenvectors; i++) {
    evec = new boost::python::list();
    for (int j = 0; j < N; j++)
      evec->append(evecs[i][j]);
    eigenvectors.append(*evec);
  }

  boost::python::list eigenvalues;
  for(int i = 0; i < num_eigenvectors; i++) {
      eigenvalues.append(evals[i]);
  }

  return boost::python::make_tuple(eigenvalues, eigenvectors);
}

void export_spectrum()
{
  using boost::python::arg;
  using boost::python::def;
  using boost::python::object;

  def("spectrum",
      &spectrum<Graph>,
      (arg("graph"),
       arg("first_eigenvector_index") = (int)1,
       arg("num_eigenvectors") = (int)1,
       arg("rel_tol") = (double)100,
       arg("abs_tol") = (double)1000)
      );

}

} } } // end namespace boost::graph::python


#include "isd.h"
#include <algorithm>
#include <boost/math/tools/roots.hpp>

using boost::math::tools::bisect;

const double tol = 1e-5;
const double epsilon = 1e-5;
bool quantum = false;

InformationSetDecoding::InformationSetDecoding(unsigned int as, double cr, double w,
                                               const std::string& m, const std::string& a)
{
    if(AlphabetSizeCheck(as))
        alphabetSize = as;
    else
        throw std::invalid_argument("Alphabet size needs to be greater or equal to 2.");

    if(MetricCheck(m))
        metric = m;
    else
        throw std::invalid_argument("This metric is not offered. Allowed metrics are hamming and lee.");

    if(ParamCheck(cr))
        codeRate = cr;
    else
        throw std::invalid_argument("Normalized value needs to be in the interval [0,1].");

    if(ParamCheck(w))
        weight = w;
    else
        throw std::invalid_argument("Normalized value needs to be in the interval [0,1].");

    if(AlgCheck(a))
        algorithm = a;
    else
        throw std::invalid_argument("This algorithm is not offered. Allowed algorithms are prange, dumer and wagner.");

    space = VectorSpace(metric, alphabetSize);
    surfaceW = space.SphereSurfArea(weight);
    if (surfaceW == -1)
    {
        throw std::invalid_argument("Surface area surfaceW is not found.");
    }
}

InformationSetDecoding& InformationSetDecoding::operator=(const InformationSetDecoding& isd)
{
    if (this == &isd)
        return *this;

    alphabetSize = isd.alphabetSize;
    codeRate = isd.codeRate;
    weight = isd.weight;
    metric = isd.metric;
    algorithm = isd.algorithm;
    space = isd.space;
    surfaceW = isd.surfaceW;

    return *this;
}

double InformationSetDecoding::BDayDecCost(double paramL, double paramP, double optLevelNum) const
{
    double distance1 = paramP / (codeRate + paramL);
    if(!ParamCheck(distance1))
    {
        if (distance1 >= 1.0 && distance1 <= 1.0 + epsilon)
        {
            distance1 = 1.0;
        }
        else
        {
            std::cout << distance1 << std::endl;
            throw std::runtime_error("Numerical issue on parameter paramP / (codeRate + paramL).");
        }
    }

    double surface1 = (codeRate + paramL) * space.SphereSurfArea(distance1);
    if (surface1 == -1)
    {
        throw std::invalid_argument("Surface area surface1 is not found.");
    }

    if (quantum)
    {
        return std::min(surface1 / (pow(2, optLevelNum) + 1), paramL / optLevelNum);
    }
    else
    {
        return std::min(surface1 / pow(2, optLevelNum), paramL / optLevelNum);
    }
}

double InformationSetDecoding::SolsPerIter(double paramL, double paramP, double optLevelNum) const
{
    double distance1 = paramP / (codeRate + paramL);
    if(!ParamCheck(distance1))
    {
        if (distance1 >= 1.0 && distance1 <= 1.0 + epsilon)
        {
            distance1 = 1.0;
        }
        else
        {
            std::cout << distance1 << std::endl;
            throw std::runtime_error("Numerical issue on parameter paramP / (codeRate + paramL).");
        }
    }

    double surface1 = (codeRate + paramL) * space.SphereSurfArea(distance1);
    if (surface1 == -1)
    {
        throw std::invalid_argument("Surface area surface1 is not found.");
    }

    double bottomListSize;
    if (quantum)
    {
        bottomListSize = std::min(surface1 / (pow(2, optLevelNum) + 1), paramL / optLevelNum);
    }
    else
    {
        bottomListSize = std::min(surface1 / pow(2, optLevelNum), paramL / optLevelNum);
    }

    double paramM = paramL - (optLevelNum - 1) * bottomListSize;
    if (quantum)
    {
        return 3 * bottomListSize - paramM;
    }
    else
    {
        return 2 * bottomListSize - paramM;
    }
}

double InformationSetDecoding::IterCost(double paramL, double paramP, double optLevelNum) const
{
    double distance1 = paramP / (codeRate + paramL);
    if(!ParamCheck(distance1))
    {
        if (distance1 >= 1.0 && distance1 <= 1.0 + epsilon)
        {
            distance1 = 1.0;
        }
        else
        {
            std::cout << distance1 << std::endl;
            throw std::runtime_error("Numerical issue on parameter paramP / (codeRate + paramL).");
        }
    }

    double surface1 = (codeRate + paramL) * space.SphereSurfArea(distance1);
    if (surface1 == -1)
    {
        throw std::invalid_argument("Surface area surface1 is not found.");
    }

    if (quantum)
    {
        return std::min(surface1 / (pow(2, optLevelNum) + 1), paramL / optLevelNum);
    }
    else
    {
        return std::min(surface1 / pow(2, optLevelNum), paramL / optLevelNum);
    }
}

double InformationSetDecoding::SolsNum() const
{
    return std::max(surfaceW - (1 - codeRate), 0.0);
}

double InformationSetDecoding::PartSolProb(double paramL, double paramP) const
{
    double distance2 = (weight - paramP) / (1 - codeRate - paramL);
    if(!ParamCheck(distance2))
    {
        if (distance2 >= 1.0 && distance2 <= 1.0 + epsilon)
        {
            distance2 = 1.0;
        }
        else
        {
            std::cout << distance2 << std::endl;
            throw std::runtime_error("Numerical issue on parameter (weight - paramP) / (1 - codeRate - paramL).");
        }
    }

    double surface2 = (1 - codeRate - paramL) * space.SphereSurfArea(distance2);
    if (surface2 == -1)
    {
        throw std::invalid_argument("Surface area surface2 is not found.");
    }

    return surface2 - surfaceW + paramL;
}

double InformationSetDecoding::AnySolProb(double paramL, double paramP) const
{
    double distance2 = (weight - paramP) / (1 - codeRate - paramL);
    if(!ParamCheck(distance2))
    {
        if (distance2 >= 1.0 && distance2 <= 1.0 + epsilon)
        {
            distance2 = 1.0;
        }
        else
        {
            std::cout << distance2 << std::endl;
            throw std::runtime_error("Numerical issue on parameter (weight - paramP) / (1 - codeRate - paramL).");
        }
    }

    double surface2 = (1 - codeRate - paramL) * space.SphereSurfArea(distance2);
    if (surface2 == -1)
    {
        throw std::invalid_argument("Surface area surface2 is not found.");
    }

    return std::min(0.0, surface2 + paramL - std::min(1 - codeRate, surfaceW));
}

double InformationSetDecoding::RunTime(double paramL, double paramP, unsigned int& optLevelNum) const
{
    double distance1 = paramP / (codeRate + paramL);
    if(!ParamCheck(distance1))
    {
        if (distance1 >= 1.0 && distance1 <= 1.0 + epsilon)
        {
            distance1 = 1.0;
        }
        else
        {
            std::cout << distance1 << std::endl;
            throw std::runtime_error("Numerical issue on parameter paramP / (codeRate + paramL).");
        }
    }

    double surface1 = (codeRate + paramL) * space.SphereSurfArea(distance1);
    if (surface1 == -1)
    {
        throw std::invalid_argument("Surface area surface1 is not found.");
    }

    double distance2 = (weight - paramP) / (1 - codeRate - paramL);
    if(!ParamCheck(distance2))
    {
        if (distance2 >= 1.0 && distance2 <= 1.0 + epsilon)
        {
            distance2 = 1.0;
        }
        else
        {
            std::cout << distance2 << std::endl;
            throw std::runtime_error("Numerical issue on parameter (weight - paramP) / (1 - codeRate - paramL).");
        }
    }

    double surface2 = (1 - codeRate - paramL) * space.SphereSurfArea(distance2);
    if (surface2 == -1)
    {
        throw std::invalid_argument("Surface area surface2 is not found.");
    }

    auto theorem = [=](unsigned int optLevelNum) -> bool
    {
        return bool(paramL <= optLevelNum / pow(2.0, optLevelNum) * surface1);
    };

    if (!algorithm.compare("prange") || !algorithm.compare("dumer"))
    {
        optLevelNum = 1;
    }
    else
    {
        optLevelNum = 2;
        while (theorem(optLevelNum))
        {
            optLevelNum++;
        }
        optLevelNum--;
    }

    double bottomListSize;
    if (quantum)
    {
        bottomListSize = std::min(surface1 / (pow(2, optLevelNum) + 1), paramL / optLevelNum);
    }
    else
    {
        bottomListSize = std::min(surface1 / pow(2, optLevelNum), paramL / optLevelNum);
    }

    double paramM = paramL - (optLevelNum - 1) * bottomListSize;
    if (quantum)
    {
        return bottomListSize - 0.5 * std::min(0.0, std::min(0.0, surface2 + paramL - std::min(1 - codeRate, surfaceW)) + 3 * bottomListSize - paramM);
    }
    else
    {
        return bottomListSize - std::min(0.0, std::min(0.0, surface2 + paramL - std::min(1 - codeRate, surfaceW)) + 2 * bottomListSize - paramM);
    }
}

double InformationSetDecoding::GoldenSectionSearch(double& paramP, unsigned int& optLevelNum)
{
    double paramPLow = std::max(0.0, weight - (1 - codeRate));
    double paramPHigh = std::min(weight, codeRate);
    std::function<double(double)> runTimeFun = [&](double p) {return RunTime(0.0, p, optLevelNum); };
    paramP = ::GoldenSectionSearch(paramPLow, paramPHigh, tol, runTimeFun);

    return log2(alphabetSize) * RunTime(0.0, paramP, optLevelNum);
}

double InformationSetDecoding::GoldenSectionSearch(double& paramL, double& paramP, unsigned int& optLevelNum)
{
    const double paramLLow = 0, paramLHigh = 1 - codeRate;
    double a = paramLLow, b = paramLHigh;
    const double gr = (sqrt(5) + 1) / 2;
    double c = b - (b - a) / gr;
    double d = a + (b - a) / gr;

    while (std::abs(c - d) > tol)
    {
        double paramPLowC = std::max(0.0, weight - (1 - codeRate - c));
        double paramPHighC = std::min(weight, codeRate + c);
        std::function<double(double)> runTimeC = [&](double p) {return RunTime(c, p, optLevelNum); };
        double optC = ::GoldenSectionSearch(paramPLowC, paramPHighC, tol, runTimeC);

        double paramPLowD = std::max(0.0, weight - (1 - codeRate - d));
        double paramPHighD = std::min(weight, codeRate + d);
        std::function<double(double)> runTimeD = [&](double p) {return RunTime(d, p, optLevelNum); };
        double optD = ::GoldenSectionSearch(paramPLowD, paramPHighD, tol, runTimeD);

        if (RunTime(c, optC, optLevelNum) < RunTime(d, optD, optLevelNum))
        {
            b = d;
        }
        else
        {
            a = c;
        }
        c = b - (b - a) / gr;
        d = a + (b - a) / gr;
    }

    paramL = (b + a) / 2;
    std::function<double(double)> runTime = [&](double p) {return RunTime(paramL, p, optLevelNum); };
    paramP = ::GoldenSectionSearch(std::max(0.0, weight - (1 - codeRate - paramL)),
        std::min(weight, codeRate + paramL), tol, runTime);

    return log2(alphabetSize) * RunTime(paramL, paramP, optLevelNum);
}

struct TerminationCondition {
    bool operator() (double min, double max) {
        return std::abs(min - max) <= tol;
    }
};

double AvgSolsNum(const VectorSpace& space, double distance, double codeRate)
{
    double surface = space.SphereSurfArea(distance);
    if (surface == -1)
    {
        throw std::invalid_argument(
            "Surface area surface1 is not found.");
    }
    return surface - (1 - codeRate);
}

double UpperRoot(std::string metric, unsigned int alphabetSize, double codeRate)
{
    VectorSpace space(metric, alphabetSize);
    std::function<double(double)> avgSolsNumCR = [&](double cr) {return AvgSolsNum(space, 1 - epsilon, cr); };
    std::pair<double, double> bracketsCR = bisect(avgSolsNumCR, 0.0, 1.0, TerminationCondition());
    double highestCodeRate = (bracketsCR.first + bracketsCR.second) / 2;

    if (codeRate > highestCodeRate)
        return 1;
    else
    {
        std::function<double(double)> avgSolsNumW = [&](double w) {return AvgSolsNum(space, w, codeRate); };
        std::pair<double, double> bracketsW = bisect(avgSolsNumW, AvgVectorWeight(space, 1.0) / MaxWeight(space), 1.0, TerminationCondition());
        return (bracketsW.first + bracketsW.second) / 2;
    }
}

double LowerRoot(std::string metric, unsigned int alphabetSize, double codeRate)
{
    VectorSpace space(metric, alphabetSize);
    std::function<double(double)> avgSolsNumW = [&](double w) {return AvgSolsNum(space, w, codeRate); };
    std::pair<double, double> bracketsW = bisect(avgSolsNumW, 0.0, AvgVectorWeight(space, 1.0) / MaxWeight(space), TerminationCondition());
    return (bracketsW.first + bracketsW.second) / 2;
}

double RunTime(std::string metric, std::string algorithm, unsigned int alphabetSize, double codeRate,
    double& weight, double& paramL, double& paramP, unsigned int& optLevelNum)
{
    if(!MetricCheck(metric))
        throw std::invalid_argument("This metric is not offered. Allowed metrics are hamming and lee.");

    double runTime;
    if (!metric.compare("hamming") && alphabetSize != 3)
    {
        weight = LowerRoot(metric, alphabetSize, codeRate) - epsilon;
    }
    else
    {
        weight = UpperRoot(metric, alphabetSize, codeRate) - epsilon;
    }

    InformationSetDecoding isd(alphabetSize, codeRate, weight, metric, algorithm);
    if (!algorithm.compare("prange"))
    {
        try
        {
            paramL = 0;
            runTime = isd.GoldenSectionSearch(paramP, optLevelNum);
        }
        catch (std::runtime_error& e)
        {
            std::cout << e.what() << std::endl;
            return -1;
        }
    }
    else if (!algorithm.compare("dumer") || !algorithm.compare("wagner"))
    {
        try
        {
            runTime = isd.GoldenSectionSearch(paramL, paramP, optLevelNum);
        }
        catch (std::runtime_error& e)
        {
            std::cout << e.what() << std::endl;
            return -1;
        }
    }
    else
    {
        std::cout << "This algorithm is not offered. Allowed algorithms are prange, dumer and wagner." << std::endl;
        return -1;
    }
    return -runTime;
}


#include <geometry.h>
#include <tiny_math_types.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(inside_cylinder_test)
{
  typedef tiny::MathTypes<double>   MT;
  typedef MT::vector3_type          V;
  typedef MT::real_type             T;

  V const center = V::make(1.0, 2.0, 4.0);
  V const axis   = V::make(0.0, 0.0, 1.0);
  T const radius = 2.0;
  T const height = 4.0;

  geometry::Cylinder<V> const cylinder = geometry::make_cylinder(radius, height, axis, center);

  BOOST_CHECK(geometry::is_valid(cylinder));

  // on bottom surface
  {
    V    const p    = V::make( 1.0, 2.0, 2.0);
    bool const test = geometry::inside_cylinder( p,  cylinder);
    BOOST_CHECK( test );
  }

  // on top surface
  {
    V    const p    = V::make( 1.0, 2.0, 6.0);
    bool const test = geometry::inside_cylinder( p,  cylinder);
    BOOST_CHECK( test );
  }

  // on cylinder surface
  {
    V    const p    = V::make( 3.0, 2.0, 4.0);
    bool const test = geometry::inside_cylinder( p,  cylinder);
    BOOST_CHECK( test );
  }

  // inside
  {
    V    const p    = V::make( 1.0, 2.0, 2.1);
    bool const test = geometry::inside_cylinder( p,  cylinder);
    BOOST_CHECK( test );
  }

  // inside
  {
    V    const p    = V::make( 1.0, 2.0, 5.9);
    bool const test = geometry::inside_cylinder( p,  cylinder);
    BOOST_CHECK( test );
  }

  // inside
  {
    V    const p    = V::make( 2.9, 2.0, 4.0);
    bool const test = geometry::inside_cylinder( p,  cylinder);
    BOOST_CHECK( test );
  }

  // outside
  {
    V    const p    = V::make( 1.0, 2.0, 1.9);
    bool const test = geometry::inside_cylinder( p,  cylinder);
    BOOST_CHECK( !test );
  }

  // outside
  {
    V    const p    = V::make( 1.0, 2.0, 6.1);
    bool const test = geometry::inside_cylinder( p,  cylinder);
    BOOST_CHECK( !test );
  }

  // outside
  {
    V    const p    = V::make( 3.1, 2.0, 4.0);
    bool const test = geometry::inside_cylinder( p,  cylinder);
    BOOST_CHECK( !test );
  }

}

BOOST_AUTO_TEST_SUITE_END();


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

/**
* @file degree_radian_conversion.hpp
* Provide cartesian<->spherical conversion without imposing particular data types.
* @author Andreas Franck a.franck@soton.ac.uk
*/

#ifndef VISR_LIBEFL_DEGREE_RADIAN_CONVERSION_HPP_INCLUDED
#define VISR_LIBEFL_DEGREE_RADIAN_CONVERSION_HPP_INCLUDED

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

#include <cmath>

namespace visr
{
namespace efl
{

template< typename T >
T degree2radian( T deg )
{
  return boost::math::constants::degree<T>( ) * deg;
}

template< typename T >
T radian2degree( T rad )
{
  return boost::math::constants::radian<T>( ) * rad;
}

} // namespace efl
} // namespace visr

#endif // #ifndef VISR_LIBEFL_DEGREE_RADIAN_CONVERSION_HPP_INCLUDED


#ifndef _COCONUT_PULP_PRIMITIVE_VECTOR_HPP_
#define _COCONUT_PULP_PRIMITIVE_VECTOR_HPP_

#include <iosfwd>

#include <coconut-tools/serialisation.hpp>

#include <boost/operators.hpp>
#include "detail/VectorType.hpp"

namespace coconut {
namespace pulp {
namespace primitive {

class Vector : // TODO: clashes with pulp::math::Vector name
	boost::additive<Vector>,
	detail::VectorType<4>
{
public:

	Vector() = default;

	Vector(const Vec3& vector) :
		detail::VectorType<4>(vector.x(), vector.y(), vector.z(), 0.0f)
	{
	}

	Vector(const Vec4& vector) :
		detail::VectorType<4>(vector.x(), vector.y(), vector.z(), 0.0f)
	{
	}

	Vector(float x, float y, float z) :
		detail::VectorType<4>(x, y, z, 0.0f)
	{
	}

	Vector& operator+=(const Vector& rhs) {
		static_cast<detail::VectorType<4>&>(*this) += rhs;
		return *this;
	}

	Vector& operator-=(const Vector& rhs) {
		static_cast<detail::VectorType<4>&>(*this) -= rhs;
		return *this;
	}

	Vector cross(const Vector& rhs) const {
		auto lhsV3 = pulp::math::Vec3(x(), y(), z());
		const auto rhsV3 = pulp::math::Vec3(rhs.x(), rhs.y(), rhs.z());
		lhsV3 = lhsV3.cross(rhsV3);
		return Vector(lhsV3.x(), lhsV3.y(), lhsV3.z());
	}

	using detail::VectorType<4>::normalise;

	float& x() {
		return get<0>();
	}

	float x() const {
		return get<0>();
	}

	float& y() {
		return get<1>();
	}

	float y() const {
		return get<1>();
	}

	float& z() {
		return get<2>();
	}

	float z() const {
		return get<2>();
	}

	using detail::VectorType<4>::storeAs;

private:

	friend class Position;

	friend std::ostream& operator<<(std::ostream& os, const Vector& vector) {
		return os << static_cast<const detail::VectorType<4>&>(vector);
	}

};

CT_MAKE_SERIALISABLE(SerialiserType, serialiser, Vector, vector) {
	serialiser(SerialiserType::Label("x"), vector.x());
	serialiser(SerialiserType::Label("y"), vector.y());
	serialiser(SerialiserType::Label("z"), vector.z());
}

} // namespace primitive

using primitive::Vector;

} // namespace pulp
} // namespace coconut

#endif /* _COCONUT_PULP_PRIMITIVE_VECTOR_HPP_ */


/*
 * Test tool for Toy Monte Carlo generation with CRAB submission
 *
 * \author Luca Lista, INFN
 *
 */
#include <boost/program_options.hpp>
#include "RooRandom.h"
#include "RooRealVar.h"
#include "RooGaussian.h"
#include "RooExponential.h"
#include "RooAddPdf.h"
#include "RooDataSet.h"
#include "RooDataHist.h"
#include "RooGlobalFunc.h"
#include "RooChi2Var.h"
#include "RooMinuit.h"
#include "RooPlot.h"
#include "TCanvas.h"
#include "TROOT.h"
#include <string>

static const char * const kHelpOpt = "help";
static const char * const kHelpCommandOpt = "help,h";
static const char * const kSeedOpt = "seed";
static const char * const kSeedCommandOpt = "seed,s";

int main(int argc, char * argv[]) {
  using namespace boost::program_options;
  using namespace std;

  gROOT->SetBatch(kTRUE);
  gROOT->SetStyle("Plain");

  string programName(argv[0]);
  string descString(programName);
  descString += " [options] ";
  options_description desc(descString);

  desc.add_options()
    (kHelpCommandOpt, "produce help message")
    (kSeedCommandOpt, value<unsigned int>(), "random generator seed");

  positional_options_description p;

  p.add(kSeedOpt, -1);

  variables_map vm;
  try {
    store(command_line_parser(argc,argv).options(desc).positional(p).run(), vm);
    notify(vm);
  } catch(const error&) {
    return 7000;
  }

  if(vm.count(kHelpOpt)) {
    cout << desc <<std::endl;
    return 0;
  }

  unsigned int seed = 123456;
  if(vm.count(kSeedOpt)) {
    seed = vm[kSeedOpt].as<unsigned int>();
    cout << "random seed specified by user as: " << seed << endl;
  } else {
    cout << "random seed set by default as: " << seed << endl;
  }

  RooRandom::randomGenerator()->SetSeed(seed);

  RooRealVar x("x", "x", -10, 10);

  RooRealVar mu("mu", "average", 0, -1, 1);
  RooRealVar sigma("sigma", "sigma", 1, 0, 5);
  RooGaussian gauss("gauss","gaussian PDF", x, mu, sigma);

  RooRealVar lambda("lambda", "slope", -0.1, -5., 0.);
  RooExponential expo("expo", "exponential PDF", x, lambda);

  RooRealVar s("s", "signal yield", 1000, 0, 10000);
  RooRealVar b("b", "background yield", 1000, 0, 10000);

  cout << "initial values: " << endl;

  mu.Print();
  sigma.Print();
  lambda.Print();
  s.Print();
  b.Print();

  RooAddPdf sum("sum", "gaussian plus exponential PDF",
		RooArgList(gauss, expo), RooArgList(s, b));

  unsigned int nSample = RooRandom::randomGenerator()->Poisson(s.getVal() + b.getVal());
  RooDataSet * data = sum.generate(x, nSample);
  x.setBins(50);
  RooDataHist hist("hist", "hist", RooArgSet(x), *data);
  RooChi2Var chi2("chi2","chi2", sum, hist, true);
  RooMinuit minuit(chi2);
  minuit.migrad();
  minuit.hesse();

  RooPlot * xFrame = x.frame() ;
  hist.plotOn(xFrame) ;
  sum.plotOn(xFrame) ;
  sum.plotOn(xFrame, RooFit::Components(expo), RooFit::LineStyle(kDashed)) ;
  TCanvas c;
  xFrame->Draw();
  c.SaveAs("binnedChi2Fit.eps");

  sum.getVariables()->Print();
  mu.Print();
  sigma.Print();
  lambda.Print();
  s.Print();
  b.Print();

  return 0;
}


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// 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.

/**
 * \file polar_processing.hpp
 * \brief Polar processing utility functions
 *
 * \author Yuchen Wu, Autonomous Space Robotics Lab (ASRL)
 */
#pragma once

#include <cstdint>
#include <cstdio>
#include <ctime>
#include <set>

#define _USE_MATH_DEFINES
#include <math.h>

#include <Eigen/Dense>

#include <vtr_lidar/cloud/cloud.hpp>
#include <vtr_lidar/nanoflann/nanoflann.hpp>
#include <vtr_lidar/pointmap/pointmap.hpp>

namespace vtr {
namespace lidar {

// KDTree type definition
using PointXYZ_KDTree = nanoflann::KDTreeSingleIndexAdaptor<
    nanoflann::L2_Simple_Adaptor<float, PointCloud>, PointCloud, 3>;

void cart2Pol_(std::vector<PointXYZ>& xyz);
PointXYZ cart2pol(const PointXYZ& p);

void pca_features(std::vector<PointXYZ>& points,
                  std::vector<float>& eigenvalues,
                  std::vector<PointXYZ>& eigenvectors);

void detect_outliers(std::vector<PointXYZ>& rtp, std::vector<float>& scores,
                     int lidar_n_lines, float lidar_angle_res, float minTheta,
                     int n_pass, float threshold);

float get_lidar_angle_res(std::vector<PointXYZ>& rtp, float& minTheta,
                          float& maxTheta, int lidar_n_lines);

void scaleAndLogRadius(std::vector<PointXYZ>& rtp, float r_scale);

void scaleHorizontal(std::vector<PointXYZ>& rtp, float h_scale);

void extract_features_multi_thread(std::vector<PointXYZ>& points,
                                   std::vector<PointXYZ>& normals,
                                   std::vector<float>& planarity,
                                   std::vector<float>& linearity,
                                   int lidar_n_lines, float h_scale,
                                   float r_scale, int verbose);
/**
 * \brief todo
 * \param[in] points point cloud in cartesian
 * \param[in] polar_pts point cloud in polar
 * \param[in] normals point cloud normal
 * \param[in] r0 ideal distance for estimating the normal
 * \param[in] theta0 maximum incidence angle for estimating the normal
 * \param[out] scores a score for each point based on normal direction
 */
void smartNormalScore(const std::vector<PointXYZ>& points,
                      const std::vector<PointXYZ>& polar_pts,
                      const std::vector<PointXYZ>& normals, const float& r0,
                      const float& theta0, std::vector<float>& scores);

void smartICPScore(std::vector<PointXYZ>& polar_pts,
                   std::vector<float>& scores);

void compare_map_to_frame(std::vector<PointXYZ>& frame_points,
                          std::vector<PointXYZ>& map_points,
                          std::vector<PointXYZ>& map_normals,
                          std::unordered_map<VoxKey, size_t>& map_samples,
                          Eigen::Matrix3d R_d, Eigen::Vector3d T_d,
                          float theta_dl, float phi_dl, float map_dl,
                          std::vector<float>& movable_probs,
                          std::vector<int>& movable_counts);

void extractNormal(const std::vector<PointXYZ>& points,
                   const std::vector<PointXYZ>& polar_pts,
                   const std::vector<PointXYZ>& queries,
                   const std::vector<PointXYZ>& polar_queries,
                   const float polar_r, const int parallel_threads,
                   std::vector<PointXYZ>& normals,
                   std::vector<float>& norm_scores);

}  // namespace lidar
}  // namespace vtr


/* adcpp_eigen.hpp
 *
 *  Created on: 21 Aug 2019
 *      Author: Fabian Meyer
 *     License: MIT
 */

#ifndef ADCPP_ADCPP_EIGEN_HPP_
#define ADCPP_ADCPP_EIGEN_HPP_

#include <adcpp/adcpp.hpp>
#include <Eigen/Core>

#define ADCPP_GEN_NUMTRAITS(T) \
    template<>\
    struct NumTraits<T>\
    {\
        using ValueType = T::Scalar;\
        using Real = T;\
        using NonInteger = T;\
        using Nested = T;\
        using Literal = T;\
        enum {\
            IsInteger = std::is_integral<ValueType>::value ? 1 : 0,\
            IsSigned = std::is_signed<ValueType>::value ? 1 : 0,\
            IsComplex = 0,\
            RequireInitialization = 1,\
            ReadCost = 1,\
            AddCost = 3,\
            MulCost = 3\
        };\
        static Real epsilon()\
        {\
            return Real(std::numeric_limits<ValueType>::epsilon());\
        }\
        static Real highest()\
        {\
            return Real(std::numeric_limits<ValueType>::max());\
        }\
        static Real lowest()\
        {\
            return Real(std::numeric_limits<ValueType>::min());\
        }\
        static Real min_exponent()\
        {\
            return Real(std::numeric_limits<ValueType>::min_exponent);\
        }\
        static Real max_exponent()\
        {\
            return Real(std::numeric_limits<ValueType>::max_exponent);\
        }\
        static Real digits()\
        {\
            return Real(std::numeric_limits<ValueType>::digits);\
        }\
        static Real digits10()\
        {\
            return Real(std::numeric_limits<ValueType>::digits10);\
        }\
    }

namespace Eigen
{
    ADCPP_GEN_NUMTRAITS(adcpp::fwd::Double);
    ADCPP_GEN_NUMTRAITS(adcpp::fwd::Float);

    ADCPP_GEN_NUMTRAITS(adcpp::bwd::Double);
    ADCPP_GEN_NUMTRAITS(adcpp::bwd::Float);
}

namespace adcpp
{
namespace fwd
{
    typedef Eigen::Matrix<Double, Eigen::Dynamic, Eigen::Dynamic> MatrixXd;
    typedef Eigen::Matrix<Double, 2, 2> Matrix2d;
    typedef Eigen::Matrix<Double, 3, 3> Matrix3d;
    typedef Eigen::Matrix<Double, 4, 4> Matrix4d;
    typedef Eigen::Matrix<Double, 5, 5> Matrix5d;

    typedef Eigen::Matrix<Double, Eigen::Dynamic, 1> VectorXd;
    typedef Eigen::Matrix<Double, 2, 1> Vector2d;
    typedef Eigen::Matrix<Double, 3, 1> Vector3d;
    typedef Eigen::Matrix<Double, 4, 1> Vector4d;
    typedef Eigen::Matrix<Double, 5, 1> Vector5d;

    typedef Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
    typedef Eigen::Matrix<Float, 2, 2> Matrix2f;
    typedef Eigen::Matrix<Float, 3, 3> Matrix3f;
    typedef Eigen::Matrix<Float, 4, 4> Matrix4f;
    typedef Eigen::Matrix<Float, 5, 5> Matrix5f;

    typedef Eigen::Matrix<Float, Eigen::Dynamic, 1> VectorXf;
    typedef Eigen::Matrix<Float, 2, 1> Vector2f;
    typedef Eigen::Matrix<Float, 3, 1> Vector3f;
    typedef Eigen::Matrix<Float, 4, 1> Vector4f;
    typedef Eigen::Matrix<Float, 5, 1> Vector5f;
}

namespace bwd
{
    typedef Eigen::Matrix<Double, Eigen::Dynamic, Eigen::Dynamic> MatrixXd;
    typedef Eigen::Matrix<Double, 2, 2> Matrix2d;
    typedef Eigen::Matrix<Double, 3, 3> Matrix3d;
    typedef Eigen::Matrix<Double, 4, 4> Matrix4d;
    typedef Eigen::Matrix<Double, 5, 5> Matrix5d;

    typedef Eigen::Matrix<Double, Eigen::Dynamic, 1> VectorXd;
    typedef Eigen::Matrix<Double, 2, 1> Vector2d;
    typedef Eigen::Matrix<Double, 3, 1> Vector3d;
    typedef Eigen::Matrix<Double, 4, 1> Vector4d;
    typedef Eigen::Matrix<Double, 5, 1> Vector5d;

    typedef Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
    typedef Eigen::Matrix<Float, 2, 2> Matrix2f;
    typedef Eigen::Matrix<Float, 3, 3> Matrix3f;
    typedef Eigen::Matrix<Float, 4, 4> Matrix4f;
    typedef Eigen::Matrix<Float, 5, 5> Matrix5f;

    typedef Eigen::Matrix<Float, Eigen::Dynamic, 1> VectorXf;
    typedef Eigen::Matrix<Float, 2, 1> Vector2f;
    typedef Eigen::Matrix<Float, 3, 1> Vector3f;
    typedef Eigen::Matrix<Float, 4, 1> Vector4f;
    typedef Eigen::Matrix<Float, 5, 1> Vector5f;

    template<typename Scalar, typename DerivedA, typename DerivedB>
    inline void gradient(const Eigen::MatrixBase<DerivedA> &x,
        const Number<Scalar> &f,
        Eigen::MatrixBase<DerivedB> &grad)
    {
        assert(grad.size() == x.size());

        typename bwd::Number<Scalar>::DerivativeMap derivative;
        f.derivative(derivative);

        grad.setZero();
        for(long int i = 0; i < x.size(); ++i)
        {
            if(derivative.contains(x(i)))
                grad(i) = derivative(x(i));
        }
    }

    template<typename DerivedA, typename DerivedB, typename DerivedC>
    inline void jacobian(const Eigen::MatrixBase<DerivedA> &x,
        const Eigen::MatrixBase<DerivedB> &f,
        Eigen::MatrixBase<DerivedC> &jac)
    {
        assert(jac.rows() == f.size());
        assert(jac.cols() == x.size());

        typename Eigen::MatrixBase<DerivedB>::Scalar::DerivativeMap derivative;
        jac.setZero();
        for(long int i = 0; i < f.size(); ++i)
        {
            f(i).derivative(derivative);
            for(long int j = 0; j < x.size(); ++j)
            {
                if(derivative.contains(x(j)))
                    jac(i, j) = derivative(x(j));
            }
        }
    }
}
}

#endif