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

#include <boost/geometry/algorithms/detail/vincenty_direct.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/core/srs.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/geometries/point.hpp>

namespace bg = boost::geometry;

namespace loc {
    const double d2r = M_PI / 180.0;
    const double r2d = 180.0 / M_PI;

    const double gda_a = 6378.1370;
    const double gda_f = 1.0 / 298.25722210;
    const double gda_b = gda_a * ( 1.0 - gda_f );

    template <typename P>
    bool LatLngUtil::non_precise_ct()
    {
        typedef typename bg::coordinate_type<P>::type ct;
        return boost::is_integral<ct>::value || boost::is_float<ct>::value;
    }

    template <typename P, typename Spheroid>
     Point2D LatLngUtil::transform(LatLng latlng, Anchor anchor, Spheroid spheroid) {
        typedef typename bg::promote_floating_point
        <typename bg::select_calculation_type<P, P, void>::type>::type calc_t;

        typedef bg::detail::vincenty_inverse<calc_t, true, true> inverse_formula;
        typename inverse_formula::result_type
        result_i = inverse_formula::apply(anchor.latlng.lng * d2r,
                                          anchor.latlng.lat * d2r,
                                          latlng.lng * d2r,
                                          latlng.lat * d2r,
                                          spheroid);
        calc_t dist = result_i.distance;
        calc_t azimuth = ((result_i.azimuth * r2d) - anchor.rotate) * d2r;

        Point2D p;
        p.x = sin(azimuth) * dist;
        p.y = cos(azimuth) * dist;
        return p;
    }

    template <typename P, typename Spheroid>
     LatLng LatLngUtil::transform(LatLng latlng, double dist, double angle, Spheroid spheroid){
        typedef typename bg::promote_floating_point
        <typename bg::select_calculation_type<P, P, void>::type>::type calc_t;

        typedef bg::detail::vincenty_direct<calc_t> direct_formula;
        typename direct_formula::result_type
        result = direct_formula::apply(latlng.lng * d2r,
                                       latlng.lat * d2r,
                                       dist,
                                       angle * d2r,
                                       spheroid);

        double lat = result.lat2 * r2d;
        double lng = result.lon2 * r2d;
        LatLng ll;
        ll.lat = lat;
        ll.lng = lng;

        return ll;
    }

     LatLng LatLngUtil::localToGlobal(const Point2D p, const Anchor anchor) {
        bg::srs::spheroid<double> const gda_spheroid(gda_a, gda_b);
        // usually atan2 takes (y, x) order but rotation is opposite in spherical coordinate
        double r = (atan2(p.x, p.y) * r2d)+anchor.rotate;
        double d = sqrt(pow(p.x,2)+pow(p.y,2)) / 1000; // in kilometer

        typedef typename bg::model::point<double, 2, bg::cs::geographic<bg::degree>> point_t;
        return transform<point_t>(anchor.latlng, d, r, gda_spheroid);
    }

    Point2D LatLngUtil::globalToLocal(const LatLng latlng, const Anchor anchor) {
        bg::srs::spheroid<double> const gda_spheroid(gda_a, gda_b);
        typedef typename bg::model::point<double, 2, bg::cs::geographic<bg::degree>> point_t;
        Point2D p = transform<point_t>(latlng, anchor, gda_spheroid);
        p.x *= 1000;
        p.y *= 1000;
        return p;
    }

};


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_MPL_LIMIT_STRING_SIZE 64
#define BOOST_METAPARSE_LIMIT_STRING_SIZE BOOST_MPL_LIMIT_STRING_SIZE

#include <boost/metaparse/grammar.hpp>

#include <boost/metaparse/string.hpp>
#include <boost/metaparse/build_parser.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/int_.hpp>
#include <boost/metaparse/transform.hpp>

#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/fold.hpp>

using boost::mpl::apply_wrap1;
using boost::mpl::front;
using boost::mpl::back;
using boost::mpl::if_;
using boost::mpl::bool_;

using boost::metaparse::build_parser;
using boost::metaparse::token;
using boost::metaparse::entire_input;
using boost::metaparse::int_;
using boost::metaparse::grammar;
using boost::metaparse::transform;

#ifdef BOOST_NO_CXX11_CONSTEXPR
int main()
{
  std::cout << "Please use a compiler that supports constexpr" << std::endl;
}
#else

#ifdef _STR
#  error _STR already defined
#endif
#define _STR BOOST_METAPARSE_STRING

template <class T, char C>
struct is_c : bool_<T::type::value == C> {};

struct build_plus_impl
{
  template <class A, class B>
  class _plus
  {
  public:
    typedef _plus type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) + _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class A, class B>
  class _minus
  {
  public:
    typedef _minus type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) - _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class State, class C>
  struct apply :
    if_<
      typename is_c<front<C>, '+'>::type,
      _plus<State, typename back<C>::type>,
      _minus<State, typename back<C>::type>
    >
  {};
};

struct build_plus
{
  typedef build_plus type;

  template <class Seq>
  struct apply :
    boost::mpl::fold<
      typename back<Seq>::type,
      typename front<Seq>::type,
      build_plus_impl
    >
  {};
};

struct build_mult_impl
{
  template <class A, class B>
  class _mult
  {
  public:
    typedef _mult type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) * _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class A, class B>
  class _div
  {
  public:
    typedef _div type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) / _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class State, class C>
  struct apply :
    if_<
      typename is_c<front<C>, '*'>::type,
      _mult<State, typename back<C>::type>,
      _div<State, typename back<C>::type>
    >
  {};
};

struct build_mult
{
  typedef build_mult type;

  template <class Seq>
  struct apply :
    boost::mpl::fold<
      typename back<Seq>::type,
      typename front<Seq>::type,
      build_mult_impl
    >
  {};
};

struct build_value
{
  typedef build_value type;

  template <class V>
  struct apply
  {
    typedef apply type;

    template <class T>
    int operator()(T) const
    {
      return V::type::value;
    }
  };
};

struct build_arg
{
  typedef build_arg type;

  template <class>
  struct apply
  {
    typedef apply type;

    template <class T>
    T operator()(T t) const
    {
      return t;
    }
  };
};

struct keep_front
{
  typedef keep_front type;

  template <class Seq>
  struct apply : front<Seq> {};
};

typedef
  grammar<_STR("plus_exp")>
    ::import<_STR("int_token"), token<transform<int_, build_value>>>::type

    ::rule<_STR("ws ::= (' ' | '\n' | '\r' | '\t')*")>::type
    ::rule<_STR("plus_token ::= '+' ws"), keep_front>::type
    ::rule<_STR("minus_token ::= '-' ws"), keep_front>::type
    ::rule<_STR("mult_token ::= '*' ws"), keep_front>::type
    ::rule<_STR("div_token ::= '/' ws"), keep_front>::type
    ::rule<_STR("arg_token ::= '_' ws"), keep_front>::type

    ::rule<_STR("plus_exp ::= prod_exp ((plus_token | minus_token) prod_exp)*"), build_plus>::type
    ::rule<_STR("prod_exp ::= value_exp ((mult_token | div_token) value_exp)*"), build_mult>::type
    ::rule<_STR("value_exp ::= int_token | arg_exp")>::type
    ::rule<_STR("arg_exp ::= arg_token"), build_arg>::type
  g;

typedef build_parser<entire_input<g>> function_parser;

#ifdef LAMBDA
  #error LAMBDA already defined
#endif
#define LAMBDA(exp) apply_wrap1<function_parser, _STR(#exp)>::type

LAMBDA(13) f1;
LAMBDA(2 + 3) f2;
LAMBDA(2 * 3) f3;
LAMBDA(1+ 2*4-6/2) f4;
LAMBDA(2 * _) f5;

int main()
{
  using std::cout;
  using std::endl;

  cout
    << f1(11) << endl
    << f2(11) << endl
    << f3(11) << endl
    << f4(11) << endl
    << f5(11) << endl
    << f5(1.1) << endl
    ;
}

#endif


//  (C) Copyright Eric Niebler 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/unit_test.hpp>
#include <boost/time_series/delta_series.hpp>
#include <boost/time_series/dense_series.hpp>
#include <boost/time_series/sparse_series.hpp>
#include <boost/time_series/numeric/numeric.hpp>
#include <boost/time_series/ordered_inserter.hpp>

using namespace boost::unit_test;
using namespace boost::time_series;

namespace seq = boost::sequence;
namespace rrs = boost::range_run_storage;

///////////////////////////////////////////////////////////////////////////////
// test_series_divides
//
void test_series_divides()
{
    // check multiplication of a sparse time series and a dense time series yields
    // a sparse time series
    dense_series<int> ss1;
    sparse_series<int> ss2;

    make_ordered_inserter(ss1)(1, 0)(3)(4)(1).commit();
    make_ordered_inserter(ss2)(4, 2)(5, 3).commit();

    BOOST_CHECK_EQUAL(4, std::distance(seq::begin(ss1), seq::end(ss1)));
    BOOST_CHECK_EQUAL(2, std::distance(seq::begin(ss2), seq::end(ss2)));

    sparse_series<int> ss3 = ss2 / ss1;
    BOOST_CHECK_EQUAL(2, std::distance(seq::begin(ss3), seq::end(ss3)));
    BOOST_CHECK_EQUAL(1, seq::elements(ss3)(*seq::begin(ss3)));
    BOOST_CHECK_EQUAL(2, rrs::offset(rrs::runs(ss3)(*seq::begin(ss3))));
    BOOST_CHECK_EQUAL(5, seq::elements(ss3)(*++seq::begin(ss3)));
    BOOST_CHECK_EQUAL(3, rrs::offset(rrs::runs(ss3)(*++seq::begin(ss3))));

    ss2 /= ss1;
    BOOST_CHECK_EQUAL(2, std::distance(seq::begin(ss2), seq::end(ss2)));
    BOOST_CHECK_EQUAL(1, seq::elements(ss2)(*seq::begin(ss2)));
    BOOST_CHECK_EQUAL(2, rrs::offset(rrs::runs(ss2)(*seq::begin(ss2))));
    BOOST_CHECK_EQUAL(5, seq::elements(ss2)(*++seq::begin(ss2)));
    BOOST_CHECK_EQUAL(3, rrs::offset(rrs::runs(ss2)(*++seq::begin(ss2))));

    //// check division of a delta time series by a dense time series yields
    //// a scaled delta time series
    //descriptor<> disc2(size = 4);
    //dense_series<int> ss4(disc2);
    //delta_unit_series<int> ss5(disc2, 2);
    //rrs::set_at(ss4, rng(0), 1);
    //rrs::set_at(ss4, rng(1), 3);
    //rrs::set_at(ss4, rng(2), 4);
    //rrs::set_at(ss4, rng(3), 1);
    //BOOST_CHECK_EQUAL(4, std::distance(seq::begin(ss4), seq::end(ss4)));
    //BOOST_CHECK_EQUAL(1, std::distance(seq::begin(ss5), seq::end(ss5)));

    //time_series_facade<int, scaled_storage_tag<delta_storage_tag>, daily> ss6 = ss5 / ss4;
    //BOOST_CHECK_EQUAL(1, std::distance(seq::begin(ss6), seq::end(ss6)));
    //BOOST_CHECK_EQUAL(1, seq::elements(ss6)(*seq::begin(ss6)));
    //BOOST_CHECK_EQUAL(2, rrs::offset(rrs::runs(ss6)(*seq::begin(ss6))));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("series divides test");

    test->add(BOOST_TEST_CASE(&test_series_divides));

    return test;
}


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/51-100/75/problem75.hpp"

BOOST_AUTO_TEST_SUITE( Problem75 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem75::solve(48);
        BOOST_CHECK_EQUAL(res, 6);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem75::solve();
        BOOST_CHECK_EQUAL(res, 161667);
    }

BOOST_AUTO_TEST_SUITE_END()


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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

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

#include <mitkGIFVolumetricDensityStatistics.h>

// MITK
#include <mitkITKImageImport.h>
#include <mitkImageCast.h>
#include <mitkImageAccessByItk.h>
#include <mitkPixelTypeMultiplex.h>
#include <mitkImagePixelReadAccessor.h>

// ITK
#include <itkLabelStatisticsImageFilter.h>
#include <itkNeighborhoodIterator.h>
#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkLabelGeometryImageFilter.h>

// VTK
#include <vtkSmartPointer.h>
#include <vtkImageMarchingCubes.h>
#include <vtkMassProperties.h>
#include <vtkDelaunay3D.h>
#include <vtkGeometryFilter.h>
#include <vtkDoubleArray.h>
#include <vtkPCAStatistics.h>
#include <vtkTable.h>

// STL
#include <limits>
#include <vnl/vnl_math.h>

// Eigen
#include <Eigen/Dense>

struct GIFVolumetricDensityStatisticsParameters
{
  double volume;
  std::string prefix;
};

