#include <vector>
#include <iostream>
#include <iomanip>

#include <Eigen/SVD>
#include <unsupported/Eigen/MatrixFunctions>

#include "tictoc.hpp"
#include "initial_homography_estimation.hpp"

using namespace std;
using namespace Eigen;

namespace fovis
{

#define dump(var) //(cerr<<" "#var<<" =[\n"<< setprecision (12)<<var<<"];"<<endl)
Eigen::ArrayXf InitialHomographyEstimator::flattenMatrix(Eigen::MatrixXf &m)
{
  return Eigen::Map<Eigen::ArrayXf>(m.data(), m.rows() * m.cols());
}

static void
grayToEigen(const uint8_t * grayData, int width, int height, int stride,
    int downsampleFactor, Eigen::MatrixXf* result)
{
  Eigen::MatrixXf& eig_imf = *result;
  if (downsampleFactor > 0) {
    int cols = width >> downsampleFactor;
    int rows = height >> downsampleFactor;
    eig_imf = Eigen::MatrixXf::Zero(rows, cols);
    for (int y = 0; y < height; y++) {
      int ey = y >> downsampleFactor;
      for (int x = 0; x < width; x++) {
        int ex = x >> downsampleFactor;
        eig_imf(ey, ex) += grayData[y * stride + x];
      }
    }
    double pixelFactor = (1 << downsampleFactor);
    eig_imf /= pixelFactor * pixelFactor;
  }
  else {
    eig_imf.resize(height, width);
    const uint8_t* row_start = grayData;
    for(int row=0; row<height; row++) {
      for(int col=0; col<width; col++) {
        eig_imf(row, col) = row_start[col];
      }
      row_start += stride;
    }
  }
}

void InitialHomographyEstimator::setTestImage(const uint8_t * grayData, int width, int height, int stride, int downsampleFactor)
{
  grayToEigen(grayData, width, height, stride, downsampleFactor, &testImage);
}

void InitialHomographyEstimator::setTemplateImage(const uint8_t * grayData, int width, int height, int stride,
    int downsampleFactor)
{
  grayToEigen(grayData, width, height, stride, downsampleFactor, &templateImage);
  template_rows = templateImage.rows();
  template_cols = templateImage.cols();

  //compute template gradients
  Eigen::MatrixXf templateDx, templateDy;
  computeGradient(templateImage, &templateDx, &templateDy);

  templateDxRow = flattenMatrix(templateDx);
  templateDyRow = flattenMatrix(templateDy);

  //setup the utility matrices
  Eigen::MatrixXf x = VectorXf::LinSpaced(template_cols, 0, template_cols - 1).transpose().replicate(template_rows, 1);
  Eigen::MatrixXf y = VectorXf::LinSpaced(template_rows, 0, template_rows - 1).replicate(1, template_cols);
  xx = flattenMatrix(x);
  yy = flattenMatrix(y);

  templatePoints.resize(3, xx.rows());
  templatePoints.row(0) = xx;
  templatePoints.row(1) = yy;
  templatePoints.row(2).setOnes();

  xx += 1;
  yy += 1;

}

double InitialHomographyEstimator::computeError(const Eigen::MatrixXf &error)
{
  return error.norm() / sqrt((double) error.rows() * error.cols());
}

Eigen::Matrix3f InitialHomographyEstimator::track(const Eigen::Matrix3f & initH, int nIters, double * finalRMS)
{

  double minError = INFINITY;
  Eigen::Matrix3f H = initH;
  Eigen::Matrix3f bestH = H;
  int bestIter = 0;
  int lastImproved = 0;
  double lastRMS = INFINITY;

  for (int iter = 0; iter < nIters; iter++) {
    tictoc("track_iter");
    tictoc("warpPoints");
    Eigen::MatrixXf warpedHomogeneousPoints;
    warpedHomogeneousPoints = H * templatePoints;
    tictoc("warpPoints");

    tictoc("constructWarpedImage");
    warpedTestImage = constructWarpedImage(testImage, warpedHomogeneousPoints);
    tictoc("constructWarpedImage");

    errorIm = warpedTestImage - templateImage;
    Eigen::VectorXf errorRow;
    errorRow = flattenMatrix(errorIm);

    double rmsError = computeError(errorRow);

    if (rmsError < minError) {
      minError = rmsError;
      bestH = H;
      bestIter = iter;
    }

    tictoc("computeJacobian");
    tictoc("computeGradient");
    Eigen::MatrixXf warpedTestImageDx, warpedTestImageDy;
    computeGradient(warpedTestImage, &warpedTestImageDx, &warpedTestImageDy);
    tictoc("computeGradient");

    Eigen::ArrayXf warpedTestImageDxRow, warpedTestImageDyRow;
    warpedTestImageDxRow = flattenMatrix(warpedTestImageDx);
    warpedTestImageDyRow = flattenMatrix(warpedTestImageDy);

    Eigen::MatrixXf Jt = computeJacobian(templateDxRow + warpedTestImageDxRow, templateDyRow + warpedTestImageDyRow);
    tictoc("computeJacobian");

    //compute the psuedo-inverse
    tictoc("update");
    tictoc("svd_pinv");
    Eigen::JacobiSVD<MatrixXf> svd(Jt, ComputeThinU | ComputeThinV);
    Eigen::VectorXf sigma = svd.singularValues();
    Eigen::MatrixXf U = svd.matrixU();
    Eigen::MatrixXf V = svd.matrixV();
    int r = 0;
    for (r = 0; r < sigma.rows(); r++) { //singular values are in decreasing order
      if (sigma(r) < 1e-7) //TODO:better way to get the tolerance?
        break;
      else
        sigma(r) = 1.0 / sigma(r);
    }
    Eigen::MatrixXf Jt_plus;
    if (r == 0)
      Jt_plus = Eigen::MatrixXf::Zero(Jt.cols(), Jt.rows());
    else {
      Jt_plus = V.block(0, 0, V.rows(), r) * sigma.head(r).asDiagonal() * U.block(0, 0, U.rows(), r).transpose();
    }
    tictoc("svd_pinv");

    // this doesn't seem to work :-/
    //    tictoc("manual_pinv");
    //    Eigen::Matrix3f JtT_Jt = Jt.transpose() * Jt;
    //    Eigen::MatrixXf Jt_plus = JtT_Jt.inverse() * Jt;
    //    tictoc("manual_pinv");

    Eigen::VectorXf lie_d = -2 * Jt_plus * errorRow;
    tictoc("update");

    tictoc("lieToH");
    H = H * lieToH(lie_d);
    tictoc("lieToH");

    if (rmsError < lastRMS)
      lastImproved = iter;

    tictoc("track_iter");

    //        cout << iter << ") rmsError= " << rmsError << " minError = " << minError << " d.norm() =" << lie_d.norm() << endl;
    //    exit(1);
    if (lie_d.norm() < 1e-6 || (rmsError - minError > 3 && iter - bestIter > 2) || iter - bestIter > 4 || iter
        - lastImproved > 2) {
      //      printf("breaking after %d iters\n", iter);
      break;
    }
    lastRMS = rmsError;

  }
  if (finalRMS != NULL)
    *finalRMS = minError;
  return bestH;

}

void InitialHomographyEstimator::computeGradient(const Eigen::MatrixXf &image, Eigen::MatrixXf *dxp, Eigen::MatrixXf *dyp)
{
  Eigen::MatrixXf & dx = *dxp;
  Eigen::MatrixXf & dy = *dyp;
  dx = Eigen::MatrixXf::Zero(image.rows(), image.cols());
  dy = Eigen::MatrixXf::Zero(image.rows(), image.cols());

  dx.block(0, 1, dx.rows(), dx.cols() - 2) = image.block(0, 2, dx.rows(), dx.cols() - 2) - image.block(0, 0, dx.rows(),
      dx.cols() - 2);
  //handle border

  dy.block(1, 0, dy.rows() - 2, dy.cols()) = image.block(2, 0, dy.rows() - 2, dy.cols()) - image.block(0, 0, dy.rows()
      - 2, dy.cols());
  //normalize
  dx /= 2.0;
  dy /= 2.0;

  //handle borders
  dx.col(0) = image.col(1) - image.col(0);
  dx.col(image.cols() - 1) = image.col(image.cols() - 1) - image.col(image.cols() - 2);
  dy.row(0) = image.row(1) - image.row(0);
  dy.row(image.rows() - 1) = image.row(image.rows() - 1) - image.row(image.rows() - 2);

}

Eigen::MatrixXf InitialHomographyEstimator::computeJacobian(const Eigen::ArrayXf &dx, const Eigen::ArrayXf &dy) const
{
  Eigen::MatrixXf Jt(dx.rows(), 3);
  Jt.col(0) = dx;
  Jt.col(1) = dy;
  Jt.col(2) = dx * yy - dy * xx;
  return Jt;
}

Eigen::Matrix3f InitialHomographyEstimator::lieToH(const Eigen::VectorXf &lie) const
{
  //TODO: support more parameters?
  Eigen::Matrix3f M;
  M << 0, lie(2), lie(0),
      -lie(2), 0, lie(1),
       0, 0, 0;
  return M.exp();
}

Eigen::MatrixXf InitialHomographyEstimator::constructWarpedImage(const Eigen::MatrixXf &srcImage,
    const Eigen::MatrixXf &warpedPoints) const
{
  Eigen::MatrixXf warped = Eigen::MatrixXf(template_rows, template_cols);

  const double defaultValue = 128;
  //Bilinear interpolation
  for (int i = 0; i < warpedPoints.cols(); i++) {
    double val;
    Eigen::Vector2f pt = warpedPoints.col(i).head(2) / warpedPoints(2, i);
    Eigen::Vector2i fipt(floor(pt(0)), floor(pt(1)));
    Eigen::Vector2i cipt(ceil(pt(0)), ceil(pt(1)));
    if (0 <= pt(0) && pt(0) < srcImage.cols() - 1 && 0 <= pt(1) && pt(1) < srcImage.rows() - 1) {
      double x1 = pt(0) - fipt(0);
      double y1 = pt(1) - fipt(1);
      double x2 = 1 - x1;
      double y2 = 1 - y1;
      val = x2 * y2 * srcImage(fipt(1), fipt(0)) + x1 * y2 * srcImage(fipt(1), fipt(0) + 1) + x2 * y1 * srcImage(
          fipt(1) + 1, fipt(0)) + x1 * y1 * srcImage(fipt(1) + 1, fipt(0) + 1);

    }
    else if (0 <= fipt(0) && fipt(0) < srcImage.cols() && 0 <= fipt(1) && fipt(1) < srcImage.rows()) {
      val = srcImage(fipt(1), fipt(0));
    }
    else if (0 <= cipt(0) && cipt(0) < srcImage.cols() && 0 <= cipt(1) && cipt(1) < srcImage.rows()) {
      val = srcImage(cipt(1), cipt(0));
    }
    else
      val = defaultValue; //templateImage(i / template_cols, i % template_cols); //default to the same as template, so error is 0

    warped(i % template_rows, i / template_rows) = val; //Eigen is Column-major
  }
  return warped;
}

}


#include <algorithm>
#include <bitset>
#include <cassert>
#include <cmath>
#include <iostream>
#include <set>
#include <vector>

#include <future>
#include <thread>

#include <Eigen/Dense>

#include <gmpxx.h>

#include "Expression.h"

//int THREADS_NUM = std::thread::hardware_concurrency();
constexpr int THREADS_NUM = 4;

typedef Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> MatrixXq;

static std::ostream& operator<<(std::ostream & stream, mpq_class const & rop) {
  stream << rop.get_str();
  return stream;
}

Expression Expression::NumericSimplify(Indices const & indices, bool print_matrix) const {
  assert(!this->IsZero());

  std::bitset<64> bs(0);
  bs[2 * indices.size()] = true;
  size_t number_of_combinations = bs.to_ulong();

  auto thread_function = [expression=*this,noc=number_of_combinations,indices=indices] (size_t thread_counter, auto coefficient, auto sum) mutable -> void {
    std::set<size_t> coefficient_set;
    std::set<ScalarSum> sum_set;
    for (size_t counter = thread_counter * noc / THREADS_NUM; counter < (thread_counter + 1) * noc / THREADS_NUM; ++counter) {
      std::vector<size_t> numbers (indices.size());
      std::bitset<64> binary_representation(counter);
      for (size_t digit_counter = 0; digit_counter < indices.size(); ++digit_counter) {
        unsigned int binary_digit_a = static_cast<unsigned int>(binary_representation[2 * digit_counter + 1]);
        unsigned int binary_digit_b = static_cast<unsigned int>(binary_representation[2 * digit_counter]);
        *(numbers.rbegin() + digit_counter) = 2 * binary_digit_a + binary_digit_b;
      }

      ScalarSum sum_tmp = expression.EvaluateIndices(indices, numbers);
      if (!sum_tmp.IsZero()) {
        coefficient_set.merge(sum_tmp.CoefficientSet());
        sum_set.insert(sum_tmp);
      }
    }
    coefficient.set_value(coefficient_set);
    sum.set_value(sum_set);
  };

  std::vector<std::thread> t (THREADS_NUM);

  std::vector<std::promise<std::set<size_t>>> coefficients_promises (THREADS_NUM);
  std::vector<std::promise<std::set<ScalarSum>>> sums_promises (THREADS_NUM);

  std::vector<std::future<std::set<size_t>>> coefficients_futures (THREADS_NUM);
  std::vector<std::future<std::set<ScalarSum>>> sums_futures (THREADS_NUM);

  for (int thread_counter = 0; thread_counter < THREADS_NUM; ++thread_counter) {
    coefficients_futures[thread_counter] = coefficients_promises[thread_counter].get_future();
    sums_futures[thread_counter] = sums_promises[thread_counter].get_future();
    std::cout << "Launching thread " << thread_counter << std::endl;
    t[thread_counter] = std::thread(thread_function, thread_counter, std::move(coefficients_promises[thread_counter]),
                                                                     std::move(sums_promises[thread_counter]));
  }

  for (int thread_counter = 0; thread_counter < THREADS_NUM; ++thread_counter) {
    t[thread_counter].join();
    std::cout << "Joined thread " << thread_counter << std::endl;
  }

  std::set<ScalarSum> sum_set;
  std::set<size_t> coefficient_set;

  for (int thread_counter = 0; thread_counter < THREADS_NUM; ++thread_counter) {
    coefficient_set.merge(coefficients_futures.at(thread_counter).get());
    sum_set.merge(sums_futures.at(thread_counter).get());
  }

  std::map<size_t, size_t> coefficient_map;
  std::for_each(coefficient_set.begin(), coefficient_set.end(), [n=0,&coefficient_map](auto a) mutable { coefficient_map[a] = n++; });

  std::cout << "number of different (not necessarily linear independent) equations : " << sum_set.size() << std::endl;
  std::cout << "number of coefficients (e_.) in these equations                    : " << coefficient_map.size() << std::endl;
  std::cout << "Thus the problem of finding linear dependencies is equivalent to the problem of finding the null space for a " << sum_set.size() << " by " << coefficient_map.size() << " matrix." << std::endl;

  std::vector<std::vector<Rational>> matrix;
  std::for_each(sum_set.begin(), sum_set.end(), [&coefficient_map, &matrix](auto & a) {
    matrix.push_back(a.CoefficientVector(coefficient_map));
    });

  MatrixXq mq(matrix.size(), coefficient_map.size());

  for (size_t row_counter = 0; row_counter < matrix.size(); ++row_counter) {
    for (size_t column_counter = 0; column_counter < coefficient_map.size(); ++column_counter) {
      Fraction frac = matrix[row_counter][column_counter].get_fraction();
      mq(row_counter, column_counter) = mpq_class(frac.first, frac.second);
    }
  }

  Eigen::FullPivLU<MatrixXq> lu_decompq(mq);

  std::cout << "the rank of the matrix is : " << lu_decompq.rank() << std::endl;
  std::cout << "null space basis: " << std::endl;

  MatrixXq kq = lu_decompq.kernel();

  if (print_matrix) {
    std::cout << kq << std::endl;
  }

  std::set<size_t> coeff_removed;

  for (int column_counter = 0; column_counter < kq.cols(); ++column_counter) {
    for (int row_counter = kq.rows() - 1; row_counter >= 0; --row_counter) {
      if (kq(row_counter, column_counter) == 0 ) {
        continue;
      } else if (std::find(coeff_removed.begin(), coeff_removed.end(), row_counter) != coeff_removed.end()) {
        continue;
      } else {
        coeff_removed.insert(row_counter);
        break;
      }
    }
  }

  std::map<size_t, size_t> coefficient_rmap;
  std::for_each(coefficient_map.begin(), coefficient_map.end(), [&coefficient_rmap](auto a) { coefficient_rmap.insert(std::make_pair(a.second, a.first)); });

  Expression ret (*this);
  std::for_each(coeff_removed.begin(), coeff_removed.end(), [&ret, &coefficient_rmap] (auto a) { ret.EliminateVariable(coefficient_rmap.at(a)); });
  ret.CanonicalisePrefactors();
  ret.RedefineScalars();

  return Expression(ret);
}


//           Copyright Matthew Pulver 2018 - 2019.
// Distributed under the Boost Software License, Version 1.0.
//      (See accompanying file LICENSE_1_0.txt or copy at
//           https://www.boost.org/LICENSE_1_0.txt)

// THIS IS NOT MEANT TO BE COMPILED - ONLY FOR INCLUSION IN DOCUMENTATION.

#include <boost/math/differentiation/autodiff.hpp>

namespace boost { namespace math { namespace differentiation {

// Type for variables and constants.
template<typename RealType, size_t Order, size_t... Orders>
using autodiff_fvar = typename detail::nest_fvar<RealType,Order,Orders...>::type;

// Function returning a variable of differentiation.
template<typename RealType, size_t Order, size_t... Orders>
autodiff_fvar<RealType,Order,Orders...> make_fvar(const RealType& ca);

// Type of combined autodiff types.
template<typename RealType, typename... RealTypes>
using promote = typename detail::promote_args_n<RealType,RealTypes...>::type;

namespace detail {

// Single autodiff variable. Independent variables are created by nesting.
template<typename RealType, size_t Order>
class fvar
{
  public:

    // Query return value of function to get the derivatives.
    template<typename... Orders>
    get_type_at<RealType, sizeof...(Orders)-1> derivative(Orders... orders) const;

    // All of the arithmetic and comparison operators are overloaded.
    template<typename RealType2, size_t Order2>
    fvar& operator+=(const fvar<RealType2,Order2>&);

    fvar& operator+=(const root_type&);

    // ...
};

// Standard math functions are overloaded and called via argument-dependent lookup (ADL).
template<typename RealType, size_t Order>
fvar<RealType,Order> floor(const fvar<RealType,Order>&);

template<typename RealType, size_t Order>
fvar<RealType,Order> exp(const fvar<RealType,Order>&);

// ...

} // namespace detail

} } } // namespace boost::math::differentiation
/**/


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     tilt_calibration.cpp
 * \author   Collin Johnson
 *
 * Definition of calibrate_laser_pitch.
 */

#include "calibration/laser/tilt_calibration.h"
#include "math/regression.h"
#include <boost/range/iterator_range.hpp>
#include <iostream>

namespace vulcan
{
namespace calibration
{

using Measurements = std::vector<Point<double>>;

tilt_calibration_results_t calibrate_laser_tilt(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                                std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                                int lineStartIndex,
                                                int lineEndIndex,
                                                double pitchStepSize,
                                                double maxPitch,
                                                double rollStepSize,
                                                double maxRoll);
Measurements extract_line_measurements(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                       std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                       int lineStartIndex,
                                       int lineEndIndex);
Point<double> apply_tilt(const Point<double>& point, double pitch, double roll);

tilt_calibration_results_t calibrate_laser_pitch(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                                 std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                                 int lineStartIndex,
                                                 int lineEndIndex,
                                                 double minStepSize,
                                                 double maxPitch)
{
    return calibrate_laser_tilt(beginLaser, endLaser, lineStartIndex, lineEndIndex, minStepSize, maxPitch, 0.0, 0.0);
}

tilt_calibration_results_t calibrate_laser_roll(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                                std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                                int lineStartIndex,
                                                int lineEndIndex,
                                                double minStepSize,
                                                double maxRoll)
{
    return calibrate_laser_tilt(beginLaser, endLaser, lineStartIndex, lineEndIndex, 0.0, 0.0, minStepSize, maxRoll);
}

tilt_calibration_results_t calibrate_laser_tilt(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                                std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                                int lineStartIndex,
                                                int lineEndIndex,
                                                double pitchStepSize,
                                                double maxPitch,
                                                double rollStepSize,
                                                double maxRoll)
{
    assert(lineEndIndex > lineStartIndex + 5);
    assert(endLaser > beginLaser);

    std::cout << "Beginning tilt calibration for " << std::distance(beginLaser, endLaser) << " laser scans using "
              << "indices " << lineStartIndex << "->" << lineEndIndex << " for fitting the line.\n";

    tilt_calibration_results_t results;

    double minError = std::numeric_limits<double>::max();

    Measurements measurements = extract_line_measurements(beginLaser, endLaser, lineStartIndex, lineEndIndex);
    Measurements pitchMeasurements = measurements;

    auto pitchLine = math::total_least_squares(measurements.begin(), measurements.end());

    for (double pitch = 0.0, roll = 0.0; pitch >= -maxPitch && roll >= -maxRoll;
         pitch -= pitchStepSize, roll -= rollStepSize) {
        std::transform(measurements.begin(),
                       measurements.end(),
                       pitchMeasurements.begin(),
                       [pitch, roll](Point<double> point) {
                           return apply_tilt(point, pitch, roll);
                       });

        double error = 0;

        for (auto point : pitchMeasurements) {
            error += distance_to_line(point, pitchLine);
        }

        tilt_t tilt(pitch, roll);
        error /= pitchMeasurements.size();
        results.tiltErrors.emplace_back(tilt, error);

        std::cout << "Pitch: " << pitch << " Roll:" << roll << " Line: " << pitchLine << " Error:" << error << '\n';

        if (error < minError) {
            results.bestTilt = tilt;
            minError = error;
        }
    }

    std::cout << "Finished tilt calibration: Pitch:" << results.bestTilt.pitch << " Roll:" << results.bestTilt.roll
              << '\n';

    return results;
}

Measurements extract_line_measurements(std::vector<polar_laser_scan_t>::const_iterator beginLaser,
                                       std::vector<polar_laser_scan_t>::const_iterator endLaser,
                                       int lineStartIndex,
                                       int lineEndIndex)
{
    Measurements measurements;
    measurements.reserve(std::distance(beginLaser, endLaser) * (lineEndIndex - lineStartIndex));
    cartesian_laser_scan_t cartesian;

    for (auto& scan : boost::make_iterator_range(beginLaser, endLaser)) {
        polar_scan_to_cartesian_scan(scan, cartesian);

        std::copy(cartesian.scanPoints.begin() + lineStartIndex,
                  cartesian.scanPoints.begin() + lineEndIndex,
                  std::back_inserter(measurements));
    }

    std::cout << "INFO: tilt_calibration: Using " << measurements.size() << " measurements for fitting.\n";

    return measurements;
}

Point<double> apply_tilt(const Point<double>& point, double pitch, double roll)
{
    return Point<double>(point.x * std::cos(pitch), point.y * std::cos(roll));
}

}   // namespace calibration
}   // namespace vulcan


/**
 * \file PanFilter.cpp
 */

#include <ATK/Tools/PanFilter.h>

#include <cmath>
#include <complex>
#include <cstdint>

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

#include <ATK/Core/TypeTraits.h>

namespace ATK
{
  template<typename DataType_>
  PanFilter<DataType_>::PanFilter(gsl::index nb_channels)
  :Parent(nb_channels, 2 * nb_channels)
  {
  }

  template<typename DataType_>
  void PanFilter<DataType_>::set_pan_law(PAN_LAWS law)
  {
    this->law = law;
  }

  template<typename DataType_>
  typename PanFilter<DataType_>::PAN_LAWS PanFilter<DataType_>::get_pan_law() const
  {
    return law;
  }

  template<typename DataType_>
  void PanFilter<DataType_>::set_pan(double pan)
  {
    if(pan < -1 || pan > 1)
    {
      throw std::out_of_range("Pan must be a value between -1 and 1");
    }
    this->pan = pan;
  }