template<typename TPixel, unsigned int VImageDimension>
void
CalculateVolumeDensityStatistic(itk::Image<TPixel, VImageDimension>* itkImage, mitk::Image::Pointer mask, GIFVolumetricDensityStatisticsParameters params, mitk::GIFVolumetricDensityStatistics::FeatureListType & featureList)
{
  typedef itk::Image<TPixel, VImageDimension> ImageType;
  typedef itk::Image<unsigned short, VImageDimension> MaskType;

  double volume = params.volume;
  std::string prefix = params.prefix;

  typename MaskType::Pointer maskImage = MaskType::New();
  mitk::CastToItkImage(mask, maskImage);

  itk::ImageRegionConstIteratorWithIndex<ImageType> imgA(itkImage, itkImage->GetLargestPossibleRegion());
  itk::ImageRegionConstIteratorWithIndex<ImageType> imgB(itkImage, itkImage->GetLargestPossibleRegion());
  itk::ImageRegionConstIteratorWithIndex<MaskType> maskA(maskImage, maskImage->GetLargestPossibleRegion());
  itk::ImageRegionConstIteratorWithIndex<MaskType> maskB(maskImage, maskImage->GetLargestPossibleRegion());

  double moranA = 0;
  double moranB = 0;
  double geary = 0;
  double Nv = 0;
  double w_ij = 0;
  double mean = 0;

  typename ImageType::PointType pointA;
  typename ImageType::PointType pointB;

  while (!imgA.IsAtEnd())
  {
    if (maskA.Get() > 0)
    {
      Nv += 1;
      mean += imgA.Get();
    }
    ++imgA;
    ++maskA;
  }
  mean /= Nv;
  imgA.GoToBegin();
  maskA.GoToBegin();

  while (!imgA.IsAtEnd())
  {
    if (maskA.Get() > 0)
    {
      imgB.GoToBegin();
      maskB.GoToBegin();
      while (!imgB.IsAtEnd())
      {
        if ((imgA.GetIndex() == imgB.GetIndex()) ||
          (maskB.Get() < 1))
        {
          ++imgB;
          ++maskB;
          continue;
        }
        itkImage->TransformIndexToPhysicalPoint(maskA.GetIndex(), pointA);
        itkImage->TransformIndexToPhysicalPoint(maskB.GetIndex(), pointB);

        double w = 1 / pointA.EuclideanDistanceTo(pointB);
        moranA += w*(imgA.Get() - mean)* (imgB.Get() - mean);
        geary += w * (imgA.Get() - imgB.Get()) * (imgA.Get() - imgB.Get());

        w_ij += w;

        ++imgB;
        ++maskB;
      }
      moranB += (imgA.Get() - mean)* (imgA.Get() - mean);
    }
    ++imgA;
    ++maskA;
  }

  MITK_INFO << "Volume: " << volume;
  MITK_INFO << " Mean: " << mean;
  featureList.push_back(std::make_pair(prefix + "Volume integrated intensity", volume* mean));
  featureList.push_back(std::make_pair(prefix + "Volume Moran's I index", Nv / w_ij * moranA / moranB));
  featureList.push_back(std::make_pair(prefix + "Volume Geary's C measure", ( Nv -1 ) / 2 / w_ij * geary/ moranB));
}

void calculateMOBB(vtkPointSet *pointset, double &volume, double &surface)
{
  volume = std::numeric_limits<double>::max();

  for (int cellID = 0; cellID < pointset->GetNumberOfCells(); ++cellID)
  {
    auto cell = pointset->GetCell(cellID);

    for (int edgeID = 0; edgeID < 3; ++edgeID)
    {
      auto edge = cell->GetEdge(edgeID);

      double pA[3], pB[3];
      double pAA[3], pBB[3];

      vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
      transform->PostMultiply();
      pointset->GetPoint(edge->GetPointId(0), pA);
      pointset->GetPoint(edge->GetPointId(1), pB);

      double angleZ = std::atan2((- pA[2] + pB[2]) ,(pA[1] - pB[1]));
      angleZ *= 180 / vnl_math::pi;
      if (pA[2] == pB[2])
        angleZ = 0;

      transform->RotateX(angleZ);
      transform->TransformPoint(pA, pAA);
      transform->TransformPoint(pB, pBB);

      double angleY = std::atan2((pAA[1] -pBB[1]) ,-(pAA[0] - pBB[0]));
      angleY *= 180 / vnl_math::pi;
      if (pAA[1] == pBB[1])
        angleY = 0;
      transform->RotateZ(angleY);

      double p0[3];
      pointset->GetPoint(edge->GetPointId(0), p0);

      double curMinX = std::numeric_limits<double>::max();
      double curMaxX = std::numeric_limits<double>::lowest();
      double curMinY = std::numeric_limits<double>::max();
      double curMaxY = std::numeric_limits<double>::lowest();
      double curMinZ = std::numeric_limits<double>::max();
      double curMaxZ = std::numeric_limits<double>::lowest();
      for (int pointID = 0; pointID < pointset->GetNumberOfPoints(); ++pointID)
      {
        double p[3];
        double p2[3];
        pointset->GetPoint(pointID, p);
        p[0] -= p0[0]; p[1] -= p0[1]; p[2] -= p0[2];
        transform->TransformPoint(p, p2);

        curMinX = std::min<double>(p2[0], curMinX);
        curMaxX = std::max<double>(p2[0], curMaxX);
        curMinY = std::min<double>(p2[1], curMinY);
        curMaxY = std::max<double>(p2[1], curMaxY);
        curMinZ = std::min<double>(p2[2], curMinZ);
        curMaxZ = std::max<double>(p2[2], curMaxZ);
      }

      if ((curMaxX - curMinX)*(curMaxY - curMinY)*(curMaxZ - curMinZ) < volume)
      {
        volume = (curMaxX - curMinX)*(curMaxY - curMinY)*(curMaxZ - curMinZ);
        surface = (curMaxX - curMinX)*(curMaxX - curMinX) + (curMaxY - curMinY)*(curMaxY - curMinY) + (curMaxZ - curMinZ)*(curMaxZ - curMinZ);
        surface *= 2;
      }

    }
  }
}

void calculateMEE(vtkPointSet *pointset, double &vol, double &surf, double tolerance=0.0001)
{
  // Inspired by https://github.com/smdabdoub/ProkaryMetrics/blob/master/calc/fitting.py

  int numberOfPoints = pointset->GetNumberOfPoints();
  int dimension = 3;
  Eigen::MatrixXd points(3, numberOfPoints);
  Eigen::MatrixXd Q(3+1, numberOfPoints);
  double p[3];

  std::cout << "Initialize Q " << std::endl;
  for (int i = 0; i < numberOfPoints; ++i)
  {
    pointset->GetPoint(i, p);
    points(0, i) = p[0];
    points(1, i) = p[1];
    points(2, i) = p[2];
    Q(0, i) = p[0];
    Q(1, i) = p[1];
    Q(2, i) = p[2];
    Q(3, i) = 1.0;
  }

  int count = 1;
  double error = 1;
  Eigen::VectorXd u_vector(numberOfPoints);
  u_vector.fill(1.0 / numberOfPoints);
  Eigen::DiagonalMatrix<double, Eigen::Dynamic> u = u_vector.asDiagonal();
  Eigen::VectorXd ones(dimension + 1);
  ones.fill(1);
  Eigen::MatrixXd Ones = ones.asDiagonal();

  // Khachiyan Algorithm
  while (error > tolerance)
  {
    auto Qt = Q.transpose();
    Eigen::MatrixXd X = Q*u*Qt;
    Eigen::FullPivHouseholderQR<Eigen::MatrixXd> qr(X);
    Eigen::MatrixXd Xi = qr.solve(Ones);

    Eigen::MatrixXd M = Qt * Xi * Q;

    double maximumValue = M(0, 0);
    int maximumPosition = 0;
    for (int i = 0; i < numberOfPoints; ++i)
    {
      if (maximumValue < M(i, i))
      {
        maximumValue = M(i, i);
        maximumPosition = i;
      }
    }
    double stepsize = (maximumValue - dimension - 1) / ((dimension + 1) * (maximumValue - 1));
    Eigen::DiagonalMatrix<double, Eigen::Dynamic> new_u = (1.0 - stepsize) * u;
    new_u.diagonal()[maximumPosition] = (new_u.diagonal())(maximumPosition) + stepsize;
    ++count;
    error = (new_u.diagonal() - u.diagonal()).norm();
    u.diagonal() = new_u.diagonal();
  }

   // U = u

  Eigen::MatrixXd Ai = points * u * points.transpose() - points * u *(points * u).transpose();
  Eigen::FullPivHouseholderQR<Eigen::MatrixXd> qr(Ai);
  Eigen::VectorXd ones2(dimension);
  ones2.fill(1);
  Eigen::MatrixXd Ones2 = ones2.asDiagonal();
  Eigen::MatrixXd A = qr.solve(Ones2)*1.0/dimension;

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A);
  double c = 1 / sqrt(svd.singularValues()[0]);
  double b = 1 / sqrt(svd.singularValues()[1]);
  double a = 1 / sqrt(svd.singularValues()[2]);
  double V = 4 * vnl_math::pi*a*b*c / 3;

  double ad_mvee= 0;
  double alpha = std::sqrt(1 - b*b / a / a);
  double beta = std::sqrt(1 - c*c / a / a);
  for (int i = 0; i < 20; ++i)
  {
    ad_mvee += 4 * vnl_math::pi*a*b*(alpha*alpha + beta*beta) / (2 * alpha*beta) * (std::pow(alpha*beta, i)) / (1 - 4 * i*i);
  }
  vol = V;
  surf = ad_mvee;
}