  template<typename DataType_>
  double PanFilter<DataType_>::get_pan() const
  {
    return pan;
  }

  template<typename DataType_>
  void PanFilter<DataType_>::process_impl(gsl::index size) const
  {
    double left_coeff = 1;
    double right_coeff = 1;

    switch(law)
    {
    case PAN_LAWS::SINCOS_0_CENTER:
      left_coeff = std::sqrt(2) * std::cos((pan + 1) / 4 * boost::math::constants::pi<double>());
      right_coeff = std::sqrt(2) * std::sin((pan + 1) / 4 * boost::math::constants::pi<double>());
      break;
    case PAN_LAWS::SINCOS_3_CENTER:
      left_coeff = std::cos((pan + 1) / 4 * boost::math::constants::pi<double>());
      right_coeff = std::sin((pan + 1) / 4 * boost::math::constants::pi<double>());
      break;
    case PAN_LAWS::SQUARE_0_CENTER:
      left_coeff = std::sqrt(2) * std::sqrt((1 - pan) / 2);
      right_coeff = std::sqrt(2) * std::sqrt((1 + pan) / 2);
      break;
    case PAN_LAWS::SQUARE_3_CENTER:
      left_coeff = std::sqrt((1 - pan) / 2);
      right_coeff = std::sqrt((1 + pan) / 2);
      break;
    case PAN_LAWS::LINEAR_TAPER:
      left_coeff = (1 - pan) / 2;
      right_coeff = (1 + pan) / 2;
      break;
    case PAN_LAWS::BALANCE:
      left_coeff = pan < 0 ? 1 : 1 - pan;
      right_coeff = pan > 0 ? 1 : 1 + pan;
      break;
    }

    assert(2 * nb_input_ports == nb_output_ports);

    for (gsl::index channel = 0; channel < nb_input_ports; ++channel)
    {
      const DataType* ATK_RESTRICT input = converted_inputs[channel];
      DataType* ATK_RESTRICT output0 = outputs[2 * channel];
      DataType* ATK_RESTRICT output1 = outputs[2 * channel + 1];
      for(gsl::index i = 0; i < size; ++i)
      {
        output0[i] = static_cast<DataType>(static_cast<typename TypeTraits<DataType>::Scalar>(left_coeff) * input[i]);
        output1[i] = static_cast<DataType>(static_cast<typename TypeTraits<DataType>::Scalar>(right_coeff) * input[i]);
      }
    }
  }

#if ATK_ENABLE_INSTANTIATION
  template class PanFilter<std::int16_t>;
  template class PanFilter<std::int32_t>;
  template class PanFilter<std::int64_t>;
  template class PanFilter<float>;
  template class PanFilter<std::complex<float>>;
  template class PanFilter<std::complex<double>>;
#endif
  template class PanFilter<double>;
}


/* Copyright (C) 5/23/18 Julian Stobbe - All Rights Reserved
 * You may use, distribute and modify this code under the
 * terms of the MIT license.
 *
 * You should have received a copy of the MIT license with
 * this file.
 */

#ifndef VALUATION_NETWORK_SIM_HPP
#define VALUATION_NETWORK_SIM_HPP

#define USE_ACTUAL_CONN 0

#include <type_traits>
#include <cmath>
#include <cstdlib>
#include <type_traits>
#include <string>
#include <unordered_map>
#include <limits>
#include <random>
#include <vector>

#include "trng/chi_square_dist.hpp"

#include "Utils.hpp"

#ifdef USE_MPI

#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>

#endif

#include "Config.hpp"
#include "StudentT.hpp"
#include "MVarNormal.hpp"
#include "Sampler.hpp"
#include "StatAcc.hpp"
#include "BlackScholesNetwork.hpp"
#include "RndGraphGen.hpp"

typedef typename std::conditional<USE_EIGEN_ACC, Eigen::MatrixXd, double>::type AccType;
typedef typename std::map<int, std::unordered_map<std::string, Eigen::MatrixXd>> ResultType;

struct SimulationParameters
{
    const long iterations, N_networks;
    NetworkType net_t;
};

constexpr int deg_of_freedom = 8;
//const std::string io_deg_str("In/Out degree distribution");

class NetwSim {
    friend class Py_ER_Net;
private:

#ifdef USE_MPI
    const boost::mpi::communicator local;
    const boost::mpi::communicator world;
    const bool isGenerator;
#endif

    trng::yarn2 gen_u;
    trng::uniform01_dist<> u_dist;
    Student_t_dist t_dist;
    Multivariate_Normal_Dist mvndist;
    //std::vector<double> dbg_weights;
    double last_weight;

    NetworkType net_t;
    int N;
    bool initialized;
    double T;              // maturity
    double r;              // interest
    double p;
    double val;
    double S0scalar;
    double sigmaScalar;
    double default_prob_scale;
    int setM;
    const double tmp[2][2] = {{1, 0},
                              {0, 1}};
    long Num_Samples;
    long Num_Networks;

    BlackScholesNetwork* bsn;
    Eigen::MatrixXd iSigma;
    Eigen::VectorXd sigma;
    Eigen::VectorXd Z;                 // Multivariate normal, used to generate lognormal assets
    Eigen::VectorXd var_h;
    Eigen::VectorXd S0;
    Eigen::VectorXd debt;
    Eigen::MatrixXd io_deg_dist;
    Eigen::MatrixXd avg_rc_sums;
    std::pair<double, double> avg_io_deg;

    std::map<int, std::unordered_map<std::string, Eigen::MatrixXd> > results;
    double connectivity;

    // last result, returned by observe
    void reset_network();

    Eigen::MatrixXd in_out_degree(Eigen::MatrixXd* M);

    void init_2DFixed_BS(const double vs01, const double vs10, const double vr01, const double vr10)
    {
        connectivity = 1;
        Eigen::MatrixXd M = Eigen::MatrixXd::Zero(2, 4);
        Utils::fixed_2d(&M, vs01, vs10, vr01, vr10);
        io_deg_dist = Utils::in_out_degree(&M);
        avg_io_deg = Utils::avg_io_deg(&M);
        avg_rc_sums = Utils::avg_row_col_sums(&M);
        bsn->re_init(M, S0, debt, sigma);
    }

    template <typename F>
    void init_BS(F gen_function) {
        if (val < 0 || val >= 1) throw std::logic_error("Row sum is not in [0,1)");
        if (p < 0 || p > 1) throw std::logic_error("p is not a probability");
        connectivity = N * p;
        Eigen::MatrixXd M = Eigen::MatrixXd::Zero(N, 2 * N);
        gen_function(&M, gen_u, p, val, setM);
        //Utils::gen_fixed_degree(&M, gen_u, p, val, which_to_set);
        io_deg_dist += Utils::in_out_degree(&M);
        avg_io_deg = Utils::avg_io_deg(&M);
        avg_rc_sums += Utils::avg_row_col_sums(&M);
        bsn->re_init(M, S0, debt, sigma);
    }

//TODO: config struct
public:
    /*!
     * @brief               (re-)initializes network to given parameters
     * @param N             Size of network
     * @param p             Probability of cross holding
     * @param val           total value in/being held by other firms
     * @param which_to_set  Flag to disable connections between parts of the network. Can be 0/1/2. 2: cross debt is 0, 1: cross equity is 0, 0: none is 0
     * @TODO: config struct
     */
    void init_network(const int N_, const double p_, const double val_, const int which_to_set, const double T_,\
        const double r_, const double S0_, const double sigma_, const double default_prob_scale_, const NetworkType net_t_);

    void init_2D_network(BSParameters& bs_params, const double vs01, const double vs10, const double vr01, const double vr10);

    virtual ~NetwSim(){
        if(bsn != nullptr)
            delete bsn;
    }

    /*!
     * @brief               Constructs the Black Scholes Model using random cross holdings.
     * @param local         local MPI communicator (between producers/consumers only)
     * @param world         global MPI communicator
     * @param isGenerator   Flag for generator/consumer ranks
     */
#ifdef USE_MPI
    NetwSim(const boost::mpi::communicator local, const boost::mpi::communicator world, const bool isGenerator):
            local(local), world(world), isGenerator(isGenerator), Z_dist(&tmp[0][0], &tmp[1][1]), chi_dist(deg_of_freedom), t_dist(deg_of_freedom)
#else
    NetwSim():
            Z_dist(&tmp[0][0], &tmp[1][1]), chi_dist(deg_of_freedom), t_dist(deg_of_freedom), initialized(false)
#endif
    {
        bsn = nullptr;
        iSigma = Eigen::MatrixXd::Zero(1,1);
        Z = Eigen::VectorXd::Zero(1,1);
        var_h = Eigen::VectorXd::Zero(1,1);
    }

    /*!
     * @brief               Constructs the Black Scholes Model using random cross holdings.
     * @param local         local MPI communicator (between producers/consumers only)
     * @param world         global MPI communicator
     * @param isGenerator   Flag for generator/consumer ranks
     * @param N             Size of network
     * @param p             Probability of connection between firms
     * @param val
     * @param which_to_set  Flag to disable connections between parts of the network. Can be 0/1/2. 2: cross debt is 0, 1: cross equity is 0, 0: none is 0
     * @param T             maturity
     * @param r             interest rate
     */
#ifdef USE_MPI
    NetwSim(const boost::mpi::communicator local, const boost::mpi::communicator world, const bool isGenerator,
               long N, double p, double val, int which_to_set, const double T, const double r, const double S0, const NetworkType net_t_) :
            local(local), world(world), isGenerator(isGenerator),
#else
    NetwSim(long N_, double p_, double val, int which_to_set, const double T_, const double r_, const double S0_, const double sigma_, const double default_scale_, const NetworkType net_t_) :
#endif
            val(val), T(T_), r(r_), S0scalar(S0_), sigmaScalar(sigma_), default_prob_scale(default_scale_)\
        , Z_dist(&tmp[0][0], &tmp[1][1]), chi_dist(deg_of_freedom), t_dist(deg_of_freedom), net_t(net_t_)
    {
        gen_u.seed();
        bsn = nullptr;
        init_network(N_, p_, val, which_to_set, T_, r_, S0_, sigma_, default_scale_, net_t_);
    }

    inline ResultType run_valuation(const SimulationParameters sim_params)
    {
        return run_valuation(sim_params.iterations, sim_params.N_networks);
    }

    /*!
     * @brief       Runs a series of example simulations
     * @param N_in  Size of network
     */
    ResultType run_valuation(const long N_Samples = 2000, const long N_networks = 100, const bool fix_degree = false);

    /*!
     * @brief   Draws a random number from a multivariate lognormal distribution
     * @return  Random sample from a multivariate lognormal distribution
     */
    const Eigen::MatrixXd draw_from_dist();

    const Eigen::MatrixXd transformZ(const Eigen::Ref<const Eigen::MatrixXd>& Z) const;

    double get_weight();

    /*!
     * @brief       Runs a single simulation of the Black Scholes model to find the fix point valuation.
     * @param St_in Initial asset value
     * @return      Valuation of firms at maturity T
     */
    auto run(const Eigen::Ref<const Eigen::VectorXd>& St_in)//Eigen::VectorXd St)
    {
        //Eigen::VectorXd St = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(St_in.data(), St_in.size());
        bsn->set_St(St_in);
        bsn->run_valuation(1000);
    }

    /*!
     * @brief   Compute \f$\Delta\f$ using the covariance matrix of the normal distribution
     * @return  \f$\Delta\f$
     */
    const Eigen::MatrixXd delta_v2();

    /*!
     * @brief   Computes the sum over all elements of the cross holdings matrix
     * @return  \f$\sum_{ij} M_{ij}\f$
     */
    std::vector<double> sumM() {
        std::vector<double> res{(bsn->get_M()).sum()};
        return res;
    }

    Eigen::MatrixXd get_M()
    {
        return bsn->get_M();
    }

    auto test_out()
    {
        auto v_o = bsn->get_valuation();
        auto s_o = bsn->get_solvent();
        std::cout << "output after sample: " << std::endl;
        LOG(INFO) << "Valuation: \n" << v_o;
        LOG(INFO) << "solvent: \n" << s_o;
        LOG(INFO) << "St: \n" << bsn->get_assets();
        LOG(INFO) << "debt: \n" << bsn->get_debt();
        LOG(INFO) << "M: \n" << bsn->get_M();
        std::cout << "------" << std::endl;
        Eigen::MatrixXd out = Eigen::MatrixXd::Constant(1,1,0);
        return out;
    }

private:

    trng::yarn2 gen_z;
    trng::yarn2 gen_chi;
    trng::chi_square_dist<double> chi_dist;
    trng::correlated_normal_dist<> Z_dist;

public:
    Eigen::MatrixXd get_io_deg_dist() const
    {
        return io_deg_dist;
    }

    Eigen::MatrixXd get_avg_row_col_sums() const
    {
        return avg_rc_sums;
    }

    void set_weight();
    //}

};

#endif //VALUATION_NETWORK_SIM_HPP


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    SO4.cpp
 * @brief   4*4 matrix representation of SO(4)
 * @author  Frank Dellaert
 * @author  Luca Carlone
 */

#include <gtsam/base/concepts.h>
#include <gtsam/base/timing.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/geometry/Unit3.h>

#include <Eigen/Eigenvalues>
#include <boost/random.hpp>

#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>

using namespace std;

namespace gtsam {

// /* *************************************************************************
// */ static Vector3 randomOmega(boost::mt19937 &rng) {
//   static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
//   return Unit3::Random(rng).unitVector() * randomAngle(rng);
// }

// /* *************************************************************************
// */
// // Create random SO(4) element using direct product of lie algebras.
// SO4 SO4::Random(boost::mt19937 &rng) {
//   Vector6 delta;
//   delta << randomOmega(rng), randomOmega(rng);
//   return SO4::Expmap(delta);
// }

//******************************************************************************
template <>
GTSAM_EXPORT
Matrix4 SO4::Hat(const Vector6& xi) {
  // skew symmetric matrix X = xi^
  // Unlike Luca, makes upper-left the SO(3) subgroup.
  Matrix4 Y = Z_4x4;
  Y(0, 1) = -xi(5);
  Y(0, 2) = +xi(4);
  Y(1, 2) = -xi(3);
  Y(0, 3) = -xi(2);
  Y(1, 3) = +xi(1);
  Y(2, 3) = -xi(0);
  return Y - Y.transpose();
}

//******************************************************************************
template <>
GTSAM_EXPORT
Vector6 SO4::Vee(const Matrix4& X) {
  Vector6 xi;
  xi(5) = -X(0, 1);
  xi(4) = +X(0, 2);
  xi(3) = -X(1, 2);
  xi(2) = -X(0, 3);
  xi(1) = +X(1, 3);
  xi(0) = -X(2, 3);
  return xi;
}

//******************************************************************************
/* Exponential map, porting MATLAB implementation by Luca, which follows
 * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
 * Ramona-Andreaa Rohan */
template <>
GTSAM_EXPORT
SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
  using namespace std;
  if (H) throw std::runtime_error("SO4::Expmap Jacobian");

  // skew symmetric matrix X = xi^
  const Matrix4 X = Hat(xi);

  // do eigen-decomposition
  auto eig = Eigen::EigenSolver<Matrix4>(X);
  Eigen::Vector4cd e = eig.eigenvalues();
  using std::abs;
  sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
    return abs(a.imag()) > abs(b.imag());
  });

  // Get a and b from eigenvalues +/i ai and +/- bi
  double a = e[0].imag(), b = e[2].imag();
  if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
    throw runtime_error("SO4::Expmap: wrong eigenvalues.");
  }

  // Build expX = exp(xi^)
  Matrix4 expX;
  using std::cos;
  using std::sin;
  const auto X2 = X * X;
  const auto X3 = X2 * X;
  double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
  if (a != 0 && b == 0) {
    double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
    return SO4(I_4x4 + X + c2 * X2 + c3 * X3);
  } else if (a == b && b != 0) {
    double sin_a = sin(a), cos_a = cos(a);
    double c0 = (a * sin_a + 2 * cos_a) / 2,
           c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
           c3 = (sin_a - a * cos_a) / (2 * a3);
    return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
  } else if (a != b) {
    double sin_a = sin(a), cos_a = cos(a);
    double sin_b = sin(b), cos_b = cos(b);
    double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
           c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
           c2 = (cos_a - cos_b) / (b2 - a2),
           c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
    return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
  } else {
    return SO4();
  }
}

//******************************************************************************
// local vectorize
static SO4::VectorN2 vec4(const Matrix4& Q) {
  return Eigen::Map<const SO4::VectorN2>(Q.data());
}

// so<4> generators
static std::vector<Matrix4, Eigen::aligned_allocator<Matrix4> > G4(
    {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
     SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
     SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});

// vectorized generators
static const Eigen::Matrix<double, 16, 6> P4 =
    (Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
     vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
        .finished();

//******************************************************************************
template <>
GTSAM_EXPORT
Matrix6 SO4::AdjointMap() const {
  // Elaborate way of calculating the AdjointMap
  // TODO(frank): find a closed form solution. In SO(3) is just R :-/
  const Matrix4& Q = matrix_;
  const Matrix4 Qt = Q.transpose();
  Matrix6 A;
  for (size_t i = 0; i < 6; i++) {
    // Calculate column i of linear map for coeffcient of Gi
    A.col(i) = SO4::Vee(Q * G4[i] * Qt);
  }
  return A;
}

//******************************************************************************
template <>
GTSAM_EXPORT
SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
  const Matrix& Q = matrix_;
  if (H) {
    // As Luca calculated, this is (I4 \oplus Q) * P4
    *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0),
        Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0);
  }
  return gtsam::vec4(Q);
}

///******************************************************************************
template <>
GTSAM_EXPORT
SO4 SO4::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
  if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
  gttic(SO4_Retract);
  const Matrix4 X = Hat(xi / 2);
  return SO4((I_4x4 + X) * (I_4x4 - X).inverse());
}

//******************************************************************************
template <>
GTSAM_EXPORT
Vector6 SO4::ChartAtOrigin::Local(const SO4& Q, ChartJacobian H) {
  if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
  const Matrix4& R = Q.matrix();
  const Matrix4 X = (I_4x4 - R) * (I_4x4 + R).inverse();
  return -2 * Vee(X);
}

//******************************************************************************
GTSAM_EXPORT Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
  const Matrix4& R = Q.matrix();
  const Matrix3 M = R.topLeftCorner<3, 3>();
  if (H) {
    const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
                  q = R.topRightCorner<3, 1>();
    *H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2,  //
        Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1,   //
        q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
  }
  return M;
}

//******************************************************************************
GTSAM_EXPORT Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
  const Matrix4& R = Q.matrix();
  const Matrix43 M = R.leftCols<3>();
  if (H) {
    const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3);
    *H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2,  //
        Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1,   //
        q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
  }
  return M;
}

//******************************************************************************

}  // end namespace gtsam


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

  Program:   Small Body Geophysical Analysis
  Module:    SBGATMassProperties.hpp

  Class derived from VTK's vtkPolyDataAlgorithm by Benjamin Bercovici

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/**
\file SBGATMassProperties.hpp
\class  SBGATMassProperties
\author Benjamin Bercovici
\author Jay McMahon
\brief  Computes volume, area, shape index, center of mass,
inertia tensor and principal axes of a polyhedral mesh of constant density
\details Computes the volume, the surface area, and the
normalized shape index, center of mass and inertia tensor of a topologically-closed, constant-density polyhedron.
This class will always use results expressed in `meters` as their distance unit (e.g center-of-mass coordinates in meters, volume in m^3,...) . Unit consistency is enforced through the use of the SetScaleMeters()
and SetScaleKiloMeters() method.

See "Inertia of Any Polyhedron" by Anthony R. Dobrovolskis, Icarus 124, 698–704 (1996) Article No. 0243
for further details.  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
\copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef SBGATMassProperties_h
#define SBGATMassProperties_h

#include <vtkFiltersCoreModule.h> // For export macro
#include <vtkPolyDataAlgorithm.h>
#include <armadillo>
#include <SBGATFilter.hpp>

class VTKFILTERSCORE_EXPORT SBGATMassProperties : public SBGATFilter{
public:
  /**
   * Constructs with initial values of zero.
   */
  static SBGATMassProperties *New();

  vtkTypeMacro(SBGATMassProperties,vtkPolyDataAlgorithm);
  void PrintSelf(std::ostream& os, vtkIndent indent) override;
  void PrintHeader(std::ostream& os, vtkIndent indent) override;
  void PrintTrailer(std::ostream& os, vtkIndent indent) override;

  /**
   * Compute and return the volume (m^3)
   */
  double GetVolume() const { return this->Volume;}

  /**
   * Compute and return the projected volume.
   * Typically you should compare this volume to the value returned by GetVolume
   * if you get an error (GetVolume()-GetVolumeProjected())*10000 that is greater
   * than GetVolume() this should identify a problem:
   * * Either the polydata is not closed
   * * Or the polydata contains triangle that are flipped
   */
  double GetVolumeProjected() const { return this->VolumeProjected;}

  /**
   * Compute and return the volume projected on to each axis aligned plane.
   */
  double GetVolumeX() { return this->VolumeX;}
  double GetVolumeY() { return this->VolumeY;}
  double GetVolumeZ() { return this->VolumeZ;}

  /**
   * Compute and return the weighting factors for the maximum unit
   * normal component (MUNC).
   */
  double GetKx() const { return this->Kx;}
  double GetKy() const { return this->Ky;}
  double GetKz() const { return this->Kz;}

  /**
   * Compute and return the area in m^2
   */
  double GetSurfaceArea()  const{ return this->SurfaceArea;
  }

  /**
   * Compute and return the min cell area in m^2
   */
  double GetMinCellArea()  const{ return this->MinCellArea;
  }

  /**
   * Compute and return the max cell area in m^2
   */
  double GetMaxCellArea()  const{ return this->MaxCellArea;
  }

  /**
  Checks whether the polydata is topologically closed or open
  If closed, the sum of the oriented surface area should be equal to zero
  */
  bool CheckClosed() const{ return this -> IsClosed;}

  /**
   * Compute and return the normalized shape index. This characterizes the
   * deviation of the shape of an object from a sphere. A sphere's NSI
   * is one. This number is always >= 1.0.
   */
  double GetNormalizedShapeIndex() const
  { return this->NormalizedShapeIndex;
  }

  /**
  * Compute and return the coordinates of the center of mass (m)
  * evaluated in the frame of origin assuming a constant density distribution
  * across the shape
  */
  const arma::vec::fixed<3> & GetCenterOfMass() const {
   return this -> center_of_mass;
 }

  /**
  * Compute and return the coordinates of the center of mass (m)
  * evaluated in the frame of origin assuming a constant density distribution
  * across the shape
  */
 void GetCenterOfMass(double * com) const{

  com[0] = this -> center_of_mass(0);
  com[1] = this -> center_of_mass(1);
  com[2] = this -> center_of_mass(2);
}

  /**
  * Compute and return the dimensionless inertia tensor
  * evaluated in the frame of origin assuming a constant density distribution
  * across the shape. The normalization applied to the inertia tensor is I_norm = I / (mass * r_avg ^ 2) where r_avg = cbrt(3/4*Volume/pi)
  */
arma::mat::fixed<3,3> GetNormalizedInertiaTensor() const {
 return this -> inertia_tensor;
}

  /**
  * Compute and return the dimensionless inertia tensor
  * evaluated in the frame of origin assuming a constant density distribution
  * across the shape. The normalization applied to the inertia tensor is I_norm = I / (rho) where rho is the density
  */
arma::mat::fixed<3,3> GetUnitDensityInertiaTensor() const{
 return unit_density_inertia_tensor;
}

  /**
  * Compute and return the dcm orienting the principal axes of the small body relative to
  the body coordinates frame. That is, denoting P the principal frame and B the frame in which the
  coordinates of the body are currently expressed, this method returns [PB]
  @return [PB] direction cosine matrix
  */
arma::mat::fixed<3,3> GetPrincipalAxes() const{
 return this -> principal_axes;
}

  /**
  Computes and returns the principal dimensions (m) of the ellipsoid associated with the inertia tensor
  tensor, sorted from the longest (smallest inertia) to shortest (largest inertia)
  @return principal dimensions associated with inertia tensor (m)
  */