mitk::GIFVolumetricDensityStatistics::FeatureListType mitk::GIFVolumetricDensityStatistics::CalculateFeatures(const Image::Pointer & image, const Image::Pointer &mask)
{
  FeatureListType featureList;
  if (image->GetDimension() < 3)
  {
    return featureList;
  }

  std::string prefix = FeatureDescriptionPrefix();

  vtkSmartPointer<vtkImageMarchingCubes> mesher = vtkSmartPointer<vtkImageMarchingCubes>::New();
  vtkSmartPointer<vtkMassProperties> stats = vtkSmartPointer<vtkMassProperties>::New();
  vtkSmartPointer<vtkMassProperties> stats2 = vtkSmartPointer<vtkMassProperties>::New();
  mesher->SetInputData(mask->GetVtkImageData());
  mesher->SetValue(0, 0.5);
  stats->SetInputConnection(mesher->GetOutputPort());
  stats->Update();

  vtkSmartPointer<vtkDelaunay3D> delaunay =
    vtkSmartPointer< vtkDelaunay3D >::New();
  delaunay->SetInputConnection(mesher->GetOutputPort());
  delaunay->SetAlpha(0);
  delaunay->Update();
  vtkSmartPointer<vtkGeometryFilter> geometryFilter =
    vtkSmartPointer<vtkGeometryFilter>::New();
  geometryFilter->SetInputConnection(delaunay->GetOutputPort());
  geometryFilter->Update();
  stats2->SetInputConnection(geometryFilter->GetOutputPort());
  stats2->Update();

  double vol_mvee;
  double surf_mvee;
  calculateMEE(mesher->GetOutput(), vol_mvee, surf_mvee);

  double vol_mobb;
  double surf_mobb;
  calculateMOBB(geometryFilter->GetOutput(), vol_mobb, surf_mobb);

  double pi = vnl_math::pi;

  double meshVolume = stats->GetVolume();
  double meshSurf = stats->GetSurfaceArea();

  GIFVolumetricDensityStatisticsParameters params;
  params.volume = meshVolume;
  params.prefix = prefix;
  AccessByItk_3(image, CalculateVolumeDensityStatistic, mask, params, featureList);

  //Calculate center of mass shift
  int xx = mask->GetDimensions()[0];
  int yy = mask->GetDimensions()[1];
  int zz = mask->GetDimensions()[2];

  double xd = mask->GetGeometry()->GetSpacing()[0];
  double yd = mask->GetGeometry()->GetSpacing()[1];
  double zd = mask->GetGeometry()->GetSpacing()[2];

  int minimumX=xx;
  int maximumX=0;
  int minimumY=yy;
  int maximumY=0;
  int minimumZ=zz;
  int maximumZ=0;

  vtkSmartPointer<vtkDoubleArray> dataset1Arr = vtkSmartPointer<vtkDoubleArray>::New();
  vtkSmartPointer<vtkDoubleArray> dataset2Arr = vtkSmartPointer<vtkDoubleArray>::New();
  vtkSmartPointer<vtkDoubleArray> dataset3Arr = vtkSmartPointer<vtkDoubleArray>::New();
  dataset1Arr->SetNumberOfComponents(1);
  dataset2Arr->SetNumberOfComponents(1);
  dataset3Arr->SetNumberOfComponents(1);
  dataset1Arr->SetName("M1");
  dataset2Arr->SetName("M2");
  dataset3Arr->SetName("M3");

  vtkSmartPointer<vtkDoubleArray> dataset1ArrU = vtkSmartPointer<vtkDoubleArray>::New();
  vtkSmartPointer<vtkDoubleArray> dataset2ArrU = vtkSmartPointer<vtkDoubleArray>::New();
  vtkSmartPointer<vtkDoubleArray> dataset3ArrU = vtkSmartPointer<vtkDoubleArray>::New();
  dataset1ArrU->SetNumberOfComponents(1);
  dataset2ArrU->SetNumberOfComponents(1);
  dataset3ArrU->SetNumberOfComponents(1);
  dataset1ArrU->SetName("M1");
  dataset2ArrU->SetName("M2");
  dataset3ArrU->SetName("M3");

  vtkSmartPointer<vtkPoints> points =
    vtkSmartPointer< vtkPoints >::New();

  for (int x = 0; x < xx; x++)
  {
    for (int y = 0; y < yy; y++)
    {
      for (int z = 0; z < zz; z++)
      {
        itk::Image<int,3>::IndexType index;

        index[0] = x;
        index[1] = y;
        index[2] = z;

        mitk::ScalarType pxImage;
        mitk::ScalarType pxMask;

        mitkPixelTypeMultiplex5(
              mitk::FastSinglePixelAccess,
              image->GetChannelDescriptor().GetPixelType(),
              image,
              image->GetVolumeData(),
              index,
              pxImage,
              0);

        mitkPixelTypeMultiplex5(
              mitk::FastSinglePixelAccess,
              mask->GetChannelDescriptor().GetPixelType(),
              mask,
              mask->GetVolumeData(),
              index,
              pxMask,
              0);

        //Check if voxel is contained in segmentation
        if (pxMask > 0)
        {
          minimumX = std::min<int>(x, minimumX);
          minimumY = std::min<int>(y, minimumY);
          minimumZ = std::min<int>(z, minimumZ);
          maximumX = std::max<int>(x, maximumX);
          maximumY = std::max<int>(y, maximumY);
          maximumZ = std::max<int>(z, maximumZ);
          points->InsertNextPoint(x*xd, y*yd, z*zd);

          if (pxImage == pxImage)
          {
            dataset1Arr->InsertNextValue(x*xd);
            dataset2Arr->InsertNextValue(y*yd);
            dataset3Arr->InsertNextValue(z*zd);
          }
        }
      }
    }
  }

  vtkSmartPointer<vtkTable> datasetTable = vtkSmartPointer<vtkTable>::New();
  datasetTable->AddColumn(dataset1Arr);
  datasetTable->AddColumn(dataset2Arr);
  datasetTable->AddColumn(dataset3Arr);

  vtkSmartPointer<vtkPCAStatistics> pcaStatistics = vtkSmartPointer<vtkPCAStatistics>::New();
  pcaStatistics->SetInputData(vtkStatisticsAlgorithm::INPUT_DATA, datasetTable);
  pcaStatistics->SetColumnStatus("M1", 1);
  pcaStatistics->SetColumnStatus("M2", 1);
  pcaStatistics->SetColumnStatus("M3", 1);
  pcaStatistics->RequestSelectedColumns();
  pcaStatistics->SetDeriveOption(true);
  pcaStatistics->Update();

  vtkSmartPointer<vtkDoubleArray> eigenvalues = vtkSmartPointer<vtkDoubleArray>::New();
  pcaStatistics->GetEigenvalues(eigenvalues);

  std::vector<double> eigen_val(3);
  eigen_val[2] = eigenvalues->GetValue(0);
  eigen_val[1] = eigenvalues->GetValue(1);
  eigen_val[0] = eigenvalues->GetValue(2);

  double major = 2*sqrt(eigen_val[2]);
  double minor = 2*sqrt(eigen_val[1]);
  double least = 2*sqrt(eigen_val[0]);

  double alpha = std::sqrt(1 - minor*minor / major / major);
  double beta = std::sqrt(1 - least*least / major / major);

  double a = (maximumX - minimumX+1) * xd;
  double b = (maximumY - minimumY+1) * yd;
  double c = (maximumZ - minimumZ+1) * zd;

  double vd_aabb = meshVolume / (a*b*c);
  double ad_aabb = meshSurf / (2 * a*b + 2 * a*c + 2 * b*c);

  double vd_aee = 3 * meshVolume / (4.0*pi*major*minor*least);
  double ad_aee = 0;
  for (int i = 0; i < 20; ++i)
  {
    ad_aee += 4 * pi*major*minor*(alpha*alpha + beta*beta) / (2 * alpha*beta) * (std::pow(alpha*beta, i)) / (1 - 4 * i*i);
  }
  ad_aee = meshSurf / ad_aee;

  double vd_ch = meshVolume / stats2->GetVolume();
  double ad_ch = meshSurf / stats2->GetSurfaceArea();

  featureList.push_back(std::make_pair(prefix + "Volume density axis-aligned bounding box", vd_aabb));
  featureList.push_back(std::make_pair(prefix + "Surface density axis-aligned bounding box", ad_aabb));
  featureList.push_back(std::make_pair(prefix + "Volume density oriented minimum bounding box", meshVolume / vol_mobb));
  featureList.push_back(std::make_pair(prefix + "Surface density oriented minimum bounding box", meshSurf / surf_mobb));
  featureList.push_back(std::make_pair(prefix + "Volume density approx. enclosing ellipsoid", vd_aee));
  featureList.push_back(std::make_pair(prefix + "Surface density approx. enclosing ellipsoid", ad_aee));
  featureList.push_back(std::make_pair(prefix + "Volume density approx. minimum volume enclosing ellipsoid", meshVolume / vol_mvee));
  featureList.push_back(std::make_pair(prefix + "Surface density approx. minimum volume enclosing ellipsoid", meshSurf / surf_mvee));
  featureList.push_back(std::make_pair(prefix + "Volume density convex hull", vd_ch));
  featureList.push_back(std::make_pair(prefix + "Surface density convex hull", ad_ch));

  return featureList;
}

mitk::GIFVolumetricDensityStatistics::GIFVolumetricDensityStatistics()
{
  SetLongName("volume-density");
  SetShortName("volden");
  SetFeatureClassName("Morphological Density");
}

mitk::GIFVolumetricDensityStatistics::FeatureNameListType mitk::GIFVolumetricDensityStatistics::GetFeatureNames()
{
  FeatureNameListType featureList;
  return featureList;
}

void mitk::GIFVolumetricDensityStatistics::AddArguments(mitkCommandLineParser &parser)
{
  std::string name = GetOptionPrefix();

  parser.addArgument(GetLongName(), name, mitkCommandLineParser::Bool, "Use Volume-Density Statistic", "calculates volume density based features", us::Any());
}

void
mitk::GIFVolumetricDensityStatistics::CalculateFeaturesUsingParameters(const Image::Pointer & feature, const Image::Pointer &mask, const Image::Pointer &, FeatureListType &featureList)
{
  auto parsedArgs = GetParameter();
  if (parsedArgs.count(GetLongName()))
  {
    MITK_INFO << "Start calculating volumetric density features ....";
    auto localResults = this->CalculateFeatures(feature, mask);
    featureList.insert(featureList.end(), localResults.begin(), localResults.end());
    MITK_INFO << "Finished calculating volumetric density features....";
  }
}


#include <NTL/ZZX.h>

#include <NTL/new.h>

NTL_START_IMPL

const ZZX& ZZX::zero()
{
   static ZZX z;
   return z;
}

void conv(ZZ_pX& x, const ZZX& a)
{
   conv(x.rep, a.rep);
   x.normalize();
}

void conv(ZZX& x, const ZZ_pX& a)
{
   conv(x.rep, a.rep);
   x.normalize();
}

istream& operator>>(istream& s, ZZX& x)
{
   s >> x.rep;
   x.normalize();
   return s;
}

ostream& operator<<(ostream& s, const ZZX& a)
{
   return s << a.rep;
}

void ZZX::normalize()
{
   long n;
   const ZZ* p;

   n = rep.length();
   if (n == 0) return;
   p = rep.elts() + n;
   while (n > 0 && IsZero(*--p)) {
      n--;
   }
   rep.SetLength(n);
}

long IsZero(const ZZX& a)
{
   return a.rep.length() == 0;
}

long IsOne(const ZZX& a)
{
    return a.rep.length() == 1 && IsOne(a.rep[0]);
}

long operator==(const ZZX& a, const ZZX& b)
{
   long i, n;
   const ZZ *ap, *bp;

   n = a.rep.length();
   if (n != b.rep.length()) return 0;

   ap = a.rep.elts();
   bp = b.rep.elts();

   for (i = 0; i < n; i++)
      if (ap[i] != bp[i]) return 0;

   return 1;
}

long operator==(const ZZX& a, long b)
{
   if (b == 0)
      return IsZero(a);

   if (deg(a) != 0)
      return 0;

   return a.rep[0] == b;
}

long operator==(const ZZX& a, const ZZ& b)
{
   if (IsZero(b))
      return IsZero(a);

   if (deg(a) != 0)
      return 0;

   return a.rep[0] == b;
}

void GetCoeff(ZZ& x, const ZZX& a, long i)
{
   if (i < 0 || i > deg(a))
      clear(x);
   else
      x = a.rep[i];
}

void SetCoeff(ZZX& x, long i, const ZZ& a)
{
   long j, m;

   if (i < 0)
      Error("SetCoeff: negative index");

   if (NTL_OVERFLOW(i, 1, 0))
      Error("overflow in SetCoeff");

   m = deg(x);

   if (i > m && IsZero(a)) return;

   if (i > m) {
      /* careful: a may alias a coefficient of x */

      long alloc = x.rep.allocated();

      if (alloc > 0 && i >= alloc) {
         ZZ aa = a;
         x.rep.SetLength(i+1);
         x.rep[i] = aa;
      }
      else {
         x.rep.SetLength(i+1);
         x.rep[i] = a;
      }

      for (j = m+1; j < i; j++)
         clear(x.rep[j]);
   }
   else
      x.rep[i] = a;

   x.normalize();
}

void SetCoeff(ZZX& x, long i)
{
   long j, m;

   if (i < 0)
      Error("coefficient index out of range");

   if (NTL_OVERFLOW(i, 1, 0))
      Error("overflow in SetCoeff");

   m = deg(x);

   if (i > m) {
      x.rep.SetLength(i+1);
      for (j = m+1; j < i; j++)
         clear(x.rep[j]);
   }
   set(x.rep[i]);
   x.normalize();
}

void SetX(ZZX& x)
{
   clear(x);
   SetCoeff(x, 1);
}

long IsX(const ZZX& a)
{
   return deg(a) == 1 && IsOne(LeadCoeff(a)) && IsZero(ConstTerm(a));
}

const ZZ& coeff(const ZZX& a, long i)
{
   if (i < 0 || i > deg(a))
      return ZZ::zero();
   else
      return a.rep[i];
}

const ZZ& LeadCoeff(const ZZX& a)
{
   if (IsZero(a))
      return ZZ::zero();
   else
      return a.rep[deg(a)];
}

const ZZ& ConstTerm(const ZZX& a)
{
   if (IsZero(a))
      return ZZ::zero();
   else
      return a.rep[0];
}

void conv(ZZX& x, const ZZ& a)
{
   if (IsZero(a))
      x.rep.SetLength(0);
   else {
      x.rep.SetLength(1);
      x.rep[0] = a;
   }
}

void conv(ZZX& x, long a)
{
   if (a == 0)
      x.rep.SetLength(0);
   else {
      x.rep.SetLength(1);
      conv(x.rep[0], a);
   }
}

void conv(ZZX& x, const vec_ZZ& a)
{
   x.rep = a;
   x.normalize();
}

void add(ZZX& x, const ZZX& a, const ZZX& b)
{
   long da = deg(a);
   long db = deg(b);
   long minab = min(da, db);
   long maxab = max(da, db);
   x.rep.SetLength(maxab+1);

   long i;
   const ZZ *ap, *bp;
   ZZ* xp;

   for (i = minab+1, ap = a.rep.elts(), bp = b.rep.elts(), xp = x.rep.elts();
        i; i--, ap++, bp++, xp++)
      add(*xp, (*ap), (*bp));

   if (da > minab && &x != &a)
      for (i = da-minab; i; i--, xp++, ap++)
         *xp = *ap;
   else if (db > minab && &x != &b)
      for (i = db-minab; i; i--, xp++, bp++)
         *xp = *bp;
   else
      x.normalize();
}

void add(ZZX& x, const ZZX& a, const ZZ& b)
{
   long n = a.rep.length();
   if (n == 0) {
      conv(x, b);
   }
   else if (&x == &a) {
      add(x.rep[0], a.rep[0], b);
      x.normalize();
   }
   else if (x.rep.MaxLength() == 0) {
      x = a;
      add(x.rep[0], a.rep[0], b);
      x.normalize();
   }
   else {
      // ugly...b could alias a coeff of x

      ZZ *xp = x.rep.elts();
      add(xp[0], a.rep[0], b);
      x.rep.SetLength(n);
      xp = x.rep.elts();
      const ZZ *ap = a.rep.elts();
      long i;
      for (i = 1; i < n; i++)
         xp[i] = ap[i];
      x.normalize();
   }
}

void add(ZZX& x, const ZZX& a, long b)
{
   if (a.rep.length() == 0) {
      conv(x, b);
   }
   else {
      if (&x != &a) x = a;
      add(x.rep[0], x.rep[0], b);
      x.normalize();
   }
}

void sub(ZZX& x, const ZZX& a, const ZZX& b)
{
   long da = deg(a);
   long db = deg(b);
   long minab = min(da, db);
   long maxab = max(da, db);
   x.rep.SetLength(maxab+1);

   long i;
   const ZZ *ap, *bp;
   ZZ* xp;

   for (i = minab+1, ap = a.rep.elts(), bp = b.rep.elts(), xp = x.rep.elts();
        i; i--, ap++, bp++, xp++)
      sub(*xp, (*ap), (*bp));

   if (da > minab && &x != &a)
      for (i = da-minab; i; i--, xp++, ap++)
         *xp = *ap;
   else if (db > minab)
      for (i = db-minab; i; i--, xp++, bp++)
         negate(*xp, *bp);
   else
      x.normalize();

}

void sub(ZZX& x, const ZZX& a, const ZZ& b)
{
   long n = a.rep.length();
   if (n == 0) {
      conv(x, b);
      negate(x, x);
   }
   else if (&x == &a) {
      sub(x.rep[0], a.rep[0], b);
      x.normalize();
   }
   else if (x.rep.MaxLength() == 0) {
      x = a;
      sub(x.rep[0], a.rep[0], b);
      x.normalize();
   }
   else {
      // ugly...b could alias a coeff of x

      ZZ *xp = x.rep.elts();
      sub(xp[0], a.rep[0], b);
      x.rep.SetLength(n);
      xp = x.rep.elts();
      const ZZ *ap = a.rep.elts();
      long i;
      for (i = 1; i < n; i++)
         xp[i] = ap[i];
      x.normalize();
   }
}

void sub(ZZX& x, const ZZX& a, long b)
{
   if (b == 0) {
      x = a;
      return;
   }

   if (a.rep.length() == 0) {
      x.rep.SetLength(1);
      conv(x.rep[0], b);
      negate(x.rep[0], x.rep[0]);
   }
   else {
      if (&x != &a) x = a;
      sub(x.rep[0], x.rep[0], b);
   }
   x.normalize();
}

void sub(ZZX& x, long a, const ZZX& b)
{
   negate(x, b);
   add(x, x, a);
}

void sub(ZZX& x, const ZZ& b, const ZZX& a)
{
   long n = a.rep.length();
   if (n == 0) {
      conv(x, b);
   }
   else if (x.rep.MaxLength() == 0) {
      negate(x, a);
      add(x.rep[0], a.rep[0], b);
      x.normalize();
   }
   else {
      // ugly...b could alias a coeff of x

      ZZ *xp = x.rep.elts();
      sub(xp[0], b, a.rep[0]);
      x.rep.SetLength(n);
      xp = x.rep.elts();
      const ZZ *ap = a.rep.elts();
      long i;
      for (i = 1; i < n; i++)
         negate(xp[i], ap[i]);
      x.normalize();
   }
}

void negate(ZZX& x, const ZZX& a)
{
   long n = a.rep.length();
   x.rep.SetLength(n);

   const ZZ* ap = a.rep.elts();
   ZZ* xp = x.rep.elts();
   long i;

   for (i = n; i; i--, ap++, xp++)
      negate((*xp), (*ap));

}

long MaxBits(const ZZX& f)
{
   long i, m;
   m = 0;

   for (i = 0; i <= deg(f); i++) {
      m = max(m, NumBits(f.rep[i]));
   }

   return m;
}

void PlainMul(ZZX& x, const ZZX& a, const ZZX& b)
{
   if (&a == &b) {
      PlainSqr(x, a);
      return;
   }

   long da = deg(a);
   long db = deg(b);

   if (da < 0 || db < 0) {
      clear(x);
      return;
   }

   long d = da+db;

   const ZZ *ap, *bp;
   ZZ *xp;

   ZZX la, lb;

   if (&x == &a) {
      la = a;
      ap = la.rep.elts();
   }
   else
      ap = a.rep.elts();

   if (&x == &b) {
      lb = b;
      bp = lb.rep.elts();
   }
   else
      bp = b.rep.elts();

   x.rep.SetLength(d+1);

   xp = x.rep.elts();

   long i, j, jmin, jmax;
   ZZ t, accum;

   for (i = 0; i <= d; i++) {
      jmin = max(0, i-db);
      jmax = min(da, i);
      clear(accum);
      for (j = jmin; j <= jmax; j++) {
	 mul(t, ap[j], bp[i-j]);
	 add(accum, accum, t);
      }
      xp[i] = accum;
   }
   x.normalize();
}

void PlainSqr(ZZX& x, const ZZX& a)
{
   long da = deg(a);

   if (da < 0) {
      clear(x);
      return;
   }

   long d = 2*da;

   const ZZ *ap;
   ZZ *xp;

   ZZX la;

   if (&x == &a) {
      la = a;
      ap = la.rep.elts();
   }
   else
      ap = a.rep.elts();

   x.rep.SetLength(d+1);

   xp = x.rep.elts();

   long i, j, jmin, jmax;
   long m, m2;
   ZZ t, accum;

   for (i = 0; i <= d; i++) {
      jmin = max(0, i-da);
      jmax = min(da, i);
      m = jmax - jmin + 1;
      m2 = m >> 1;
      jmax = jmin + m2 - 1;
      clear(accum);
      for (j = jmin; j <= jmax; j++) {
	 mul(t, ap[j], ap[i-j]);
	 add(accum, accum, t);
      }
      add(accum, accum, accum);
      if (m & 1) {
	 sqr(t, ap[jmax + 1]);
	 add(accum, accum, t);
      }

      xp[i] = accum;
   }

   x.normalize();
}

static
void PlainMul(ZZ *xp, const ZZ *ap, long sa, const ZZ *bp, long sb)
{
   if (sa == 0 || sb == 0) return;

   long sx = sa+sb-1;

   long i, j, jmin, jmax;
   static ZZ t, accum;

   for (i = 0; i < sx; i++) {
      jmin = max(0, i-sb+1);
      jmax = min(sa-1, i);
      clear(accum);
      for (j = jmin; j <= jmax; j++) {
         mul(t, ap[j], bp[i-j]);
         add(accum, accum, t);
      }
      xp[i] = accum;
   }
}

static
void KarFold(ZZ *T, const ZZ *b, long sb, long hsa)
{
   long m = sb - hsa;
   long i;

   for (i = 0; i < m; i++)
      add(T[i], b[i], b[hsa+i]);

   for (i = m; i < hsa; i++)
      T[i] = b[i];
}

static
void KarSub(ZZ *T, const ZZ *b, long sb)
{
   long i;

   for (i = 0; i < sb; i++)
      sub(T[i], T[i], b[i]);
}

static
void KarAdd(ZZ *T, const ZZ *b, long sb)
{
   long i;

   for (i = 0; i < sb; i++)
      add(T[i], T[i], b[i]);
}

static
void KarFix(ZZ *c, const ZZ *b, long sb, long hsa)
{
   long i;

   for (i = 0; i < hsa; i++)
      c[i] = b[i];

   for (i = hsa; i < sb; i++)
      add(c[i], c[i], b[i]);
}

static void PlainMul1(ZZ *xp, const ZZ *ap, long sa, const ZZ& b)
{
   long i;

   for (i = 0; i < sa; i++)
      mul(xp[i], ap[i], b);
}

static
void KarMul(ZZ *c, const ZZ *a,
            long sa, const ZZ *b, long sb, ZZ *stk)
{
   if (sa < sb) {
      { long t = sa; sa = sb; sb = t; }
      { const ZZ *t = a; a = b; b = t; }
   }

   if (sb == 1) {
      if (sa == 1)
         mul(*c, *a, *b);
      else
         PlainMul1(c, a, sa, *b);

      return;
   }

   if (sb == 2 && sa == 2) {
      mul(c[0], a[0], b[0]);
      mul(c[2], a[1], b[1]);
      add(stk[0], a[0], a[1]);
      add(stk[1], b[0], b[1]);
      mul(c[1], stk[0], stk[1]);
      sub(c[1], c[1], c[0]);
      sub(c[1], c[1], c[2]);

      return;

   }

   long hsa = (sa + 1) >> 1;

   if (hsa < sb) {
      /* normal case */

      long hsa2 = hsa << 1;

      ZZ *T1, *T2, *T3;

      T1 = stk; stk += hsa;
      T2 = stk; stk += hsa;
      T3 = stk; stk += hsa2 - 1;

      /* compute T1 = a_lo + a_hi */

      KarFold(T1, a, sa, hsa);

      /* compute T2 = b_lo + b_hi */

      KarFold(T2, b, sb, hsa);

      /* recursively compute T3 = T1 * T2 */

      KarMul(T3, T1, hsa, T2, hsa, stk);

      /* recursively compute a_hi * b_hi into high part of c */
      /* and subtract from T3 */

      KarMul(c + hsa2, a+hsa, sa-hsa, b+hsa, sb-hsa, stk);
      KarSub(T3, c + hsa2, sa + sb - hsa2 - 1);

      /* recursively compute a_lo*b_lo into low part of c */
      /* and subtract from T3 */

      KarMul(c, a, hsa, b, hsa, stk);
      KarSub(T3, c, hsa2 - 1);

      clear(c[hsa2 - 1]);

      /* finally, add T3 * X^{hsa} to c */

      KarAdd(c+hsa, T3, hsa2-1);
   }
   else {
      /* degenerate case */

      ZZ *T;

      T = stk; stk += hsa + sb - 1;

      /* recursively compute b*a_hi into high part of c */

      KarMul(c + hsa, a + hsa, sa - hsa, b, sb, stk);

      /* recursively compute b*a_lo into T */

      KarMul(T, a, hsa, b, sb, stk);

      KarFix(c, T, hsa + sb - 1, hsa);
   }
}

void KarMul(ZZX& c, const ZZX& a, const ZZX& b)
{
   if (IsZero(a) || IsZero(b)) {
      clear(c);
      return;
   }

   if (&a == &b) {
      KarSqr(c, a);
      return;
   }

   vec_ZZ mem;

   const ZZ *ap, *bp;
   ZZ *cp;

   long sa = a.rep.length();
   long sb = b.rep.length();

   if (&a == &c) {
      mem = a.rep;
      ap = mem.elts();
   }
   else
      ap = a.rep.elts();

   if (&b == &c) {
      mem = b.rep;
      bp = mem.elts();
   }
   else
      bp = b.rep.elts();

   c.rep.SetLength(sa+sb-1);
   cp = c.rep.elts();

   long maxa, maxb, xover;

   maxa = MaxBits(a);
   maxb = MaxBits(b);
   xover = 2;

   if (sa < xover || sb < xover)
      PlainMul(cp, ap, sa, bp, sb);
   else {
      /* karatsuba */

      long n, hn, sp, depth;

      n = max(sa, sb);
      sp = 0;
      depth = 0;
      do {
         hn = (n+1) >> 1;
         sp += (hn << 2) - 1;
         n = hn;
         depth++;
      } while (n >= xover);

      ZZVec stk;
      stk.SetSize(sp,
         ((maxa + maxb + NumBits(min(sa, sb)) + 2*depth + 10)
          + NTL_ZZ_NBITS-1)/NTL_ZZ_NBITS);

      KarMul(cp, ap, sa, bp, sb, stk.elts());
   }

   c.normalize();
}

void PlainSqr(ZZ* xp, const ZZ* ap, long sa)
{
   if (sa == 0) return;

   long da = sa-1;
   long d = 2*da;

   long i, j, jmin, jmax;
   long m, m2;
   static ZZ t, accum;

   for (i = 0; i <= d; i++) {
      jmin = max(0, i-da);
      jmax = min(da, i);
      m = jmax - jmin + 1;
      m2 = m >> 1;
      jmax = jmin + m2 - 1;
      clear(accum);
      for (j = jmin; j <= jmax; j++) {
	 mul(t, ap[j], ap[i-j]);
	 add(accum, accum, t);
      }
      add(accum, accum, accum);
      if (m & 1) {
	 sqr(t, ap[jmax + 1]);
	 add(accum, accum, t);
      }

      xp[i] = accum;
   }
}

static
void KarSqr(ZZ *c, const ZZ *a, long sa, ZZ *stk)
{
   if (sa == 1) {
      sqr(*c, *a);
      return;
   }

   if (sa == 2) {
      sqr(c[0], a[0]);
      sqr(c[2], a[1]);
      mul(c[1], a[0], a[1]);
      add(c[1], c[1], c[1]);

      return;
   }

   if (sa == 3) {
      sqr(c[0], a[0]);
      mul(c[1], a[0], a[1]);
      add(c[1], c[1], c[1]);
      sqr(stk[0], a[1]);
      mul(c[2], a[0], a[2]);
      add(c[2], c[2], c[2]);
      add(c[2], c[2], stk[0]);
      mul(c[3], a[1], a[2]);
      add(c[3], c[3], c[3]);
      sqr(c[4], a[2]);

      return;

   }

   long hsa = (sa + 1) >> 1;
   long hsa2 = hsa << 1;

   ZZ *T1, *T2;

   T1 = stk; stk += hsa;
   T2 = stk; stk += hsa2-1;

   KarFold(T1, a, sa, hsa);
   KarSqr(T2, T1, hsa, stk);

   KarSqr(c + hsa2, a+hsa, sa-hsa, stk);
   KarSub(T2, c + hsa2, sa + sa - hsa2 - 1);

   KarSqr(c, a, hsa, stk);
   KarSub(T2, c, hsa2 - 1);

   clear(c[hsa2 - 1]);

   KarAdd(c+hsa, T2, hsa2-1);
}