arma::vec::fixed<3> GetPrincipalDimensions() const {
 return this -> principal_dimensions;
}

  /**
  * Compute and return the normalized inertia moments assuming uniform density distribution
  * across the shape, sorted from the smallest inertia to the largest.
  * The normalization applied to the inertia tensor is I_norm = I / (mass * r_avg ^ 2) where r_avg = cbrt(3/4*Volume/pi)
  */
arma::vec::fixed<3> GetNormalizedInertiaMoments() const {
 return normalized_principal_moments;
}

  /**
  * Compute and return the inertia moments assuming uniform unit density distribution
  * across the shape, sorted from the smallest inertia to the largest.
  */
arma::vec::fixed<3> GetUnitDensityInertiaMoments() const {
 return unit_density_principal_moments;
}

  /**
  Return the average radius of the shape (that is, the radius of a sphere occupying the same volume) (m)
  */
double GetAverageRadius() const {
 return this -> r_avg;
}

    /**
    Computes the mass properties of the provided shape and saves the results to a JSON file
    @param shape pointer to considered shape
    @param path savepath (ex: "mass_properties.json")
    */
static void ComputeAndSaveMassProperties(vtkSmartPointer<vtkPolyData> shape,std::string path);

  /**
  Save the computed mass properties to a JSON file
  @param path savepath (ex: "mass_properties.json")

  */
void SaveMassProperties(std::string path) const ;

/**
  Return signed contribution to total volume of tetrahedron subtended by facet
  f (m^3)
  @param f facet index
  @return signed volume of tetrahedron subtended by facet
  */
double GetDeltaV(const int & f) const;

  /**
  Return coordinates of the tetrahedron's center-of-mass (m)
  @param f facet index
  @return coordinates of tetrahedron
  */
arma::vec::fixed<3> GetDeltaCM(const int & f) const ;

  /**
  Return the unit-density tetrahedron's inertia tensor divided by tetrahedron's signed volume (m^2)
  @param f facet index
  @return tetrahedron's inertia tensor divided by tetrahedron's signed volume
  */
arma::mat::fixed<3,3> GetDeltaIOverDeltaV(const int & f) const ;

/**
Return the parametrization of the the unit-density tetrahedron's inertia tensor
@param f facet index
@return parametrization of the tetrahedron's inertia tensor
*/
arma::vec::fixed<6> GetDeltaIf(const int & f) const;

protected:
  SBGATMassProperties();
  ~SBGATMassProperties() override;

  int RequestData(vtkInformation* request,
    vtkInformationVector** inputVector,
    vtkInformationVector* outputVector) override;

  arma::vec::fixed<3> center_of_mass;
  arma::mat::fixed<3,3> inertia_tensor;
  arma::mat::fixed<3,3> principal_axes;

  arma::vec::fixed<3> normalized_principal_moments;
  arma::vec::fixed<3> unit_density_principal_moments;
  arma::mat::fixed<3,3> unit_density_inertia_tensor;
  arma::vec::fixed<3> principal_dimensions;

  double  SurfaceArea;
  double  MinCellArea;
  double  MaxCellArea;
  double  Volume;
  double  VolumeProjected;
  double  VolumeX;
  double  VolumeY;
  double  VolumeZ;
  double  Kx;
  double  Ky;
  double  Kz;
  double  NormalizedShapeIndex;
  double r_avg;
  bool IsClosed;

private:
  SBGATMassProperties(const SBGATMassProperties&) = delete;
  void operator=(const SBGATMassProperties&) = delete;
};

#endif


// ----------------------------------------------------------------------------
// FILENAME: timetest.cpp
//
// DESCRIPTION:
//    This file contians the function that used for save the running time of
//    computing polynomial real root isolation
//
// AUTHOR: Xinlong Yi
//
// ----------------------------------------------------------------------------

#include "budan.h"
#include "poly.h"
#include "range.h"
#include "vincent.h"
#include <boost/numeric/interval/utility_fwd.hpp>
#include <chrono>
#include <time.h>

static const int kTESTDEGREE = 7;
static const int digit = 4; // number of digit after point
static const int digit_control = std::pow(10, digit); // controler of digit

static const double max_root = 10000;
// static const double max_root = std::pow(2, kTESTDEGREE);

/**
 * Get random double in range min to max
 */
double rand_double(double min, double max) {
  double f = (double)rand() / RAND_MAX;
  f = min + f * (max - min);
  f = std::ceil(f * digit_control) / digit_control;
  return f;
}

int main() {
  srand(time(NULL));

  // Get random polynomial
  double *coeffs = new double[kTESTDEGREE];

  double budan_total = 0, vincent_total = 0;

  for (int i = 0; i < 1000; i++) {

    for (size_t i = 0; i <= kTESTDEGREE; i++) {
      coeffs[i] = rand_double(-max_root, max_root);
    }

    Poly<kTESTDEGREE + 1> tt(coeffs, kTESTDEGREE);

    std::cout << tt << std::endl;

    // save roots
    Range *roots = new Range[kTESTDEGREE];

    // Budan
    auto budan_start = std::chrono::high_resolution_clock::now();
    BudanRootIsolate(coeffs, kTESTDEGREE, roots);
    auto budan_end = std::chrono::high_resolution_clock::now();

    // Vincent
    auto vincent_start = std::chrono::high_resolution_clock::now();
    VincentRootIsolate(coeffs, kTESTDEGREE, roots);
    auto vincent_end = std::chrono::high_resolution_clock::now();

    //     Time
    auto budan_duration = std::chrono::duration_cast<std::chrono::microseconds>(
        budan_end - budan_start);
    auto vincent_duration =
        std::chrono::duration_cast<std::chrono::microseconds>(vincent_end -
                                                              vincent_start);

    budan_total += budan_duration.count();
    vincent_total += vincent_duration.count();
  }

  std::cout << "Budan Theorem takes " << budan_total / 1000.0 << " us for "
            << kTESTDEGREE - 1 << " degree" << std::endl;

  std::cout << "Continued Fraction takes " << vincent_total / 1000.0
            << " us for " << kTESTDEGREE - 1 << " degree" << std::endl;

  return 0;
}


#ifndef STAN_MATH_PRIM_MAT_FUN_QUAD_FORM_HPP
#define STAN_MATH_PRIM_MAT_FUN_QUAD_FORM_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/prim/mat/fun/dot_product.hpp>
#include <stan/math/prim/mat/fun/multiply.hpp>
#include <stan/math/prim/mat/fun/transpose.hpp>

namespace stan {
  namespace math {
    /**
     * Compute B^T A B
     **/
    template<int RA, int CA, int RB, int CB, typename T>
    inline Eigen::Matrix<T, CB, CB>
    quad_form(const Eigen::Matrix<T, RA, CA>& A,
              const Eigen::Matrix<T, RB, CB>& B) {
      check_square("quad_form", "A", A);
      check_multiplicable("quad_form", "A", A, "B", B);
      return multiply(transpose(B), multiply(A, B));
    }

    template<int RA, int CA, int RB, typename T>
    inline T
    quad_form(const Eigen::Matrix<T, RA, CA>& A,
              const Eigen::Matrix<T, RB, 1>& B) {
      check_square("quad_form", "A", A);
      check_multiplicable("quad_form", "A", A, "B", B);
      return dot_product(B, multiply(A, B));
    }

  }
}

#endif