void KarSqr(ZZX& c, const ZZX& a)
{
   if (IsZero(a)) {
      clear(c);
      return;
   }

   vec_ZZ mem;

   const ZZ *ap;
   ZZ *cp;

   long sa = a.rep.length();

   if (&a == &c) {
      mem = a.rep;
      ap = mem.elts();
   }
   else
      ap = a.rep.elts();

   c.rep.SetLength(sa+sa-1);
   cp = c.rep.elts();

   long maxa, xover;

   maxa = MaxBits(a);

   xover = 2;

   if (sa < xover)
      PlainSqr(cp, ap, sa);
   else {
      /* karatsuba */

      long n, hn, sp, depth;

      n = sa;
      sp = 0;
      depth = 0;
      do {
         hn = (n+1) >> 1;
         sp += hn+hn+hn - 1;
         n = hn;
         depth++;
      } while (n >= xover);

      ZZVec stk;
      stk.SetSize(sp,
         ((2*maxa + NumBits(sa) + 2*depth + 10)
          + NTL_ZZ_NBITS-1)/NTL_ZZ_NBITS);

      KarSqr(cp, ap, sa, stk.elts());
   }

   c.normalize();
}

NTL_END_IMPL


#ifndef PIDCONTROLLER_H
#define PIDCONTROLLER_H

#include <Eigen/Core>
#include <cslibs_navigation_utilities/Stopwatch.h>

/**
 * @brief Multidimensional PID controller.
 *
 * Implements a basic PID controller that is capable to work with multidimensional input (that
 * is it can actually controll multiple variables at onces).
 *
 * The first template parameter defines the number of variables. Parameters, errors, etc are
 * passed via Eigen::Vectors.
 * For the special case of dim=1, there are overloaded methods, that take floats/doubles.
 *
 * The second template Parameter can be used to change precision from float to double.
 *
 * Usage
 * -----
 *
 * One dimensional case:
 *
 *     PidController<1> pid_1d(1, 0.1, 0, 0.5); // P = 1, I = 0.1, D = 0, update interval = 0.5s
 *     float u;
 *     pid_1d.execute(error, &u);
 *
 * Multidimensional case:
 *
 *     typedef PidController<3>::Vector Vec3;
 *
 *     PidController<3> pid_3d(Vec3(1, 1,   0.5),
 *                             Vec3(0, 0.1, 0.2),
 *                             Vec3(0, 0,   0.1),
 *                             0.5);
 *     Vec3 u;
 *     pid_3d.execute(error, &u);
 *
 *     // --> u[0] is controlled with P = 1, I,D = 0
 *     //     u[1] is controlled with P = 1, I = 0.1, D = 0
 *     //     u[2] is controlled with P = 0.5, I = 0.2, D = 0.1
 */
template<unsigned int dim=1, typename real_t=float>
class PidController
{
public:
    typedef Eigen::Matrix<real_t, dim, 1> Vector;
    typedef Eigen::Matrix<real_t, dim, dim> Matrix;

    PidController():
        PidController(Vector::Zero(), Vector::Zero(), Vector::Zero(), 0)
    {}

    PidController(Vector K_p, Vector K_i, Vector K_d, real_t dt):
        K_p_(K_p.asDiagonal()),
        K_i_(K_i.asDiagonal()),
        K_d_(K_d.asDiagonal()),
        dt_(dt)
    {
        reset();
    }

    PidController(real_t K_p, real_t K_i, real_t K_d, real_t dt):
        PidController(Matrix(Matrix::Constant(K_p)),
                      Matrix(Matrix::Constant(K_i)),
                      Matrix(Matrix::Constant(K_d)),
                      dt)
    {
        // This constructor could also be disabled by setting the type of `K_p` to
        // `typename std::enable_if<dim == 1, real_t>::type`, but I found the error output of a
        // static_assert more helpful.
        static_assert(dim == 1,
                      "The scalar constuctor can only be used for one-dimensional PID"
                      " controllers");
    }

    //! Reset the controller.
    void reset()
    {
        previous_error_ = Vector::Zero();
        integral_ = Vector::Zero();
        timer_.restart();
    }

    /**
     * @brief Execute one control cycle if enough time has elapsed since last call.
     * @param error The error between desired and real value (`error = x_desired - x_real`)
     * @param u_out Output. The control command. Only set if method returns true!
     * @return True if enougth time has elapsed and u_out is set. False if not.
     */
    bool execute(const Vector &error, Vector *u_out)
    {
        /*
         * Pseudocode from Wikipedia:
         *
         * previous_error = 0
         * integral = 0
         * start:
         *   error = setpoint - measured_value
         *   integral = integral + error*dt
         *   derivative = (error - previous_error)/dt
         *   output = Kp*error + Ki*integral + Kd*derivative
         *   previous_error = error
         *   wait(dt)
         *   goto start
         *
         */

        // Only compute new u, if at least dt_ time has elapsed.
        double elapsed = timer_.msElapsed() / 1000.0;
        if (elapsed > 0.0 && elapsed >= dt_) {
            // Use the exact elapsed time (`elapsed`) instead of `dt_`
            integral_ += error * elapsed;
            Vector derivative = (error - previous_error_) / elapsed;
            *u_out = K_p_ * error + K_i_ * integral_ + K_d_ * derivative;
            previous_error_ = error;

            timer_.restart();
            return true;
        } else {
            return false;
        }
    }

    //! Overloaded method for the one-dimensional case
    bool execute(real_t error, real_t *u_out) {
        static_assert(dim == 1,
                      "The scalar execute() can only be used for one-dimensional PID"
                      " controllers");

        Vector vec_u;
        bool res = execute(Vector::Constant(error), &vec_u);
        *u_out = vec_u[0];
        return res;
    }

private:
    Matrix K_p_;
    Matrix K_i_;
    Matrix K_d_;
    real_t dt_;

    Vector previous_error_;
    Vector integral_;

    Stopwatch timer_;
};

#endif // PIDCONTROLLER_H


// Copyright Robert Eisele 2017
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include "readCSV.h"

Eigen::MatrixXd readCSV(std::string file, int rows, int cols) {

  std::ifstream in(file);

  std::string line;

  int row = 0;
  int col = 0;

  Eigen::MatrixXd res = Eigen::MatrixXd(rows, cols);

  if (in.is_open()) {

    while (std::getline(in, line)) {

      char *ptr = (char *) line.c_str();
      int len = line.length();

      col = 0;

      char *start = ptr;
      for (int i = 0; i < len; i++) {

        if (ptr[i] == ',') {
          res(row, col++) = atof(start);
          start = ptr + i + 1;
        }
      }
      res(row, col) = atof(start);

      row++;
    }

    in.close();
  }
  return res;
}


#include "Viewer2.h"
#include "Helpers/mesh_importer.h"
#include <QOpenGLFunctions>
#include <QtOpenGL>
#include <vector>
#include <Eigen>
#include <QOpenGLFunctions>
#include <QtOpenGL>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <math.h>
#include <QRadioButton>
#include <QLineEdit>

//shader ID
enum {VIEW2};

Viewer2::Viewer2(const QGLFormat &glf, QWidget *parent) : HW(glf, parent) , mesh_importer(this)
{
    // init vars
    setFocusPolicy(Qt::StrongFocus);
}

void Viewer2::initializeGL()
{
    // initialize GL function resolution for current context
    initializeGLFunctions();

    // init vertex and fragment shaders
    initShaders();

    glEnable(GL_NORMALIZE);
    glEnable(GL_CULL_FACE);
    glCullFace(GL_BACK);
    glEnable(GL_DEPTH_TEST);

    // initialize vertex buffer and write positions to vertex shader
    initVertexBuffer();

    // init state variables
    glClearColor(1.0, 1.0, 1.0, 1.0);	// set background color
    glColor3f   (1.0, 1.0, 0.0);		// set foreground color

    if(!timer.isCreated()){
        timer.create();
    }

    LoD=4;

}

void Viewer2::resizeGL(int w, int h)
{

    if (h == 0) h = 1;
    width_ = w;
    height_ = h;

    camera_.SetViewport(0, 0, w, h);
    camera_.SetProjection(kFieldOfView, kZNear, kZFar);

    m_winW = w;
    m_winH = h;

    return;
}

void Viewer2::paintGL()
{

     if(timer.isCreated()) timer.begin();

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // use glsl program
    glUseProgram(m_program[VIEW2].programId());

    // pass the following parameters to vertex the shader:
    // projection matrix, modelview matrix, theta, and twist
    camera_.SetViewport();

    Eigen::Matrix4f projection = camera_.SetProjection();
    Eigen::Matrix4f view = camera_.SetView();
    Eigen::Matrix4f model = camera_.SetModel();

    Eigen::Matrix4f t = view * model;
    Eigen::Matrix3f normal;
    for (int i = 0; i < 3; ++i)
      for (int j = 0; j < 3; ++j) normal(i, j) = t(i, j);

    normal = normal.inverse().transpose();

    glUniformMatrix4fv(const_uniform[VIEW2][CONST_UNIFORM_PROJ], 1, GL_FALSE, projection.data());
    glUniformMatrix4fv(const_uniform[VIEW2][CONST_UNIFORM_MODEL], 1, GL_FALSE, model.data());
    glUniformMatrix4fv(const_uniform[VIEW2][CONST_UNIFORM_VIEW], 1, GL_FALSE, view.data());
    glUniformMatrix3fv(const_uniform[VIEW2][CONST_UNIFORM_NORMAL], 1, GL_FALSE, normal.data());

    const Eigen::Affine3f piRotation(Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0.0, 1.0, 0.0)));
    model = piRotation* model ;
    // draw triangles
    if (mesh_ == nullptr) return;

    //////////
    glBindVertexArray = (_glBindVertexArray) QGLWidget::context()->getProcAddress("glBindVertexArray");
    switch(LoD){

    case 0:
        glBindVertexArray(VAO_ID_16);
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID_16);
        glDrawElements(GL_TRIANGLES,mesh_->faces_16.size(), GL_UNSIGNED_INT, 0);
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
        glBindVertexArray(0);
        break;
    case 1:
        glBindVertexArray(VAO_ID_32);
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID_32);
        glDrawElements(GL_TRIANGLES,mesh_->faces_32.size(), GL_UNSIGNED_INT, 0);
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
        glBindVertexArray(0);
        break;
    case 2:
        glBindVertexArray(VAO_ID_64);
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID_64);
        glDrawElements(GL_TRIANGLES,mesh_->faces_64.size(), GL_UNSIGNED_INT, 0);
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
        glBindVertexArray(0);
        break;
    case 3:
        glBindVertexArray(VAO_ID_128);
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID_128);
        glDrawElements(GL_TRIANGLES,mesh_->faces_128.size(), GL_UNSIGNED_INT, 0);
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
        glBindVertexArray(0);
        break;
    case 4:
        glBindVertexArray(VAO_ID);
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID);
        glDrawElements(GL_TRIANGLES,mesh_->faces_.size(), GL_UNSIGNED_INT, 0);
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
        glBindVertexArray(0);
        break;

    }

    // terminate program; rendering is done
    glUseProgram(0);

    if(timer.isCreated()){
         timer.end();
         resTime = timer.waitForResult();
         if(resTime==0){return;}

         fps = 1.0/((float)resTime/1000000000.0);

         std::stringstream s_fps;
         s_fps << std::fixed << std::setprecision(1);
         s_fps << fps << " FPS" << std::ends;
         s_fps << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
         fpsString = QString::fromStdString(s_fps.str());
         m_fpsCount->setText(fpsString);
     }

}

QGroupBox* Viewer2::controlPanel()
{
    // init group box
    QGroupBox *groupBox = HW::controlPanel();
    groupBox->setStyleSheet(GroupBoxStyle);
    mesh_importer.AddImportExportPLY(groupBox);
    chooseLOD(groupBox);
    showFramerate(groupBox);
    //HW::AddImportExportPLY(groupBox);

    return(groupBox);
}

void Viewer2::reset()
{
    mesh_ = nullptr;	// recompute geometry
    initVertexBuffer();
    // draw
    updateGL();
}

void Viewer2::initShaders()
{
    // init uniforms hash table based on uniform variable names and location IDs
    UniformMap uniforms;

    // compile shader, bind attribute vars, link shader, and initialize uniform var table
    initShader(VIEW2, QString(":/SRGGE/Shaders/s1s2s3.vert"), QString(":/SRGGE/Shaders/s1s2s3.frag"), uniforms);
}

void Viewer2::initVertexBuffer()
{

    if (mesh_ == nullptr) return;

    initBuffer_original();
    initBuffer_16();
    initBuffer_32();
    initBuffer_64();
    initBuffer_128();

   updateGL();

}

void Viewer2::importModel(){
    QString filename;

      filename = QFileDialog::getOpenFileName(this, tr("Load model"), "./",
                                              tr("PLY Files ( *.ply )"));
      if (!filename.isNull()) {
          if(!mesh_importer.LoadModel(filename))
            QMessageBox::warning(this, tr("Error"),
                               tr("The file could not be opened"));
      }

}
void Viewer2::exportModel(){
    QString filename;

      filename = QFileDialog::getSaveFileName(this, tr("Export model"), "untitled.ply",
                                              tr("PLY Files ( *.ply )"));
      if (!filename.isNull()) {
          if(!mesh_importer.SaveModel(filename,LoD,*mesh_))
            QMessageBox::warning(this, tr("Error"),
                               tr("The file could not be saved"));
      }

}

void Viewer2::showFramerate(QGroupBox *groupBox){
    m_fpsCount = new QLabel("0");
    QFrame* line = new QFrame();

    line->setFrameShape(QFrame::HLine);
    QLabel *title = new QLabel(" FPS:");

    auto layout = dynamic_cast<QGridLayout*>(groupBox->layout());

    int row = layout->rowCount() + 1;
    int headRow = row;
    row++;
    layout->addWidget(title,row,0);
 //   row++;
    layout->addWidget(m_fpsCount,row, 1);

    layout->addWidget(line,headRow, 0, 1 ,layout->columnCount());

    groupBox->setLayout(layout);
}

void Viewer2::chooseLOD(QGroupBox *groupBox){

    QLabel *levelDetail= new QLabel(tr("Level of detail:"));

    QSpinBox *levelDetailBox = new QSpinBox;
    levelDetailBox->setRange(0, 4);
    levelDetailBox->setSingleStep(1);
    levelDetailBox->setValue(4);

    QFrame* line = new QFrame();
    line->setFrameShape(QFrame::HLine);
    QLabel *title = new QLabel("Vertex Clustering:");

    connect(levelDetailBox, SIGNAL(valueChanged(int)),this, SLOT(setLOD(int)));
    auto layout = dynamic_cast<QGridLayout*>(groupBox->layout());

    int row = layout->rowCount() + 1;
    int headRow = row;
    row++;
    layout->addWidget(title,row,0);
    row++;
    layout->addWidget(levelDetail,row, 0);
    layout->addWidget(levelDetailBox,row, 1);

    layout->addWidget(line,headRow, 0, 1 ,layout->columnCount());

    groupBox->setLayout(layout);

}
void Viewer2::setLOD(int b){
    LoD=b;

    switch(LoD){
    case 0:
        mesh_importer.updateNumVert(mesh_->vertices_16.size()/3,(int)mesh_->faces_16.size() / 3);
        break;
    case 1:
        mesh_importer.updateNumVert(mesh_->vertices_32.size()/3,(int)mesh_->faces_32.size() / 3);
        break;
    case 2:
        mesh_importer.updateNumVert(mesh_->vertices_64.size()/3,(int)mesh_->faces_64.size() / 3);
        break;
    case 3:
        mesh_importer.updateNumVert(mesh_->vertices_128.size()/3,(int)mesh_->faces_128.size() / 3);
        break;
    case 4:
        mesh_importer.updateNumVert((int)mesh_->vertices_.size() / 3,(int)mesh_->faces_.size() / 3);
        break;
    }

    //mesh_importer.updateNumVert((int) N*N*mesh_->vertices_.size() / 3,(int)N*N*mesh_->faces_.size() / 3);
    initVertexBuffer();
}

//***********INIT VERTEX FOR ALL LEVEL OF DETAIL***************//
void Viewer2::initBuffer_original(){

    glGenBuffers(1, &vertexBufferVBO_ID);
    glGenBuffers(1, &normalBufferVBO_ID);
    glGenBuffers(1, &facesBufferVBO_ID);

    std::vector<GLfloat> data;
    data.clear();

    for(int i =0;i<(int)mesh_->vertices_.size()/3;i++){

        data.push_back(mesh_->vertices_[3*i]);
        data.push_back(mesh_->vertices_[3*i+1]);
        data.push_back(mesh_->vertices_[3*i+2]);
        data.push_back(mesh_->normals_[3*i]);
        data.push_back(mesh_->normals_[3*i+1]);
        data.push_back(mesh_->normals_[3*i+2]);
    }

    glGenVertexArrays = (_glGenVertexArrays) QGLWidget::context()->getProcAddress("glGenVertexArrays");
    glBindVertexArray = (_glBindVertexArray) QGLWidget::context()->getProcAddress("glBindVertexArray");

    glGenVertexArrays(1, &VAO_ID);

    glBindVertexArray(VAO_ID);
    glGenBuffers(1, &VBO_ID);
    glBindBuffer(GL_ARRAY_BUFFER, VBO_ID);

     glBufferData(GL_ARRAY_BUFFER,2*mesh_->vertices_.size() * sizeof(GLfloat), &data[0], GL_STATIC_DRAW);
     glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float), 0);
     glEnableVertexAttribArray(0);
     glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float),(void*)(3*sizeof(float)));

     glEnableVertexAttribArray(1);

     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID);
     glBufferData(GL_ELEMENT_ARRAY_BUFFER, mesh_->faces_.size() * sizeof(int), &mesh_->faces_[0], GL_STATIC_DRAW);
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
     glBindVertexArray(0);
}
void Viewer2::initBuffer_16(){
    glGenBuffers(1, &vertexBufferVBO_ID_16);
    glGenBuffers(1, &normalBufferVBO_ID_16);
    glGenBuffers(1, &facesBufferVBO_ID_16);

    std::vector<GLfloat> data;
    data.clear();

    for(int i =0;i<(int)mesh_->vertices_16.size()/3;i++){

        data.push_back(mesh_->vertices_16[3*i]);
        data.push_back(mesh_->vertices_16[3*i+1]);
        data.push_back(mesh_->vertices_16[3*i+2]);
        data.push_back(mesh_->normals_16[3*i]);
        data.push_back(mesh_->normals_16[3*i+1]);
        data.push_back(mesh_->normals_16[3*i+2]);
    }

    glGenVertexArrays = (_glGenVertexArrays) QGLWidget::context()->getProcAddress("glGenVertexArrays");
    glBindVertexArray = (_glBindVertexArray) QGLWidget::context()->getProcAddress("glBindVertexArray");

    glGenVertexArrays(1, &VAO_ID_16);

    glBindVertexArray(VAO_ID_16);
    glGenBuffers(1, &VBO_ID_16);
    glBindBuffer(GL_ARRAY_BUFFER, VBO_ID_16);

     glBufferData(GL_ARRAY_BUFFER,2*mesh_->vertices_16.size() * sizeof(GLfloat), &data[0], GL_STATIC_DRAW);
     glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float), 0);
     glEnableVertexAttribArray(0);
     glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float),(void*)(3*sizeof(float)));

     glEnableVertexAttribArray(1);

     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID_16);
     glBufferData(GL_ELEMENT_ARRAY_BUFFER, mesh_->faces_16.size() * sizeof(int), &mesh_->faces_16[0], GL_STATIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
glBindVertexArray(0);
}
void Viewer2::initBuffer_32(){
    glGenBuffers(1, &vertexBufferVBO_ID_32);
    glGenBuffers(1, &normalBufferVBO_ID_32);
    glGenBuffers(1, &facesBufferVBO_ID_32);

    std::vector<GLfloat> data;
    data.clear();

    for(int i =0;i<(int)mesh_->vertices_32.size()/3;i++){

        data.push_back(mesh_->vertices_32[3*i]);
        data.push_back(mesh_->vertices_32[3*i+1]);
        data.push_back(mesh_->vertices_32[3*i+2]);
        data.push_back(mesh_->normals_32[3*i]);
        data.push_back(mesh_->normals_32[3*i+1]);
        data.push_back(mesh_->normals_32[3*i+2]);
    }

    glGenVertexArrays = (_glGenVertexArrays) QGLWidget::context()->getProcAddress("glGenVertexArrays");
    glBindVertexArray = (_glBindVertexArray) QGLWidget::context()->getProcAddress("glBindVertexArray");

    glGenVertexArrays(1, &VAO_ID_32);

    glBindVertexArray(VAO_ID_32);
    glGenBuffers(1, &VBO_ID_32);
    glBindBuffer(GL_ARRAY_BUFFER, VBO_ID_32);

     glBufferData(GL_ARRAY_BUFFER,2*mesh_->vertices_32.size() * sizeof(GLfloat), &data[0], GL_STATIC_DRAW);
     glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float), 0);
     glEnableVertexAttribArray(0);
     glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float),(void*)(3*sizeof(float)));

     glEnableVertexAttribArray(1);

     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID_32);
     glBufferData(GL_ELEMENT_ARRAY_BUFFER, mesh_->faces_32.size() * sizeof(int), &mesh_->faces_32[0], GL_STATIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
glBindVertexArray(0);
}
void Viewer2::initBuffer_64(){
    glGenBuffers(1, &vertexBufferVBO_ID_64);
    glGenBuffers(1, &normalBufferVBO_ID_64);
    glGenBuffers(1, &facesBufferVBO_ID_64);

    std::vector<GLfloat> data;
    data.clear();

    for(int i =0;i<(int)mesh_->vertices_64.size()/3;i++){

        data.push_back(mesh_->vertices_64[3*i]);
        data.push_back(mesh_->vertices_64[3*i+1]);
        data.push_back(mesh_->vertices_64[3*i+2]);
        data.push_back(mesh_->normals_64[3*i]);
        data.push_back(mesh_->normals_64[3*i+1]);
        data.push_back(mesh_->normals_64[3*i+2]);
    }

    glGenVertexArrays = (_glGenVertexArrays) QGLWidget::context()->getProcAddress("glGenVertexArrays");
    glBindVertexArray = (_glBindVertexArray) QGLWidget::context()->getProcAddress("glBindVertexArray");

    glGenVertexArrays(1, &VAO_ID_64);

    glBindVertexArray(VAO_ID_64);
    glGenBuffers(1, &VBO_ID_64);
    glBindBuffer(GL_ARRAY_BUFFER, VBO_ID_64);

     glBufferData(GL_ARRAY_BUFFER,2*mesh_->vertices_64.size() * sizeof(GLfloat), &data[0], GL_STATIC_DRAW);
     glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float), 0);
     glEnableVertexAttribArray(0);
     glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float),(void*)(3*sizeof(float)));

     glEnableVertexAttribArray(1);

     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID_64);
     glBufferData(GL_ELEMENT_ARRAY_BUFFER, mesh_->faces_64.size() * sizeof(int), &mesh_->faces_64[0], GL_STATIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
glBindVertexArray(0);
}
void Viewer2::initBuffer_128(){
    glGenBuffers(1, &vertexBufferVBO_ID_128);
    glGenBuffers(1, &normalBufferVBO_ID_128);
    glGenBuffers(1, &facesBufferVBO_ID_128);

    std::vector<GLfloat> data;
    data.clear();

    for(int i =0;i<(int)mesh_->vertices_128.size()/3;i++){

        data.push_back(mesh_->vertices_128[3*i]);
        data.push_back(mesh_->vertices_128[3*i+1]);
        data.push_back(mesh_->vertices_128[3*i+2]);
        data.push_back(mesh_->normals_128[3*i]);
        data.push_back(mesh_->normals_128[3*i+1]);
        data.push_back(mesh_->normals_128[3*i+2]);
    }

    glGenVertexArrays = (_glGenVertexArrays) QGLWidget::context()->getProcAddress("glGenVertexArrays");
    glBindVertexArray = (_glBindVertexArray) QGLWidget::context()->getProcAddress("glBindVertexArray");

    glGenVertexArrays(1, &VAO_ID_128);

    glBindVertexArray(VAO_ID_128);
    glGenBuffers(1, &VBO_ID_128);
    glBindBuffer(GL_ARRAY_BUFFER, VBO_ID_128);

     glBufferData(GL_ARRAY_BUFFER,2*mesh_->vertices_128.size() * sizeof(GLfloat), &data[0], GL_STATIC_DRAW);
     glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float), 0);
     glEnableVertexAttribArray(0);
     glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float),(void*)(3*sizeof(float)));

     glEnableVertexAttribArray(1);

     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID_128);
     glBufferData(GL_ELEMENT_ARRAY_BUFFER, mesh_->faces_128.size() * sizeof(int), &mesh_->faces_128[0], GL_STATIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
glBindVertexArray(0);
}


#pragma once
#include "sico/types/orientations.hpp"

#ifdef SICO_USE_EIGEN
#pragma warning(push)
#pragma warning(disable : 4365)
#pragma warning(disable : 4464)
#pragma warning(disable : 4514)
#pragma warning(disable : 4710)
#pragma warning(disable : 4625)
#pragma warning(disable : 4626)
#pragma warning(disable : 4820)
#pragma warning(disable : 5026)
#pragma warning(disable : 5027)
#pragma warning(disable : 5045)
#define EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
#include <Eigen/Dense>
#pragma warning(pop)

namespace sico {

using vector3d   = Eigen::Vector3d;
using matrix3d   = Eigen::Matrix3d;
using quaternion = Eigen::Quaterniond;

} // namespace sico

#else
#include "sico/types/angles.hpp"

#include <array>
#include <type_traits>