/*******************************************************************************
 * Copyright (c) 2014, 2015  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 <boost/math/distributions/chi_squared.hpp>
#include "MathUtils.hpp"
#include "LocException.hpp"

double MathUtils::quantileChiSquaredDistribution(int degreeOfFreedom, double cumulativeDensity){
    boost::math::chi_squared chi_sq(degreeOfFreedom);
    double x = boost::math::quantile(chi_sq, cumulativeDensity);
    return x;
}

DirectionalStatistics MathUtils::computeDirectionalStatistics(std::vector<double> orientations){
    size_t n = orientations.size();
    if(n==0){
        BOOST_THROW_EXCEPTION(LocException("The size of input orientation vector is zero."));
    }
    double x = 0, y = 0;
    for(auto ori : orientations){
        x += std::cos(ori);
        y += std::sin(ori);
    }
    x /= n;
    y /= n;
    double meanOri = std::atan2(y,x);
    double R = std::sqrt(x*x + y*y);
    double v = 1.0 - R;
    DirectionalStatistics oristat(meanOri, v);
    return oristat;
}

WrappedNormalParameter MathUtils::computeWrappedNormalParameters(const std::vector<double>& orientations){

    size_t n = orientations.size();

    DirectionalStatistics dstats = MathUtils::computeDirectionalStatistics(orientations);

    double mu = dstats.circularMean();
    double R = 1.0 - dstats.circularVariance();

    double R2 = R*R>1.0/n? R*R : 1.0/n;
    double Re2 = (double)n/(n-1)*(R2 - 1.0/n);

    double sigma2 = std::log(1.0/Re2);
    double sigma =  sigma2>0? std::sqrt(sigma2) : 0.0;

    WrappedNormalParameter param(mu, sigma);
    return param;
}


#include <tagloc/tracking.h>
#include <Eigen/Dense>
#define USE_MATH_DEFINES
#include <math.h>
#include <iostream>

using namespace std;

int main(int argc, char** argv){
	Eigen::MatrixXd t = Eigen::MatrixXd::Zero(2,1);
	Eigen::MatrixXd r = Eigen::MatrixXd::Zero(2,1);
	r<<10,10;
	double th = M_PI/4;
	Eigen::MatrixXd R = Eigen::MatrixXd::Zero(2,2);
	R<<cos(th),-sin(th),sin(th),cos(th);
	Eigen::MatrixXd P = Eigen::MatrixXd::Zero(2,2);
	P<<4,0,0,1;
	P = R*P*R.transpose();
	cout<<P<<endl;

	Eigen::MatrixXd cp;

	cout<<"---"<<endl;
	cp = RSN::closest_pt_ellipse(t,P,r,1);
	cout<<"Expect something along [1,1]"<<endl;
	cout<<cp<<endl;

	cout<<"---"<<endl;
	r<<-10,-10;
	cp = RSN::closest_pt_ellipse(t,P,r,1);
	cout<<"Expect reflection of above"<<endl;
	cout<<cp<<endl;

	cout<<"---"<<endl;
	r<<-10,-10;
	cout<<"x:"<<endl;
	cout<<r<<endl;
	th = 0;
	R<<cos(th),-sin(th),sin(th),cos(th);
	P<<4,0,0,1;
	P = R*P*R.transpose();
	cout<<P<<endl;
	cout<<"cp:"<<endl;
	cp = RSN::closest_pt_ellipse(t,P,r,1);
	cout<<cp<<endl;

	cout<<"---"<<endl;
	cout<<"Same as above, but with target shifted to [20,0]"<<endl;
	t<<20,0;
	r<<-10,-10;
	cout<<"x:"<<endl;
	cout<<r<<endl;
	th = 0;
	R<<cos(th),-sin(th),sin(th),cos(th);
	P<<4,0,0,1;
	P = R*P*R.transpose();
	cout<<P<<endl;
	cout<<"cp:"<<endl;
	cp = RSN::closest_pt_ellipse(t,P,r,1);
	cout<<cp<<endl;
	return 0;
}


#include "forward_dynamics.h"

#include <Eigen/Cholesky>
#include <iit/rbd/robcogen_commons.h>

using namespace iit::rbd;

// Initialization of static-const data
const ur5::rcg::ForwardDynamics::ExtForces
    ur5::rcg::ForwardDynamics::zeroExtForces(Force::Zero());

ur5::rcg::ForwardDynamics::ForwardDynamics(InertiaProperties& inertia, MotionTransforms& transforms) :
    inertiaProps( & inertia ),
    motionTransforms( & transforms )
{
    shoulder_v.setZero();
    shoulder_c.setZero();
    upper_arm_v.setZero();
    upper_arm_c.setZero();
    forearm_v.setZero();
    forearm_c.setZero();
    wrist_1_v.setZero();
    wrist_1_c.setZero();
    wrist_2_v.setZero();
    wrist_2_c.setZero();
    wrist_3_v.setZero();
    wrist_3_c.setZero();

    vcross.setZero();
    Ia_r.setZero();

}

void ur5::rcg::ForwardDynamics::fd(
    JointState& qdd,
    const JointState& qd,
    const JointState& tau,
    const ExtForces& fext/* = zeroExtForces */)
{

    shoulder_AI = inertiaProps->getTensor_shoulder();
    shoulder_p = - fext[SHOULDER];
    upper_arm_AI = inertiaProps->getTensor_upper_arm();
    upper_arm_p = - fext[UPPER_ARM];
    forearm_AI = inertiaProps->getTensor_forearm();
    forearm_p = - fext[FOREARM];
    wrist_1_AI = inertiaProps->getTensor_wrist_1();
    wrist_1_p = - fext[WRIST_1];
    wrist_2_AI = inertiaProps->getTensor_wrist_2();
    wrist_2_p = - fext[WRIST_2];
    wrist_3_AI = inertiaProps->getTensor_wrist_3();
    wrist_3_p = - fext[WRIST_3];
    // ---------------------- FIRST PASS ---------------------- //
    // Note that, during the first pass, the articulated inertias are really
    //  just the spatial inertia of the links (see assignments above).
    //  Afterwards things change, and articulated inertias shall not be used
    //  in functions which work specifically with spatial inertias.

    // + Link shoulder
    //  - The spatial velocity:
    shoulder_v(AZ) = qd(SHOULDER_PAN);

    //  - The bias force term:
    shoulder_p += vxIv(qd(SHOULDER_PAN), shoulder_AI);

    // + Link upper_arm
    //  - The spatial velocity:
    upper_arm_v = (motionTransforms-> fr_upper_arm_X_fr_shoulder) * shoulder_v;
    upper_arm_v(AZ) += qd(SHOULDER_LIFT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(upper_arm_v, vcross);
    upper_arm_c = vcross.col(AZ) * qd(SHOULDER_LIFT);

    //  - The bias force term:
    upper_arm_p += vxIv(upper_arm_v, upper_arm_AI);

    // + Link forearm
    //  - The spatial velocity:
    forearm_v = (motionTransforms-> fr_forearm_X_fr_upper_arm) * upper_arm_v;
    forearm_v(AZ) += qd(ELBOW);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(forearm_v, vcross);
    forearm_c = vcross.col(AZ) * qd(ELBOW);

    //  - The bias force term:
    forearm_p += vxIv(forearm_v, forearm_AI);

    // + Link wrist_1
    //  - The spatial velocity:
    wrist_1_v = (motionTransforms-> fr_wrist_1_X_fr_forearm) * forearm_v;
    wrist_1_v(AZ) += qd(WR1);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(wrist_1_v, vcross);
    wrist_1_c = vcross.col(AZ) * qd(WR1);

    //  - The bias force term:
    wrist_1_p += vxIv(wrist_1_v, wrist_1_AI);

    // + Link wrist_2
    //  - The spatial velocity:
    wrist_2_v = (motionTransforms-> fr_wrist_2_X_fr_wrist_1) * wrist_1_v;
    wrist_2_v(AZ) += qd(WR2);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(wrist_2_v, vcross);
    wrist_2_c = vcross.col(AZ) * qd(WR2);

    //  - The bias force term:
    wrist_2_p += vxIv(wrist_2_v, wrist_2_AI);

    // + Link wrist_3
    //  - The spatial velocity:
    wrist_3_v = (motionTransforms-> fr_wrist_3_X_fr_wrist_2) * wrist_2_v;
    wrist_3_v(AZ) += qd(WR3);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(wrist_3_v, vcross);
    wrist_3_c = vcross.col(AZ) * qd(WR3);

    //  - The bias force term:
    wrist_3_p += vxIv(wrist_3_v, wrist_3_AI);

    // ---------------------- SECOND PASS ---------------------- //
    Matrix66 IaB;
    Force pa;

    // + Link wrist_3
    wrist_3_u = tau(WR3) - wrist_3_p(AZ);
    wrist_3_U = wrist_3_AI.col(AZ);
    wrist_3_D = wrist_3_U(AZ);

    compute_Ia_revolute(wrist_3_AI, wrist_3_U, wrist_3_D, Ia_r);  // same as: Ia_r = wrist_3_AI - wrist_3_U/wrist_3_D * wrist_3_U.transpose();
    pa = wrist_3_p + Ia_r * wrist_3_c + wrist_3_U * wrist_3_u/wrist_3_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_wrist_3_X_fr_wrist_2, IaB);
    wrist_2_AI += IaB;
    wrist_2_p += (motionTransforms-> fr_wrist_3_X_fr_wrist_2).transpose() * pa;

    // + Link wrist_2
    wrist_2_u = tau(WR2) - wrist_2_p(AZ);
    wrist_2_U = wrist_2_AI.col(AZ);
    wrist_2_D = wrist_2_U(AZ);

    compute_Ia_revolute(wrist_2_AI, wrist_2_U, wrist_2_D, Ia_r);  // same as: Ia_r = wrist_2_AI - wrist_2_U/wrist_2_D * wrist_2_U.transpose();
    pa = wrist_2_p + Ia_r * wrist_2_c + wrist_2_U * wrist_2_u/wrist_2_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_wrist_2_X_fr_wrist_1, IaB);
    wrist_1_AI += IaB;
    wrist_1_p += (motionTransforms-> fr_wrist_2_X_fr_wrist_1).transpose() * pa;

    // + Link wrist_1
    wrist_1_u = tau(WR1) - wrist_1_p(AZ);
    wrist_1_U = wrist_1_AI.col(AZ);
    wrist_1_D = wrist_1_U(AZ);

    compute_Ia_revolute(wrist_1_AI, wrist_1_U, wrist_1_D, Ia_r);  // same as: Ia_r = wrist_1_AI - wrist_1_U/wrist_1_D * wrist_1_U.transpose();
    pa = wrist_1_p + Ia_r * wrist_1_c + wrist_1_U * wrist_1_u/wrist_1_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_wrist_1_X_fr_forearm, IaB);
    forearm_AI += IaB;
    forearm_p += (motionTransforms-> fr_wrist_1_X_fr_forearm).transpose() * pa;

    // + Link forearm
    forearm_u = tau(ELBOW) - forearm_p(AZ);
    forearm_U = forearm_AI.col(AZ);
    forearm_D = forearm_U(AZ);

    compute_Ia_revolute(forearm_AI, forearm_U, forearm_D, Ia_r);  // same as: Ia_r = forearm_AI - forearm_U/forearm_D * forearm_U.transpose();
    pa = forearm_p + Ia_r * forearm_c + forearm_U * forearm_u/forearm_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_forearm_X_fr_upper_arm, IaB);
    upper_arm_AI += IaB;
    upper_arm_p += (motionTransforms-> fr_forearm_X_fr_upper_arm).transpose() * pa;

    // + Link upper_arm
    upper_arm_u = tau(SHOULDER_LIFT) - upper_arm_p(AZ);
    upper_arm_U = upper_arm_AI.col(AZ);
    upper_arm_D = upper_arm_U(AZ);

    compute_Ia_revolute(upper_arm_AI, upper_arm_U, upper_arm_D, Ia_r);  // same as: Ia_r = upper_arm_AI - upper_arm_U/upper_arm_D * upper_arm_U.transpose();
    pa = upper_arm_p + Ia_r * upper_arm_c + upper_arm_U * upper_arm_u/upper_arm_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_upper_arm_X_fr_shoulder, IaB);
    shoulder_AI += IaB;
    shoulder_p += (motionTransforms-> fr_upper_arm_X_fr_shoulder).transpose() * pa;

    // + Link shoulder
    shoulder_u = tau(SHOULDER_PAN) - shoulder_p(AZ);
    shoulder_U = shoulder_AI.col(AZ);
    shoulder_D = shoulder_U(AZ);

    // ---------------------- THIRD PASS ---------------------- //
    shoulder_a = (motionTransforms-> fr_shoulder_X_fr_base).col(LZ) * (ur5::rcg::g);
    qdd(SHOULDER_PAN) = (shoulder_u - shoulder_U.dot(shoulder_a)) / shoulder_D;
    shoulder_a(AZ) += qdd(SHOULDER_PAN);

    upper_arm_a = (motionTransforms-> fr_upper_arm_X_fr_shoulder) * shoulder_a + upper_arm_c;
    qdd(SHOULDER_LIFT) = (upper_arm_u - upper_arm_U.dot(upper_arm_a)) / upper_arm_D;
    upper_arm_a(AZ) += qdd(SHOULDER_LIFT);

    forearm_a = (motionTransforms-> fr_forearm_X_fr_upper_arm) * upper_arm_a + forearm_c;
    qdd(ELBOW) = (forearm_u - forearm_U.dot(forearm_a)) / forearm_D;
    forearm_a(AZ) += qdd(ELBOW);

    wrist_1_a = (motionTransforms-> fr_wrist_1_X_fr_forearm) * forearm_a + wrist_1_c;
    qdd(WR1) = (wrist_1_u - wrist_1_U.dot(wrist_1_a)) / wrist_1_D;
    wrist_1_a(AZ) += qdd(WR1);

    wrist_2_a = (motionTransforms-> fr_wrist_2_X_fr_wrist_1) * wrist_1_a + wrist_2_c;
    qdd(WR2) = (wrist_2_u - wrist_2_U.dot(wrist_2_a)) / wrist_2_D;
    wrist_2_a(AZ) += qdd(WR2);

    wrist_3_a = (motionTransforms-> fr_wrist_3_X_fr_wrist_2) * wrist_2_a + wrist_3_c;
    qdd(WR3) = (wrist_3_u - wrist_3_U.dot(wrist_3_a)) / wrist_3_D;
    wrist_3_a(AZ) += qdd(WR3);

}


#include <QTime>
#include <QApplication>
#include <QAction>
#include <QStringList>

#include "opengl_tools.h"
#include "Scene_polyhedron_item.h"
#include "Scene_points_with_normal_item.h"
#include "Scene_polylines_item.h"
#include "Scene_polyhedron_selection_item.h"
#include "Polyhedron_type.h"

#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>

#include <CGAL/convex_hull_3.h>
#include <boost/iterator/transform_iterator.hpp>
using namespace CGAL::Three;
class Polyhedron_demo_convex_hull_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
public:
    void init(QMainWindow* mainWindow,
              Scene_interface* scene_interface)
    {
        mw = mainWindow;
        scene = scene_interface;
        actions_map["actionConvexHull"] = getActionFromMainWindow(mw, "actionConvexHull");
        actions_map["actionConvexHull"]->setProperty("subMenuName",
                                                     "3D Convex Hulls");
        autoConnectActions();

    }

  // used by Polyhedron_demo_plugin_helper
  QStringList actionsNames() const {
    return QStringList() << "actionConvexHull";
  }

  bool applicable(QAction*) const {
    return
      qobject_cast<Scene_polyhedron_item*>(scene->item(scene->mainSelectionIndex())) ||
      qobject_cast<Scene_polylines_item*>(scene->item(scene->mainSelectionIndex())) ||
      qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex())) ||
      qobject_cast<Scene_polyhedron_selection_item*>(scene->item(scene->mainSelectionIndex()));
  }

public Q_SLOTS:
  void on_actionConvexHull_triggered();

}; // end Polyhedron_demo_convex_hull_plugin

// for transform iterator
struct Get_point {
  typedef const Polyhedron::Point_3& result_type;
  result_type operator()(const Polyhedron::Vertex_handle v) const
  { return v->point(); }
};