#pragma warning(push)
#pragma warning(disable : 5045) // spectre
namespace sico {

using scalar_t = double;

template<size_t S>
using arrayd = std::array<scalar_t, S>;

// Basic linear algebra based on arrays of doubles
using vector3d   = arrayd<3>;
using matrix3d   = arrayd<9>;
using quaternion = arrayd<4>;

// Beneric operations
template<size_t S>
arrayd<S> operator+(arrayd<S> const& s1, arrayd<S> const& s2)
{
    arrayd<S> s;
    for (size_t i = 0; i < S; i++)
        s[i] = s1[i] + s2[i];
    return s;
}
template<size_t S>
arrayd<S>& operator+=(arrayd<S>& s1, arrayd<S> const& s2)
{
    for (size_t i = 0; i < S; i++)
        s1[i] += s2[i];
    return s1;
}
template<size_t S>
arrayd<S>& operator+=(arrayd<S>& s1, scalar_t const v)
{
    for (size_t i = 0; i < S; i++)
        s1[i] += v;
    return s1;
}
template<size_t S>
arrayd<S> operator-(arrayd<S> const& s1)
{
    arrayd<S> s;
    for (size_t i = 0; i < S; i++)
        s[i] = -s1[i];
    return s;
}
template<size_t S>
arrayd<S> operator*(arrayd<S> const& s1, scalar_t const t)
{
    arrayd<S> s;
    for (size_t i = 0; i < S; i++)
        s[i] = s1[i] * t;
    return s;
}
template<size_t S>
arrayd<S>& operator*=(arrayd<S>& s, scalar_t const t)
{
    for (size_t i = 0; i < S; i++)
        s[i] *= t;
    return s;
}
template<size_t S>
bool operator==(arrayd<S> const& s1, arrayd<S> const& s2)
{
    bool r = true;
    for (size_t i = 0; i < S; i++)
        r = r && s1[i] == s2[i];
    return r;
}
template<size_t S>
scalar_t dot(arrayd<S> const& s1, arrayd<S> const& s2)
{
    scalar_t d = 0;
    for (size_t i = 0; i < S; i++) {
        d += s1[i] * s2[i];
    }
    return d;
}

// generic operations, based on previously defined ones
template<size_t S>
arrayd<S> operator-(arrayd<S> const& s1, arrayd<S> const& s2)
{
    return s1 + -s2;
}
template<size_t S>
arrayd<S>& operator-=(arrayd<S>& s1, arrayd<S> const& s2)
{
    s1 += -s2;
    return s1;
}
template<size_t S>
arrayd<S>& operator-=(arrayd<S>& s1, scalar_t const v)
{
    s1 += -v;
    return s1;
}
template<size_t S>
arrayd<S> operator/(arrayd<S> const& s1, scalar_t const d)
{
    return s1 * (1.0 / d);
}
template<size_t S>
arrayd<S>& operator/=(arrayd<S>& s1, scalar_t const d)
{
    s1 *= 1.0 / d;
    return s1;
}
template<size_t S>
bool operator!=(arrayd<S> const& s1, arrayd<S> const& s2)
{
    return !(s1 == s2);
}
template<size_t S>
scalar_t length_squared(arrayd<S> const& s)
{
    return dot(s, s);
}
template<size_t S>
scalar_t length(arrayd<S> const& s)
{
    return sqrt(length_squared(s));
}
template<size_t S>
arrayd<S>& normalize(arrayd<S>& s)
{
    auto const l = length(s);
    if (l < 0.0001)
        return s;
    s /= l;
    return s;
}
template<size_t S>
arrayd<S> normalized(arrayd<S> s)
{
    normalize(s);
    return s;
}

// quaternion operations
inline quaternion operator*(quaternion const& q1, quaternion const& q2)
{
    return { q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],
             q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],
             q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],
             q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] };
}
inline quaternion conj(quaternion const& q)
{
    return { -q[0], -q[1], -q[2], q[3] };
}
inline vector3d operator*(quaternion const& rot, vector3d const& v)
{
    quaternion const pure { v[0], v[1], v[2], 0.0 };
    quaternion const rot_conj = conj(rot);
    quaternion const temp { pure[3] * rot_conj[0] + pure[0] * rot_conj[3] + pure[1] * rot_conj[2]
                                - pure[2] * rot_conj[1],
                            pure[3] * rot_conj[1] + pure[1] * rot_conj[3] + pure[2] * rot_conj[0]
                                - pure[0] * rot_conj[2],
                            pure[3] * rot_conj[2] + pure[2] * rot_conj[3] + pure[0] * rot_conj[1]
                                - pure[1] * rot_conj[0],
                            pure[3] * rot_conj[3] - pure[0] * rot_conj[0] - pure[1] * rot_conj[1]
                                - pure[2] * rot_conj[2] };

    return { rot[3] * temp[0] + rot[0] * temp[3] + rot[1] * temp[2] - rot[2] * temp[1],
             rot[3] * temp[1] + rot[1] * temp[3] + rot[2] * temp[0] - rot[0] * temp[2],
             rot[3] * temp[2] + rot[2] * temp[3] + rot[0] * temp[1] - rot[1] * temp[0] };
}
inline quaternion from_euler(scalar_t xRot, scalar_t yRot, scalar_t zRot)
{
    using std::cos;
    using std::sin;
    auto const       hX = xRot * 0.5;
    auto const       hY = yRot * 0.5;
    auto const       hZ = zRot * 0.5;
    quaternion const qx { sin(hX), 0.0, 0.0, cos(hX) };
    quaternion const qy { 0.0, sin(hY), 0.0, cos(hY) };
    quaternion const qz { 0.0, 0.0, sin(hZ), cos(hZ) };
    quaternion       result = qx * qy * qz;
    normalize(result);
    return result;
}

// matrix operations
inline double& mat(matrix3d& m, size_t row, size_t column)
{
    return m[row * 3 + column];
}
inline double mat(matrix3d const& m, size_t row, size_t column)
{
    return m[row * 3 + column];
}
inline vector3d operator*(matrix3d const& m, vector3d const& v)
{
    vector3d res;
    res.fill(0);
    for (unsigned r = 0; r < 3; ++r)
        for (unsigned c = 0; c < 3; ++c)
            res[r] += mat(m, r, c) * v[c];

    return res;
}
inline matrix3d rot_mat(quaternion const& q)
{
    auto const xs = q[0] + q[0];
    auto const ys = q[1] + q[1];
    auto const zs = q[2] + q[2];
    auto const xx = q[0] * xs;
    auto const xy = q[0] * ys;
    auto const xz = q[0] * zs;
    auto const yy = q[1] * ys;
    auto const yz = q[1] * zs;
    auto const zz = q[2] * zs;
    auto const wx = q[3] * xs;
    auto const wy = q[3] * ys;
    auto const wz = q[3] * zs;

    matrix3d m;
    mat(m, 0, 0) = double(1.0) - (yy + zz);
    mat(m, 1, 0) = xy + wz;
    mat(m, 2, 0) = xz - wy;
    mat(m, 0, 1) = xy - wz;
    mat(m, 1, 1) = double(1.0) - (xx + zz);
    mat(m, 2, 1) = yz + wx;
    mat(m, 0, 2) = xz + wy;
    mat(m, 1, 2) = yz - wx;
    mat(m, 2, 2) = double(1.0) - (xx + yy);
    return m;
}
inline void to_euler(matrix3d const& m, scalar_t& x, scalar_t& y, scalar_t& z)
{
    y = asin(mat(m, 0, 2));
    if (y < SICO_PI2) {
        if (y > -SICO_PI2) {
            x = atan2(-mat(m, 1, 2), mat(m, 2, 2));
            z = atan2(-mat(m, 0, 1), mat(m, 0, 0));
        }
        else {
            x = -atan2(mat(m, 1, 0), mat(m, 1, 1));
            z = 0;
        }
    }
    else {
        x = atan2(mat(m, 1, 0), mat(m, 1, 1));
        z = 0;
    }
}

} // namespace sico
#pragma warning(pop)
#endif

namespace sico {
template<typename Ref>
inline quaternion make_quat(quat<Ref> const& q)
{
#ifdef SICO_USE_EIGEN
    return quaternion(q.w, q.x, q.y, q.z);
#else
    return quaternion { q.x, q.y, q.z, q.w };
#endif
}

template<typename Ref>
inline quat<Ref> make_quat(quaternion const& q)
{
#ifdef SICO_USE_EIGEN
    return quat<Ref> { q.x(), q.y(), q.z(), q.w() };
#else
    return quat<Ref> { q[0], q[1], q[2], q[3] };
#endif
}
} // namespace sico

//
// Simulation-Coordinates library
// Author F.Jacomme
// MIT Licensed
//


#ifndef TYPES_HPP
#define TYPES_HPP

#include <thrust/device_ptr.h>
#include <thrust/device_malloc.h>
#include <thrust/device_free.h>
#include <thrust/device_vector.h>
#include <Eigen/Dense>

// ----------------------------------------
// ------- Some useful typedefs for vectors and matrices

typedef double REAL;
typedef int obs_kind;
typedef typename Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> mat;
typedef typename Eigen::Ref<Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > mat_ref;
typedef typename Eigen::Matrix<REAL, Eigen::Dynamic, 1> vec;
typedef typename Eigen::Ref<Eigen::Matrix<REAL, Eigen::Dynamic, 1> > vec_ref;

/// Extract the raw pointer from a device vector
template <typename T>
T *dev_ptr(thrust::device_vector<T>& dv)
{
    return dv.data().get();
}

// ----------------------------------------
// enum to allow the python code to select the type of "nugget"
enum nugget_type {NUG_ADAPTIVE, NUG_FIT, NUG_FIXED};

// enum to allow python code to select Kernel function
enum kernel_type {SQUARED_EXPONENTIAL, MATERN52};

#endif


// (1) schedule is n+1. in FloatingLeg <ql/cashflows/cashflowvectors.hpp> and #include <ql/utilities/vectors.hpp>
// It takes index min(i,n), where n = schedule.size()-1
// that is, schedule has start date

// (2) composition. Eventually it needs to separate payment schedule from fixing schedule.
// but a quick fix is to add composition factor, e.g. 1mth/3mth would be 3, and a high-frequency schedule
// In class IborLeg, add composition to IborCoupon and overload indexFixing() to composition

#ifndef qlex_ibor_basis_swap_hpp
#define qlex_ibor_basis_swap_hpp

#include <ql/instruments/swap.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/schedule.hpp>
#include <boost/optional.hpp>

using namespace QuantLib;

namespace QLExtension {

	class QuantLib::IborIndex;
	class QuantLib::OvernightIndex;

	//! Ibor basis swap, e.g. 1m vs 3m
	class GenericSwap : public Swap {
	public:
		enum Type { Receiver = -1, Payer = 1 };		// rec/pay first leg
		// fixed rate with notional and notionals
		GenericSwap(
			Type type,
			Real nominal,
			const Schedule& firstLegResetSchedule,			// fixed leg
			const Schedule& firstLegPaymenttSchedule,			// fixed leg
			const Rate firstLegRate,					// fixed rate
			const DayCounter& firstLegDayCount,
			const Schedule& secondLegResetSchedule,
			const Schedule& secondLegPaymentSchedule,
			const boost::shared_ptr<IborIndex>& secondLegIndex,
			const DayCounter& secondLegDayCount,
			Spread secondLegSpread = 0.0,
			bool arithmeticAveragedCoupon = false,
			boost::optional<BusinessDayConvention> firstLegConvention =
			boost::none,
			boost::optional<BusinessDayConvention> secondLegConvention =
			boost::none);
		GenericSwap(
			Type type,
			std::vector<Real> firstLegNominals,
			const Schedule& firstLegResetSchedule,			// fixed leg
			const Schedule& firstLegPaymenttSchedule,
			const Rate firstLegRate,					// fixed rate
			const DayCounter& firstLegDayCount,
			std::vector<Real> secoondLegNominals,
			const Schedule& secondLegResetSchedule,
			const Schedule& secondLegPaymentSchedule,
			const boost::shared_ptr<IborIndex>& secondLegIndex,
			const DayCounter& secondLegDayCount,
			Spread secondLegSpread = 0.0,
			bool arithmeticAveragedCoupon = false,
			boost::optional<BusinessDayConvention> firstLegConvention =
			boost::none,
			boost::optional<BusinessDayConvention> secondLegConvention =
			boost::none);

		// basis leg with notional and notionals
		GenericSwap(
			Type type,
			Real nominal,
			const Schedule& firstLegResetSchedule,			// LIB3M
			const Schedule& firstLegPaymenttSchedule,
			const boost::shared_ptr<IborIndex>& firstLegIndex,
			const DayCounter& firstLegDayCount,
			const Schedule& secondLegResetSchedule,
			const Schedule& secondLegPaymentSchedule,
			const boost::shared_ptr<IborIndex>& secondLegIndex,
			const DayCounter& secondLegDayCount,
			Spread firstLegSpread = 0.0,
			Spread secondLegSpread = 0.0,
			bool arithmeticAveragedCoupon = false,
			boost::optional<BusinessDayConvention> firstLegConvention =
			boost::none,
			boost::optional<BusinessDayConvention> secondLegConvention =
			boost::none);
		GenericSwap(
			Type type,
			std::vector<Real> firstLegNominals,
			const Schedule& firstLegResetSchedule,			// LIB3M
			const Schedule& firstLegPaymenttSchedule,
			const boost::shared_ptr<IborIndex>& firstLegIndex,
			const DayCounter& firstLegDayCount,
			std::vector<Real> secondLegNominals,
			const Schedule& secondLegResetSchedule,
			const Schedule& secondLegPaymentSchedule,
			const boost::shared_ptr<IborIndex>& secondLegIndex,
			const DayCounter& secondLegDayCount,
			Spread firstLegSpread = 0.0,
			Spread secondLegSpread = 0.0,
			bool arithmeticAveragedCoupon = false,
			boost::optional<BusinessDayConvention> firstLegConvention =
			boost::none,
			boost::optional<BusinessDayConvention> secondLegConvention =
			boost::none);
		//! \name Inspectors
		//@{
		Type type() const { return type_; }
		Real nominal() const;

		std::vector<Real> firstLegNominals() const { return firstLegNominals_; }
		const Schedule& firstLegResetSchedule() const { return firstLegResetSchedule_; }
		const Schedule& firstLegPaymentSchedule() const { return firstLegPaymentSchedule_; }
		const boost::shared_ptr<IborIndex>& firstLegIndex() const { return firstLegIndex_; }
		const DayCounter& firstLegDayCount() const { return firstLegDayCount_; }
		const BusinessDayConvention& firstLegConvention() const { return firstLegConvention_; }

		std::vector<Real> secondLegNominals() const { return secondLegNominals_; }
		const Schedule& secondLegResetSchedule() { return secondLegResetSchedule_; }
		const Schedule& secondLegPaymentSchedule() { return secondLegPaymentSchedule_; }
		const boost::shared_ptr<IborIndex>& secondLegIndex() { return secondLegIndex_; }
		const DayCounter& secondLegDayCount() const { return secondLegDayCount_; }
		const BusinessDayConvention& secondLegConvention() const { return secondLegConvention_; }

		Rate fixedRate() { return firstLegRate_; }
		Spread firstLegSpread() { return firstLegSpread_; }
		Spread secondLegSpread() { return secondLegSpread_; }
		const Leg& firstLeg() const { return legs_[0]; }
		const Leg& secondLeg() const { return legs_[1]; }
		const std::vector<std::string>& firstLegInfo() const { return firstLegInfo_; }
		const std::vector<std::string>& secondLegInfo() const { return secondLegInfo_; }
		//@}

		//! \name Results
		//@{
		Real firstLegBPS() const;
		Real firstLegNPV() const;
		Real fairRate() const;

		Real secondLegBPS() const;
		Real secondLegNPV() const;
		Spread fairSpread() const;
		//@}
	private:
		void initialize(bool isfixed=false);
		Type type_;
		bool arithmeticAveragedCoupon_;

		std::vector<Real> firstLegNominals_;
		Schedule firstLegResetSchedule_;
		Schedule firstLegPaymentSchedule_;
		boost::shared_ptr<IborIndex> firstLegIndex_;
		DayCounter firstLegDayCount_;
		BusinessDayConvention firstLegConvention_;
		Rate firstLegRate_;
		Spread firstLegSpread_;

		std::vector<Real> secondLegNominals_;
		Schedule secondLegResetSchedule_;
		Schedule secondLegPaymentSchedule_;
		boost::shared_ptr<IborIndex> secondLegIndex_;
		boost::shared_ptr<OvernightIndex> overnightIndex_;
		DayCounter secondLegDayCount_;
		BusinessDayConvention secondLegConvention_;
		Spread secondLegSpread_;

		std::vector<std::string> firstLegInfo_;
		std::vector<std::string> secondLegInfo_;
	};

	// inline
	inline Real GenericSwap::nominal() const {
		QL_REQUIRE(firstLegNominals_.size() == 1, "varying nominals");
		return firstLegNominals_[0];
	}
}

#endif


#ifndef HPP_PIECE_NODE
#define HPP_PIECE_NODE

#include <boost/serialization/access.hpp>

#include <vector>
#include <limits>
#include <utility>

#include "piece.hpp"

class piece_node {
    enum direction {
        LEFT, RIGHT
    };

    size_t             d_point;
    piece_node*        d_left;
    piece_node*        d_right;
    std::vector<piece> d_pieces;

public:
    explicit piece_node(const std::vector<piece>& pieces)
        : d_point(0)
        , d_left(nullptr)
        , d_right(nullptr)
        , d_pieces()
    {
        d_point = median(pieces);
        std::vector<piece> to_left, to_right;

        for (const auto& p : pieces) {
            if (p.end() < d_point) {
                to_left.push_back(p);
            }
            else if (p.start() > d_point) {
                to_right.push_back(p);
            }
            else {
                d_pieces.push_back(p);
            }
        }

        if (to_left.size() > 0) {
            d_left = new piece_node(to_left);
        }
        if (to_right.size() > 0) {
            d_right = new piece_node(to_right);
        }
    }

    size_t median(const std::vector<piece>& pieces) const {
        auto start = std::numeric_limits<size_t>::max();
        auto end   = std::numeric_limits<size_t>::max();

        for (const auto& p : pieces) {
            auto cur_start = p.start();
            auto cur_end = p.end();

            if (start == std::numeric_limits<size_t>::max() || cur_start < start) {
                start = cur_start;
            }
            if (end == std::numeric_limits<size_t>::max() || cur_end > end) {
                end = cur_end;
            }
        }

        return (start + end) / 2;
    }

    std::vector<piece> overlaps(const piece& p) {
        std::vector<piece> overlaps;

        if (d_point < p.start()) {
            add_overlaps(p, overlaps, find_node_overlaps(d_right, p));
            add_overlaps(p, overlaps, overlaps_right(p));
        }
        else if (d_point > p.end()) {
            add_overlaps(p, overlaps, find_node_overlaps(d_left, p));
            add_overlaps(p, overlaps, overlaps_left(p));
        }
        else {
            add_overlaps(p, overlaps, d_pieces);
            add_overlaps(p, overlaps, find_node_overlaps(d_left, p));
            add_overlaps(p, overlaps, find_node_overlaps(d_right, p));
        }

        return std::vector<piece>(overlaps);
    }

protected:
    void add_overlaps(const piece& p, std::vector<piece>& overlaps, std::vector<piece> new_overlaps) const {
        for (const auto& cur : new_overlaps) {
            if (cur != p) {
                overlaps.push_back(cur);
            }
        }
    }

    std::vector<piece> overlaps_left(const piece& p) const {
        return std::vector<piece>(overlaps_direction(p, LEFT));
    }

    std::vector<piece> overlaps_right(const piece& p) const {
        return std::vector<piece>(overlaps_direction(p, RIGHT));
    }

    std::vector<piece> overlaps_direction(const piece& p, direction d) const {
        std::vector<piece> overlaps;

        for (const auto& cur : d_pieces) {
            switch (d) {
                case LEFT:
                    if (cur.start() <= p.end()) {
                        overlaps.push_back(cur);
                    }
                    break;

                case RIGHT:
                    if (cur.end() >= p.start()) {
                        overlaps.push_back(cur);
                    }
                    break;
            }
        }

        return std::vector<piece>(overlaps);
    }

    std::vector<piece> find_node_overlaps(piece_node* piece_node, const piece& p) const {
        if (piece_node) {
            return std::vector<piece>(piece_node->overlaps(p));
        }

        return std::vector<piece>();
    }

private:
    friend class boost::serialization::access;

    template <typename Archive>
    void serialize(Archive &ar, const unsigned int version) {
        ar & d_point;
        ar & d_left;
        ar & d_right;
        ar & d_pieces;
    }
};

#endif


/**
 * \file      accelerometer-gyrometer-magnetometer.hpp
 * \author    Mehdi Benallegue, Joseph Mirabel
 * \date      2015
 * \brief     Implements the accelerometer-gyrometer-magnetometer inertial
 *            measurement unit
 *
 *
 */

#ifndef SIMULATIONACCELEROMETERGYROMETERMAGNETOMETERSENSORHPP
#define SIMULATIONACCELEROMETERGYROMETERMAGNETOMETERSENSORHPP

#include <boost/assert.hpp>
#include <Eigen/Core>

#include <state-observation/api.h>
#include <state-observation/sensors-simulation/algebraic-sensor.hpp>
#include <state-observation/sensors-simulation/algorithm/linear-acceleration.hpp>
#include <state-observation/sensors-simulation/algorithm/magnetic-field.hpp>
#include <state-observation/sensors-simulation/algorithm/rotation-velocity.hpp>

namespace stateObservation
{
/**
 * \class  AccelerometerGyrometerMagnetometer
 * \brief  Implements the accelerometer-gyrometer-magnetometer measurements
 *
 *
 *
 * \details
 *
 */

class STATE_OBSERVATION_DLLAPI AccelerometerGyrometerMagnetometer : public AlgebraicSensor,
                                                                    protected algorithm::LinearAcceleration,
                                                                    protected algorithm::RotationVelocity,
                                                                    protected algorithm::MagneticField
{
public:
  AccelerometerGyrometerMagnetometer();

  /// Virtual destructor
  virtual ~AccelerometerGyrometerMagnetometer() {}

  void setMatrixMode(bool matrixMode);

protected:
  /// Gets the state vector Size
  virtual Index getStateSize_() const;

  /// Gets the measurements vector size
  virtual Index getMeasurementSize_() const;

  virtual Vector computeNoiselessMeasurement_();

  Matrix3 r_;
  Vector3 acc_;
  Vector3 omega_;
  Vector3 magne_;
  Vector output_;

  bool matrixMode_;

  static const Index stateSize_ = 10;
  static const Index stateSizeMatrix_ = 15;

  static const Index measurementSize_ = 9;

  Index currentStateSize_;
};

} // namespace stateObservation

#endif // SIMULATIONACCELEROMETERGYROMETERMAGNETOMETERSENSORHPP


/**
 * @file network_util_test.cpp
 * @author Marcus Edel
 *
 * Simple tests for things in the network_util file.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/network_util.hpp>
#include <mlpack/methods/ann/layer/linear_layer.hpp>
#include <mlpack/methods/ann/layer/base_layer.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(NetworkUtilTest);

/**
 * Test the network size auxiliary function.
 */
BOOST_AUTO_TEST_CASE(NetworkSizeTest)
{
  // Create a two layer network without weights.
  BaseLayer<> baseLayer1;
  BaseLayer<> baseLayer2;
  auto noneWeightNetwork = std::tie(baseLayer1, baseLayer2);

  BOOST_REQUIRE_EQUAL(NetworkSize(noneWeightNetwork), 0);

  // Create a two layer network.
  LinearLayer<> linearLayer1(10, 10);
  LinearLayer<> linearLayer2(10, 100);

  // Reuse the layer form the first network.
  auto weightNetwork = std::tie(linearLayer1, baseLayer1, linearLayer2,
      baseLayer2);

  BOOST_REQUIRE_EQUAL(NetworkSize(weightNetwork), 1100);
}

/**
 * Test the layer size auxiliary function.
 */
BOOST_AUTO_TEST_CASE(LayerSizeTest)
{
  // Create layer without weights.
  BaseLayer<> baseLayer;
  BOOST_REQUIRE_EQUAL(LayerSize(baseLayer, baseLayer.OutputParameter()), 0);

  // Create layer with weights.
  LinearLayer<> linearLayer(10, 10);
  BOOST_REQUIRE_EQUAL(LayerSize(linearLayer,
      linearLayer.OutputParameter()), 100);
}

/**
 * Test the network input size auxiliary function.
 */
BOOST_AUTO_TEST_CASE(NetworkInputSizeTest)
{
  // Create a two layer network without weights.
  BaseLayer<> baseLayer1;
  BaseLayer<> baseLayer2;
  auto noneWeightNetwork = std::tie(baseLayer1, baseLayer2);

  BOOST_REQUIRE_EQUAL(NetworkInputSize(noneWeightNetwork), 0);

  // Create a two layer network.
  LinearLayer<> linearLayer1(5, 10);
  LinearLayer<> linearLayer2(10, 100);

  // Reuse the layer form the first network.
  auto weightNetwork = std::tie(linearLayer1, baseLayer1, linearLayer2,
      baseLayer2);

  BOOST_REQUIRE_EQUAL(NetworkInputSize(weightNetwork), 5);
}

/**
 * Test the layer input size auxiliary function.
 */
BOOST_AUTO_TEST_CASE(LayerInputSizeTest)
{
  // Create layer without weights.
  BaseLayer<> baseLayer;
  BOOST_REQUIRE_EQUAL(LayerInputSize(baseLayer,
    baseLayer.OutputParameter()), 0);

  // Create layer with weights.
  LinearLayer<> linearLayer(5, 10);
  BOOST_REQUIRE_EQUAL(LayerInputSize(linearLayer,
      linearLayer.OutputParameter()), 5);
}

/**
 * Test the network weight auxiliary function using the given initialization
 * rule.
 */
BOOST_AUTO_TEST_CASE(NetworkWeightsInitTest)
{
  // Create a two layer network.
  LinearLayer<> linearLayer1(10, 10);
  LinearLayer<> linearLayer2(10, 100);

  arma::mat parameter = arma::zeros<arma::mat>(1100, 1);

  // Create the network.
  auto network = std::tie(linearLayer1, linearLayer2);

  BOOST_REQUIRE_EQUAL(arma::accu(parameter), 0);

  RandomInitialization constantInit(1, 1);
  NetworkWeights(constantInit, parameter, network);

  BOOST_REQUIRE_EQUAL(arma::accu(linearLayer1.Weights()), 100);
  BOOST_REQUIRE_EQUAL(arma::accu(linearLayer2.Weights()), 1000);
  BOOST_REQUIRE_EQUAL(arma::accu(parameter), 1100);
}

/**
 * Test the layer weight auxiliary function using the given initialization rule.
 */
BOOST_AUTO_TEST_CASE(LayerWeightsInitTest)
{
  // Create a two layer network.
  LinearLayer<> linearLayer1(10, 10);

  arma::mat parameter = arma::zeros<arma::mat>(100, 1);

  BOOST_REQUIRE_EQUAL(arma::accu(parameter), 0);

  RandomInitialization constantInit(1, 1);
  arma::mat output;
  LayerWeights(constantInit, linearLayer1, parameter, 0, output);

  BOOST_REQUIRE_EQUAL(arma::accu(linearLayer1.Weights()), 100);
  BOOST_REQUIRE_EQUAL(arma::accu(parameter), 100);
}

BOOST_AUTO_TEST_SUITE_END();