void Polyhedron_demo_convex_hull_plugin::on_actionConvexHull_triggered()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

  Scene_polyhedron_item* poly_item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  Scene_points_with_normal_item* pts_item =
    qobject_cast<Scene_points_with_normal_item*>(scene->item(index));

  Scene_polylines_item* lines_item =
    qobject_cast<Scene_polylines_item*>(scene->item(index));

  Scene_polyhedron_selection_item* selection_item =
    qobject_cast<Scene_polyhedron_selection_item*>(scene->item(index));

  if(poly_item || pts_item || lines_item || selection_item)
  {
    // wait cursor
    QApplication::setOverrideCursor(Qt::WaitCursor);

    QTime time;
    time.start();
    std::cout << "Convex hull...";

    // add convex hull as new polyhedron
    Polyhedron *pConvex_hull = new Polyhedron;
    if(selection_item) {
      CGAL::convex_hull_3(
        boost::make_transform_iterator(selection_item->selected_vertices.begin(), Get_point()),
        boost::make_transform_iterator(selection_item->selected_vertices.end(), Get_point()),
        *pConvex_hull);
    }
    else if ( poly_item ){
      Polyhedron* pMesh = poly_item->polyhedron();
      CGAL::convex_hull_3(pMesh->points_begin(),pMesh->points_end(),*pConvex_hull);
    }
    else{
      if (pts_item)
        CGAL::convex_hull_3(pts_item->point_set()->begin(),pts_item->point_set()->end(),*pConvex_hull);
      else{
        std::size_t nb_points=0;
        for(std::list<std::vector<Kernel::Point_3> >::const_iterator it = lines_item->polylines.begin();
            it != lines_item->polylines.end();
            ++it)  nb_points+=it->size();

        std::vector<Kernel::Point_3> all_points;
        all_points.reserve( nb_points );

        for(std::list<std::vector<Kernel::Point_3> >::const_iterator it = lines_item->polylines.begin();
            it != lines_item->polylines.end();
            ++it)  std::copy(it->begin(), it->end(),std::back_inserter( all_points ) );

        CGAL::convex_hull_3(all_points.begin(),all_points.end(),*pConvex_hull);
      }
    }
    std::cout << "ok (" << time.elapsed() << " ms)" << std::endl;

    Scene_polyhedron_item* new_item = new Scene_polyhedron_item(pConvex_hull);
    new_item->setName(tr("%1 (convex hull)").arg(scene->item(index)->name()));
    new_item->setColor(Qt::magenta);
    new_item->setRenderingMode(FlatPlusEdges);
    scene->addItem(new_item);

    // default cursor
    QApplication::restoreOverrideCursor();
  }
}

#include "Convex_hull_plugin.moc"


///////////////////////////////////////////////////////////////////////////////
// sum_kahan.hpp
//
//  Copyright 2010 Gaetano Mendola, 2011 Simon West. 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)

#ifndef BOOST_ACCUMULATORS_STATISTICS_SUM_KAHAN_HPP_EAN_26_07_2010
#define BOOST_ACCUMULATORS_STATISTICS_SUM_KAHAN_HPP_EAN_26_07_2010

#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/weighted_sum_kahan.hpp>
#include <boost/numeric/conversion/cast.hpp>

namespace boost { namespace accumulators
{

namespace impl
{

#if _MSC_VER > 1400
# pragma float_control(push)
# pragma float_control(precise, on)
#endif

template<typename Sample, typename Tag>
struct sum_kahan_impl
  : accumulator_base
{
    typedef Sample result_type;

    ////////////////////////////////////////////////////////////////////////////
    // sum_kahan_impl
    /**
        @brief Kahan summation algorithm

        The Kahan summation algorithm reduces the numerical error obtained with standard
        sequential sum.

    */
    template<typename Args>
    sum_kahan_impl(Args const & args)
      : sum(args[parameter::keyword<Tag>::get() | Sample()]),
        compensation(boost::numeric_cast<Sample>(0.0))
    {
    }

    template<typename Args>
    void
#if BOOST_ACCUMULATORS_GCC_VERSION > 40305
    __attribute__((optimize("no-associative-math")))
#endif
    operator ()(Args const & args)
    {
        const Sample myTmp1 = args[parameter::keyword<Tag>::get()] - this->compensation;
        const Sample myTmp2 = this->sum + myTmp1;
        this->compensation = (myTmp2 - this->sum) - myTmp1;
        this->sum = myTmp2;
    }

    result_type result(dont_care) const
    {
      return this->sum;
    }

private:
    Sample sum;
    Sample compensation;
};

#if _MSC_VER > 1400
# pragma float_control(pop)
#endif

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::sum_kahan
// tag::sum_of_weights_kahan
// tag::sum_of_variates_kahan
//
namespace tag
{

    struct sum_kahan
      : depends_on<>
    {
        /// INTERNAL ONLY
        ///
        typedef impl::sum_kahan_impl< mpl::_1, tag::sample > impl;
    };

    struct sum_of_weights_kahan
      : depends_on<>
    {
        typedef mpl::true_ is_weight_accumulator;
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::sum_kahan_impl<mpl::_2, tag::weight> impl;
    };

    template<typename VariateType, typename VariateTag>
    struct sum_of_variates_kahan
      : depends_on<>
    {
        /// INTERNAL ONLY
        ///
        typedef mpl::always<accumulators::impl::sum_kahan_impl<VariateType, VariateTag> > impl;
    };

} // namespace tag

///////////////////////////////////////////////////////////////////////////////
// extract::sum_kahan
// extract::sum_of_weights_kahan
// extract::sum_of_variates_kahan
//
namespace extract
{
    extractor<tag::sum_kahan> const sum_kahan = {};
    extractor<tag::sum_of_weights_kahan> const sum_of_weights_kahan = {};
    extractor<tag::abstract_sum_of_variates> const sum_of_variates_kahan = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(sum_kahan)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(sum_of_weights_kahan)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(sum_of_variates_kahan)
} // namespace extract

using extract::sum_kahan;
using extract::sum_of_weights_kahan;
using extract::sum_of_variates_kahan;

// sum(kahan) -> sum_kahan
template<>
struct as_feature<tag::sum(kahan)>
{
    typedef tag::sum_kahan type;
};

// sum_of_weights(kahan) -> sum_of_weights_kahan
template<>
struct as_feature<tag::sum_of_weights(kahan)>
{
    typedef tag::sum_of_weights_kahan type;
};

// So that sum_kahan can be automatically substituted with
// weighted_sum_kahan when the weight parameter is non-void.
template<>
struct as_weighted_feature<tag::sum_kahan>
{
    typedef tag::weighted_sum_kahan type;
};

template<>
struct feature_of<tag::weighted_sum_kahan>
  : feature_of<tag::sum>
{};

// for the purposes of feature-based dependency resolution,
// sum_kahan provides the same feature as sum
template<>
struct feature_of<tag::sum_kahan>
  : feature_of<tag::sum>
{
};

// for the purposes of feature-based dependency resolution,
// sum_of_weights_kahan provides the same feature as sum_of_weights
template<>
struct feature_of<tag::sum_of_weights_kahan>
  : feature_of<tag::sum_of_weights>
{
};

template<typename VariateType, typename VariateTag>
struct feature_of<tag::sum_of_variates_kahan<VariateType, VariateTag> >
  : feature_of<tag::abstract_sum_of_variates>
{
};

}} // namespace boost::accumulators

#endif


/**
 * @file layer_main.cc
 * @brief Solves CD BVP for exact solution with an internal layer
 * @author Philippe Peter
 * @date July 2020
 * @copyright Developed at SAM, ETH Zurich
 */
#include <lf/fe/fe.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <memory>
#include <string>

#include "cd_tools.h"
#include "standard_fem.h"
#include "supg.h"
#include "upwind.h"

int main() {
  // parameter functions:
  // boundary conditions
  const auto g = [](const Eigen::Vector2d &x) {
    return x(0) > x(1) ? 1.0 : 0.0;
  };
  // velocity field
  const auto v = [](const Eigen::Vector2d &x) {
    return Eigen::Vector2d(1.0, 1.0);
  };
  // diffusion coefficient
  const auto eps = [](const Eigen::Vector2d &x) { return 10E-10; };
  // source function
  const auto f = [](const Eigen::Vector2d &x) { return 0.0; };

  // Read Mesh from file
  std::string mesh_file = CURRENT_SOURCE_DIR "/meshes/mesh_square.msh";
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory), mesh_file);
  auto mesh_p = reader.mesh();

  // Construct dofhanlder for linear finite element space on the mesh.
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  // Compute solutions using Standard FE, Upwind, SUPG method
  Eigen::VectorXd sol_standard =
      ConvectionDiffusion::SolveCDBVPStandardFem(fe_space, eps, v, f, g);
  lf::fe::MeshFunctionFE sol_standard_mf(fe_space, sol_standard);

  Eigen::VectorXd sol_stable =
      ConvectionDiffusion::SolveCDBVPUpwind(fe_space, eps, v, f, g);
  lf::fe::MeshFunctionFE sol_upwind_mf(fe_space, sol_stable);

  Eigen::VectorXd sol_supg =
      ConvectionDiffusion::SolveCDBVPSupg(fe_space, eps, v, f, g);
  lf::fe::MeshFunctionFE sol_supg_mf(fe_space, sol_supg);

  // Output solution along the curve gamma
  auto gamma = [](double t) { return Eigen::Vector2d(t, 1 - t); };
  ConvectionDiffusion::SampleMeshFunction("results_standard_FEM.txt", mesh_p,
                                          gamma, sol_standard_mf, 300);
  ConvectionDiffusion::SampleMeshFunction("results_upwind.txt", mesh_p, gamma,
                                          sol_upwind_mf, 300);
  ConvectionDiffusion::SampleMeshFunction("results_supg.txt", mesh_p, gamma,
                                          sol_supg_mf, 300);

  // Plot
  std::system("python3 " CURRENT_SOURCE_DIR
              "/plot_layer.py " CURRENT_BINARY_DIR);
  return 0;
}


#pragma once

#include <cstdint>
#include <cmath>

#include <Eigen/Sparse>
#include <Eigen/Dense>

#include <crest/geometry/indexed_mesh.hpp>
#include <crest/util/eigen_extensions.hpp>
#include <crest/quadrature/triquad.hpp>
#include <crest/basis/basis.hpp>

namespace crest
{
    /**
     * A standard linear Lagrangian basis
     */
    template <typename Scalar>
    class LagrangeBasis2d : public Basis<Scalar, LagrangeBasis2d<Scalar>>
    {
    public:
        explicit LagrangeBasis2d(const IndexedMesh<Scalar, int> & mesh) : _mesh(mesh) {}

        virtual std::vector<int> boundary_nodes() const override { return _mesh.boundary_vertices(); }
        virtual std::vector<int> interior_nodes() const override { return _mesh.compute_interior_vertices(); }

        virtual Assembly<Scalar> assemble() const override;

        virtual int num_dof() const override { return _mesh.num_vertices(); }

        template <typename Function2d>
        VectorX<Scalar> interpolate(const Function2d &f) const;

        template <typename Function2d>
        VectorX<Scalar> interpolate_boundary(const Function2d &f) const;

        template <int QuadStrength, typename Function2d>
        VectorX<Scalar> load(const Function2d &f) const;

        template <int QuadStrength, typename Function2d>
        Scalar error_l2(const Function2d &f, const VectorX<Scalar> & weights) const;

        template <int QuadStrength, typename Function2d_x, typename Function2d_y>
        Scalar error_h1_semi(const Function2d_x & f_x,
                             const Function2d_y & f_y,
                             const VectorX<Scalar> & weights) const;

    private:
        const IndexedMesh<double, int> & _mesh;
    };

    namespace detail
    {
        template <typename Scalar>
        struct assembly_triplets {
            std::vector<Eigen::Triplet<Scalar>> stiffness_triplets;
            std::vector<Eigen::Triplet<Scalar>> mass_triplets;
        };

        template <typename Scalar>
        assembly_triplets<Scalar> assemble_linear_lagrangian_system_triplets(
                const crest::IndexedMesh<Scalar, int> & mesh);
    }

    /*
     * IMPLEMENTATION BELOW
     */

    template <typename Scalar>
    detail::assembly_triplets<Scalar> detail::assemble_linear_lagrangian_system_triplets(
            const crest::IndexedMesh<Scalar, int> & mesh)
    {
        const static Eigen::Matrix<Scalar, 3, 3> M_LOCAL_REF = (1.0 / 24.0) * (Eigen::Matrix3d()
                <<
                2.0, 1.0, 1.0,
                1.0, 2.0, 1.0,
                1.0, 1.0, 2.0
        ).finished().cast<Scalar>();

        const static Eigen::Matrix<Scalar, 3, 3> A11 = (1.0 / 2.0) * (Eigen::Matrix3d()
                <<
                1.0, 0.0, -1.0,
                0.0, 0.0, 0.0,
                -1.0, 0.0, 1.0
        ).finished().cast<Scalar>();

        const static Eigen::Matrix<Scalar, 3, 3> A12 = (1.0 / 2.0) * (Eigen::Matrix3d()
                <<
                0.0, 1.0, -1.0,
                1.0, 0.0, -1.0,
                -1.0, -1.0, 2.0
        ).finished().cast<Scalar>();

        const static Eigen::Matrix<Scalar, 3, 3> A22 = (1.0 / 2.0) * (Eigen::Matrix3d()
                <<
                0.0, 0.0, 0.0,
                0.0, 1.0, -1.0,
                0.0, -1.0, 1.0
        ).finished().cast<Scalar>();

        std::vector<Eigen::Triplet<Scalar>> mass_triplets;
        std::vector<Eigen::Triplet<Scalar>> stiffness_triplets;
        mass_triplets.reserve(3 * mesh.num_elements());
        stiffness_triplets.reserve(3 * mesh.num_elements());

        for (const auto & element : mesh.elements())
        {
            const auto a = mesh.vertices()[element.vertex_indices[0]];
            const auto b = mesh.vertices()[element.vertex_indices[1]];
            const auto c = mesh.vertices()[element.vertex_indices[2]];

            const auto v1 = a - c;
            const auto v2 = b - c;

            const Eigen::Matrix2d jacobian = (Eigen::Matrix2d() << v1.x, v2.x, v1.y, v2.y).finished();
            const Eigen::Matrix2d jacobian_inverse = jacobian.inverse();
            const Eigen::Matrix2d C = jacobian_inverse * jacobian_inverse.transpose();
            const auto abs_det_jacobian = std::abs(jacobian.determinant());

            const Eigen::Matrix<Scalar, 3, 3> A_local = abs_det_jacobian * (C(0, 0) * A11 + C(0, 1) * A12 + C(1, 1) * A22);
            const Eigen::Matrix<Scalar, 3, 3> M_local = abs_det_jacobian * M_LOCAL_REF;

            typedef Eigen::Triplet<Scalar> T;
            for (size_t i = 0; i < 3; ++i)
            {
                for (size_t j = 0; j < 3; ++j)
                {
                    const auto I = element.vertex_indices[i];
                    const auto J = element.vertex_indices[j];
                    mass_triplets.emplace_back(T(I, J, M_local(i, j)));
                    stiffness_triplets.emplace_back(T(I, J, A_local(i, j)));
                }
            }
        }

        return detail::assembly_triplets<Scalar> {
                std::move(stiffness_triplets),
                std::move(mass_triplets)
        };
    }

    template <typename Scalar>
    Assembly<Scalar> LagrangeBasis2d<Scalar>::assemble() const
    {
        const auto triplets = detail::assemble_linear_lagrangian_system_triplets(_mesh);

        Assembly<Scalar> assembly;

        // Stiffness
        assembly.stiffness = Eigen::SparseMatrix<Scalar>(num_dof(), num_dof());
        assembly.stiffness.setFromTriplets(triplets.stiffness_triplets.cbegin(), triplets.stiffness_triplets.cend());

        // Mass
        assembly.mass = Eigen::SparseMatrix<Scalar>(num_dof(), num_dof());
        assembly.mass.setFromTriplets(triplets.mass_triplets.cbegin(), triplets.mass_triplets.cend());

        return assembly;
    }

    template <typename Scalar>
    template <int QuadStrength, typename Function2d>
    VectorX<Scalar> LagrangeBasis2d<Scalar>::load(const Function2d & f) const
    {
        Eigen::VectorXd load(_mesh.num_vertices());
        load.setZero();

        // See triquad.hpp for the mapping used here
        const auto a_basis = [] (auto x, auto  ) { return Scalar(0.5) * x + Scalar(0.5); };
        const auto b_basis = [] (auto  , auto y) { return Scalar(0.5) * y + Scalar(0.5); };
        const auto c_basis = [] (auto x, auto y) { return Scalar(0.5) * (-x - y); };

        for (const auto element : _mesh.elements())
        {
            const auto z0 = element.vertex_indices[0];
            const auto z1 = element.vertex_indices[1];
            const auto z2 = element.vertex_indices[2];

            const auto & a = _mesh.vertices()[z0];
            const auto & b = _mesh.vertices()[z1];
            const auto & c = _mesh.vertices()[z2];
            const auto transform = triquad_transform(a, b, c);
            const auto transformed_f = [&f, &transform] (auto x, auto y)
            {
                const auto coords = transform.transform_from_reference(x, y);
                return f(coords.x, coords.y);
            };

            const auto absdet = transform.absolute_determinant();

            load(z0) += absdet * triquad_ref<QuadStrength, Scalar>(
                    [&] (auto x, auto y) { return transformed_f(x, y) * a_basis(x, y); }
            );
            load(z1) += absdet * triquad_ref<QuadStrength, Scalar>(
                    [&] (auto x, auto y) { return transformed_f(x, y) * b_basis(x, y); }
            );
            load(z2) += absdet * triquad_ref<QuadStrength, Scalar>(
                    [&] (auto x, auto y) { return transformed_f(x, y) * c_basis(x, y); }
            );
        }

        return load;
    }

    template <typename Scalar>
    template <typename Function2d>
    VectorX<Scalar> LagrangeBasis2d<Scalar>::interpolate(const Function2d & f) const
    {
        // Simple nodal interpolation
        auto result = VectorX<Scalar>(_mesh.num_vertices());
        for (int i = 0; i < _mesh.num_vertices(); ++i)
        {
            const auto vertex = _mesh.vertices()[i];
            result(i) = f(vertex.x, vertex.y);
        }
        return result;
    }

    template <typename Scalar>
    template <typename Function2d>
    VectorX<Scalar> LagrangeBasis2d<Scalar>::interpolate_boundary(const Function2d & f) const
    {
        // Simple nodal interpolation
        auto result = VectorX<Scalar>(_mesh.num_boundary_vertices());
        const auto & boundary_indices = _mesh.boundary_vertices();
        for (int i = 0; i < _mesh.num_boundary_vertices(); ++i)
        {
            const auto vertex_index = boundary_indices[i];
            const auto vertex = _mesh.vertices()[vertex_index];
            result(i) = f(vertex.x, vertex.y);
        }
        return result;
    }

    template <typename Scalar>
    template <int QuadStrength, typename Function2d>
    Scalar LagrangeBasis2d<Scalar>::error_l2(const Function2d &f, const VectorX<Scalar> & weights) const
    {
        Scalar error_squared = Scalar(0);

        // See triquad.hpp for the mapping used here
        const auto basis0 = [] (auto x, auto  ) { return Scalar(0.5) * x + Scalar(0.5); };
        const auto basis1 = [] (auto  , auto y) { return Scalar(0.5) * y + Scalar(0.5); };
        const auto basis2 = [] (auto x, auto y) { return Scalar(0.5) * (-x - y); };

#pragma omp parallel for reduction(+:error_squared)
        for (int element_index = 0; element_index < _mesh.num_elements(); ++element_index)
        {
            const auto element = _mesh.elements()[element_index];
            const auto z0 = element.vertex_indices[0];
            const auto z1 = element.vertex_indices[1];
            const auto z2 = element.vertex_indices[2];

            const auto w0 = weights(z0);
            const auto w1 = weights(z1);
            const auto w2 = weights(z2);

            const auto & a = _mesh.vertices()[z0];
            const auto & b = _mesh.vertices()[z1];
            const auto & c = _mesh.vertices()[z2];
            const auto transform = triquad_transform(a, b, c);
            const auto f_ref = [&f, &transform] (auto x, auto y)
            {
                const auto coords = transform.transform_from_reference(x, y);
                return f(coords.x, coords.y);
            };

            // Computes the square of the difference of f and f_h in the reference triangle
            const auto diff_ref_squared = [&] (auto x, auto y)
            {
                const auto f_h_ref = w0 * basis0(x, y) +
                                     w1 * basis1(x, y) +
                                     w2 * basis2(x, y);

                const auto diff = f_ref(x, y) - f_h_ref;
                return diff * diff;
            };

            const auto absdet = transform.absolute_determinant();
            error_squared += absdet * triquad_ref<QuadStrength, Scalar>(diff_ref_squared);
        }

        return std::sqrt(error_squared);
    };

    template <typename Scalar>
    template <int QuadStrength, typename Function2d_x, typename Function2d_y>
    Scalar LagrangeBasis2d<Scalar>::error_h1_semi(const Function2d_x & f_x,
                                                  const Function2d_y & f_y,
                                                  const VectorX<Scalar> & weights) const
    {
        Scalar error_squared = Scalar(0);

        // See triquad.hpp for the mapping used here
        const auto basis0_x = Scalar(0.5);
        const auto basis0_y = Scalar(0.0);
        const auto basis1_x = Scalar(0.0);
        const auto basis1_y = Scalar(0.5);
        const auto basis2_x = Scalar(-0.5);
        const auto basis2_y = Scalar(-0.5);

#pragma omp parallel for reduction(+:error_squared)
        for (int element_index = 0; element_index < _mesh.num_elements(); ++element_index)
        {
            const auto element = _mesh.elements()[element_index];
            const auto z0 = element.vertex_indices[0];
            const auto z1 = element.vertex_indices[1];
            const auto z2 = element.vertex_indices[2];

            const auto w0 = weights(z0);
            const auto w1 = weights(z1);
            const auto w2 = weights(z2);

            const auto & a = _mesh.vertices()[z0];
            const auto & b = _mesh.vertices()[z1];
            const auto & c = _mesh.vertices()[z2];
            const auto transform = triquad_transform(a, b, c);

            const auto f_grad_ref = [&f_x, &f_y, &transform] (auto x, auto y)
            {
                const auto coords = transform.transform_from_reference(x, y);
                Eigen::Matrix<Scalar, 2, 1> grad;
                grad(0) = f_x(coords.x, coords.y);
                grad(1) = f_y(coords.x, coords.y);
                return grad;
            };

            // Since we have linear elements, the gradients are constants
            Eigen::Matrix<Scalar, 2, 1> f_h_grad_ref;
            f_h_grad_ref(0) = w0 * basis0_x +
                              w1 * basis1_x +
                              w2 * basis2_x;
            f_h_grad_ref(1) = w0 * basis0_y +
                              w1 * basis1_y +
                              w2 * basis2_y;

            // Due to change of variables, we have to left-apply J^-T
            const Eigen::Matrix<Scalar, 2, 2> J_inv_t = transform.jacobian().inverse().transpose();
            const Eigen::Matrix<Scalar, 2, 1> f_h_grad_ref_transformed = J_inv_t * f_h_grad_ref;

            // Computes the square of the difference of grad(f) and grad(f_h)
            // in the reference triangle
            const auto diff_squared = [&] (auto x, auto y)
            {
                // Note that J_inv_t cancels with J_t for f_grad_ref
                const Eigen::Matrix<Scalar, 2, 1> diff = f_grad_ref(x, y) - f_h_grad_ref_transformed;
                return diff.dot(diff);
            };

            const auto absdet = transform.absolute_determinant();
            error_squared += absdet * triquad_ref<QuadStrength, Scalar>(diff_squared);
        }

        return std::sqrt(error_squared);
    };
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

template <typename T>
std::unordered_map<size_t, T> group_count(std::vector<T>& v) {
	std::unordered_map<size_t, T> c;
	for(T& e : v) {
		c[e]++;
	}
	return c;
}

double memo[301][301][301];

double solve(int n, int c1, int c2, int c3) {
	if(memo[c1][c2][c3] >= 0) {
		return memo[c1][c2][c3];
	}
	if(c1 == 0 && c2 == 0 && c3 == 0) {
		return 0.0;
	}

	return memo[c1][c2][c3] =
			   ((c1 > 0 ? (solve(n, c1 - 1, c2, c3) * c1) : 0.0) +
				(c2 > 0 ? (solve(n, c1 + 1, c2 - 1, c3) * c2) : 0.0) +
				(c3 > 0 ? (solve(n, c1, c2 + 1, c3 - 1) * c3) : 0.0) + n) /
			   static_cast<double>(c1 + c2 + c3);
}

int main() {
	int n;
	cin >> n;
	VI a(n);
	REP(i, n) { cin >> a[i]; }
	unordered_map<size_t, int> cnt = group_count(a);
	FILL(memo, -1);
	cout << PRECISION(15) << solve(n, cnt[1], cnt[2], cnt[3]) << endl;
	return 0;
}