/*
 * Copyright 2022 Sean McBane under the terms of the MIT license.
 * This file contains validation via method of manufactured solution for the
 * linear elasticity code. The analytical solution for displacement is given
 * by
 *
 * u_x = (-3.0/2.0*pow(x, 3) + (23.0/4.0)*pow(x, 2) - 5*x)*sin(M_PI*y) +
 *  ((3.0/2.0)*pow(x, 3) - 13.0/4.0*pow(x, 2) + 1)*cos(M_PI*y);
 *
 * u_y = -2*x*y + x + 2*y - 1;
 *
 * Young's modulus: E = 1 + sin(M_PI * x) * cos(M_PI * y) / 4;
 * Poisson's ratio: nu = 3 / 10;
 *
 * The functions below are all derived symbolically from these solutions.
 */

#include <Eigen/Core>
#include <Eigen/SparseCholesky>
#include <cassert>
#include <chrono>
#include <cmath>

#include "convert_to_eigen.hpp"
#include "elasticity.hpp"
#include "get_triangles.hpp"
#include "mesh_tools.hpp"
#include "read_mesh.hpp"

using namespace Elasticity;
using namespace std::chrono;

double manufactured_ux(double x, double y)
{
    return (-3.0 / 2.0 * pow(x, 3) + (23.0 / 4.0) * pow(x, 2) - 5 * x) * sin(M_PI * y) +
           ((3.0 / 2.0) * pow(x, 3) - 13.0 / 4.0 * pow(x, 2) + 1) * cos(M_PI * y);
}

double manufactured_uy(double x, double y) { return -2 * x * y + x + 2 * y - 1; }

Eigen::Vector2d u_true(std::array<double, 2> coord)
{
    const auto [x, y] = coord;
    return Eigen::Vector2d(manufactured_ux(x, y), manufactured_uy(x, y));
}

Eigen::Vector2d u_right(std::array<double, 2> coord) { return u_true(coord); }

Eigen::Vector2d u_left(std::array<double, 2> coord) { return u_true(coord); }

Eigen::Vector2d u_upper(std::array<double, 2> coord) { return u_true(coord); }

Eigen::Vector2d u_lower(std::array<double, 2> coord) { return u_true(coord); }

/*
 * Definitions of the Lame parameters for the manufactured solution.
 */
double manufactured_lambda(std::array<double, 2> coord)
{
    auto [x, y] = coord;
    return (15.0 / 182.0) * sin(M_PI * x) * cos(M_PI * y) + 30.0 / 91.0;
}

double manufactured_mu(std::array<double, 2> coord)
{
    auto [x, y] = coord;
    return (5.0 / 52.0) * sin(M_PI * x) * cos(M_PI * y) + 5.0 / 13.0;
}

/*
 * Manufactured solution traction boundary condition for the lower boundary.
 */
double traction_lower_x(double x, [[maybe_unused]] double y) noexcept
{
    return (5.0 / 208.0) * (M_PI * x * (6 * pow(x, 2) - 23 * x + 20) - 4) * (sin(M_PI * x) + 4);
}

double traction_lower_y(double x, [[maybe_unused]] double y) noexcept
{
    return (5.0 / 364.0) * (sin(M_PI * x) + 4) * (-27 * pow(x, 2) + 79 * x - 40);
}

Eigen::Vector2d traction_lower(std::array<double, 2> coord) noexcept
{
    auto [x, y] = coord;
    return Eigen::Vector2d(traction_lower_x(x, y), traction_lower_y(x, y));
}

/*
 * Manufactured solution traction boundary condition for the upper boundary.
 */
double traction_upper_x(double x, [[maybe_unused]] double y) noexcept
{
    return (5.0 / 208.0) * (-M_PI * x * (6 * pow(x, 2) - 23 * x + 20) + 4) * (sin(M_PI * x) - 4);
}

double traction_upper_y(double x, [[maybe_unused]] double y) noexcept
{
    return (5.0 / 364.0) * (sin(M_PI * x) - 4) * (27 * pow(x, 2) + x - 40);
}

Eigen::Vector2d traction_upper(std::array<double, 2> coord) noexcept
{
    auto [x, y] = coord;
    return Eigen::Vector2d(traction_upper_x(x, y), traction_upper_y(x, y));
}

/*
 * Manufactured solution forcing term on \Omega
 */
double forcing_x(double x, double y) noexcept
{
    return (5.0 / 52.0) * ((13 - 18 * x) * cos(M_PI * y) + (18 * x - 23) * sin(M_PI * y)) *
               (sin(M_PI * x) * cos(M_PI * y) + 4) +
           (15.0 / 364.0) * (sin(M_PI * x) * cos(M_PI * y) + 4) *
               ((13 - 18 * x) * cos(M_PI * y) + (18 * x - 23) * sin(M_PI * y) + 4) +
           (5.0 / 208.0) * (sin(M_PI * x) * cos(M_PI * y) + 4) *
               (-pow(M_PI, 2) * x * (6 * pow(x, 2) - 23 * x + 20) * sin(M_PI * y) +
                pow(M_PI, 2) * (6 * pow(x, 3) - 13 * pow(x, 2) + 4) * cos(M_PI * y) + 8) -
           5.0 / 52.0 * M_PI *
               (x * (9 * x - 13) * cos(M_PI * y) +
                (-9 * pow(x, 2) + 23 * x - 10) * sin(M_PI * y)) *
               cos(M_PI * x) * cos(M_PI * y) +
           (15.0 / 364.0) * M_PI *
               (-x * (9 * x - 13) * cos(M_PI * y) + 4 * x +
                (9 * pow(x, 2) - 23 * x + 10) * sin(M_PI * y) - 4) *
               cos(M_PI * x) * cos(M_PI * y) -
           5.0 / 208.0 * M_PI *
               (M_PI * x * (6 * pow(x, 2) - 23 * x + 20) * cos(M_PI * y) + 8 * y +
                M_PI * (6 * pow(x, 3) - 13 * pow(x, 2) + 4) * sin(M_PI * y) - 4) *
               sin(M_PI * x) * sin(M_PI * y);
}

double forcing_y(double x, double y) noexcept
{
    return (5.0 / 1456.0) * M_PI *
           ((112 - 112 * x) * sin(M_PI * x) * sin(M_PI * y) +
            26 * (sin(M_PI * x) * cos(M_PI * y) + 4) *
                (x * (9 * x - 13) * sin(M_PI * y) +
                 (9 * pow(x, 2) - 23 * x + 10) * cos(M_PI * y)) -
            12 *
                (-x * (9 * x - 13) * cos(M_PI * y) + 4 * x +
                 (9 * pow(x, 2) - 23 * x + 10) * sin(M_PI * y) - 4) *
                sin(M_PI * x) * sin(M_PI * y) +
            7 *
                (M_PI * x * (6 * pow(x, 2) - 23 * x + 20) * cos(M_PI * y) + 8 * y +
                 M_PI * (6 * pow(x, 3) - 13 * pow(x, 2) + 4) * sin(M_PI * y) - 4) *
                cos(M_PI * x) * cos(M_PI * y));
}

Eigen::Vector2d forcing(std::array<double, 2> coord) noexcept
{
    auto [x, y] = coord;
    return Eigen::Vector2d(forcing_x(x, y), forcing_y(x, y));
}

auto compute_parameter_functions(const MeshVariant &mv)
{
    Eigen::VectorXd lambda = evaluate_on_mesh(mv, manufactured_lambda);
    Eigen::VectorXd mu = evaluate_on_mesh(mv, manufactured_mu);
    return std::pair(lambda, mu);
}

auto get_boundary_conditions(const MeshVariant &mv, size_t right_bound, size_t left_bound)
{
    Eigen::Matrix2Xd uright = evaluate_on_boundary(mv, right_bound, u_right);
    Eigen::Matrix2Xd uleft = evaluate_on_boundary(mv, left_bound, u_left);

    return std::pair(uright, uleft);
}

namespace
{

Eigen::Matrix2Xd solve(const MeshVariant &mv)
{
    size_t right_bound = find_boundary_with_tag(mv, "RIGHT").value();
    size_t left_bound = find_boundary_with_tag(mv, "LEFT").value();
    size_t upper_bound = find_boundary_with_tag(mv, "UPPER").value();
    size_t lower_bound = find_boundary_with_tag(mv, "LOWER").value();

    auto tp_start = steady_clock::now();
    Eigen::Matrix2Xd force_coefficients = evaluate_on_mesh(mv, forcing);
    Eigen::Matrix2Xd nodal_forcing = Elasticity::integrate_volume_force(mv, force_coefficients);

    Eigen::Matrix2Xd traction_upper_coefficients =
        evaluate_on_boundary(mv, upper_bound, traction_upper);
    Eigen::Matrix2Xd traction_lower_coefficients =
        evaluate_on_boundary(mv, lower_bound, traction_lower);

    Eigen::Matrix2Xd traction_upper =
        integrate_traction_force(mv, upper_bound, traction_upper_coefficients);
    Eigen::Matrix2Xd traction_lower =
        integrate_traction_force(mv, lower_bound, traction_lower_coefficients);

    add_traction_force(mv, nodal_forcing, upper_bound, traction_upper);
    add_traction_force(mv, nodal_forcing, lower_bound, traction_lower);

    auto tp_end = steady_clock::now();

    size_t us = duration_cast<microseconds>(tp_end - tp_start).count();
    printf("Assembling forcing took %ld us\n", us);

    Eigen::VectorXd lambda, mu;
    std::tie(lambda, mu) = compute_parameter_functions(mv);

    Eigen::Matrix2Xd uright, uleft;
    std::tie(uright, uleft) = get_boundary_conditions(mv, right_bound, left_bound);

    tp_start = steady_clock::now();
    auto K_unassembled = Elasticity::assemble_stiffness(mv, lambda, mu);

    auto flat_forcing = nodal_forcing.reshaped();
    Elasticity::impose_dirichlet_condition(mv, K_unassembled, flat_forcing, right_bound, uright);
    Elasticity::impose_dirichlet_condition(mv, K_unassembled, flat_forcing, left_bound, uleft);

    auto K = convert_to_eigen(K_unassembled);
    tp_end = steady_clock::now();
    us = duration_cast<microseconds>(tp_end - tp_start).count();
    printf("Assembling stiffness took %ld us\n", us);

    tp_start = steady_clock::now();
    Eigen::Matrix2Xd u(2, nodal_forcing.cols());
    u.reshaped() = Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper>(K).solve(
        nodal_forcing.reshaped());
    tp_end = steady_clock::now();
    us = duration_cast<microseconds>(tp_end - tp_start).count();
    printf("Linear system solve took %ld us\n", us);

    return u;
}

} // namespace

int main(int argc, char *argv[])
{
    if (!(argc > 1))
    {
        fprintf(stderr, "Usage: ./manufactured [mesh order] [mesh files...]\n");
        return 1;
    }

    int order = atoi(argv[1]);
    if (!(order >= 1 && order <= 4))
    {
        fprintf(stderr, "Got mesh order %d; order should be 1-4\n", order);
        return 2;
    }

    for (int fi = 2; fi < argc; ++fi)
    {
        FILE *infile = fopen(argv[fi], "r");
        if (!infile)
        {
            fprintf(stderr, "Skipping mesh file %s (couldn't be opened)\n", argv[fi]);
            continue;
        }
        fclose(infile);
        auto mesh = read_mesh(argv[fi], order);
        FILE *outfile = fopen("mesh_elements.dat", "wb");
        if (!outfile)
        {
            fprintf(stderr, "Skipping output, couldn't open file\n");
        }
        else
        {
            get_triangle_info(mesh).serialize(outfile);
            fclose(outfile);
        }
        Eigen::Matrix2Xd u = solve(mesh);
        outfile = fopen("u.dat", "wb");
        int64_t dims[] = {u.rows(), u.cols()};
        fwrite(dims, sizeof(int64_t), 2, outfile);
        fwrite(u.data(), sizeof(double), u.size(), outfile);
        fclose(outfile);

        auto [l2_error, u_norm] = Elasticity::integrate_error(mesh, u, u_true);

        printf(
            "Error for input file %s:  %E (L^2 error)  %E (L^2 u)\n", argv[fi], l2_error, u_norm);
    }

    return 0;
}


#include "clustering/spectral_cluster.h"
#include "clustering/kmeans.h"

#include <algorithm>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Spectra/SymEigsSolver.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include <glog/logging.h>

#include "util/timer.h"

namespace GraphSfM {

std::unordered_map<int, int> SpectralCluster::ComputeCluster(
        const std::vector<std::pair<int, int>>& edges,
        const std::vector<int>& weights,
        const int num_partitions)
{
    if (num_partitions == 1) {
        for (auto node : nodes_) {
            labels_[node] = 0;
        }
        return labels_;
    }

    colmap::Timer timer;
    std::vector<Eigen::Triplet<double>> s_triplets;
    std::unordered_map<int, int> degrees;
    const int k = num_partitions;
    cluster_num_ = num_partitions;

    for (uint i = 0; i < edges.size(); i++) {
        auto edge = edges[i];
        nodes_.push_back(edge.first);
        nodes_.push_back(edge.second);
    }

    std::sort(nodes_.begin(), nodes_.end());
    nodes_.erase(std::unique(nodes_.begin(), nodes_.end()), nodes_.end());

    // 1. Compute similarity graph.
    for (uint i = 0; i < nodes_.size(); i++) {
        node_mapper_[nodes_[i]] = i;
    }

    timer.Start();
    const int N = nodes_.size();
    Eigen::SparseMatrix<double> S(N, N);
    for (uint i = 0; i < edges.size(); i++) {
        int src = edges[i].first, dst = edges[i].second;
        s_triplets.push_back(Eigen::Triplet<double>(src, dst, weights[i]));
        s_triplets.push_back(Eigen::Triplet<double>(dst, src, weights[i]));
        degrees[src] += 2;
    }

    S.setFromTriplets(s_triplets.begin(), s_triplets.end());
    S.makeCompressed();
    timer.Pause();
    LOG(INFO) << "1. Similarity Graph Computation Time: " << timer.ElapsedSeconds();

    // 2. Compute Laplacian matrix.
    timer.Start();
    Eigen::SparseMatrix<double> L = ComputeLaplacian(S, degrees);
    L.makeCompressed();
    timer.Pause();
    LOG(INFO) << "2. Laplacian Matrix Computation Time: " << timer.ElapsedSeconds();

    // 3. Compute the top-k smallest eigen values and corresponding eigen vectors.
    timer.Start();
    Spectra::SparseSymMatProd<double> op(L);
    Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE,
                          Spectra::SparseSymMatProd<double>> eigs(&op,
                                                                  k,
                                                                  std::min(2 * k, N));
    eigs.init();
    int nconv = eigs.compute();

    Eigen::VectorXd eigen_values;
    Eigen::MatrixXd eigen_vectors;
    if (eigs.info() == Spectra::SUCCESSFUL) {
        eigen_values = eigs.eigenvalues();
        eigen_vectors = eigs.eigenvectors();
    }

    timer.Pause();
    LOG(INFO) << "3. EigenValue Computation Time: " << timer.ElapsedSeconds();

    // 4. Reverse original eigen vectors(as it stored in descending order)
    timer.Start();
    uint i = 0, j = k - 1;
    while (i < j) {
        const Eigen::VectorXd tmp = eigen_vectors.col(i);
        eigen_vectors.col(i) = eigen_vectors.col(j);
        eigen_vectors.col(j) = tmp;
        i++; j--;
    }

    std::vector<Eigen::VectorXd> source_data;
    source_data.reserve(eigen_vectors.rows());
    for (uint i = 0; i < eigen_vectors.rows(); i++) {
        source_data.push_back(eigen_vectors.row(i));
    }
    timer.Pause();
    LOG(INFO) << "4. Eigen Vectors Reverse Time: " << timer.ElapsedSeconds();

    // 5. Invert K-Means for clustering.
    timer.Start();
    std::vector<uint32_t> cluster_assignment;
    std::vector<Eigen::VectorXd> centers;
    KMeans(source_data, cluster_assignment, centers, k);
    timer.Pause();
    LOG(INFO) << "5. KMeans Time: " << timer.ElapsedSeconds();

    for (uint i = 0; i < cluster_assignment.size(); i++) {
        labels_[nodes_[i]] = cluster_assignment[i];
    }
    return labels_;
}

Eigen::SparseMatrix<double> SpectralCluster::ComputeLaplacian(
                const Eigen::SparseMatrix<double>& S,
                const std::unordered_map<int, int>& degrees) const
{
    // Compute degree matrix.
    const int N = degrees.size();
    Eigen::SparseMatrix<double> D(N, N);
    // Eigen::SparseMatrix<double> D_inv(N, N);
    // Eigen::SparseMatrix<double> D_sqrt(N, N);
    for (auto it = degrees.begin(); it != degrees.end(); ++it) {
        int id = it->first;
        D.insert(node_mapper_.at(id), node_mapper_.at(id)) = it->second;
        // D_inv.insert(node_mapper_.at(id), node_mapper_.at(id)) = 1.0 / it->second;
        // D_sqrt.insert(node_mapper_.at(id), node_mapper_.at(id)) = -1.0 / sqrt(it->second);
    }
    D.makeCompressed();
    // D_inv.makeCompressed();
    // D_sqrt.makeCompressed();

    // Compute Laplacian matrix.
    Eigen::SparseMatrix<double> L = D - S;
    // Eigen::MatrixXd L_random = D_inv * L;
    // Eigen::MatrixXd L_sym = D_sqrt * L * D_sqrt;

    return L;
}

} // namespace GraphSfM


#include "stats/rmsd.h"
#include <xmd/utils/convert.h>
#include <Eigen/Core>
#include <Eigen/SVD>

namespace xmd {
    void compute_rmsd::operator()() const {
        using matrix_t = Eigen::Matrix<real, Eigen::Dynamic, 3>;
        matrix_t P = matrix_t::Zero(num_particles, 3);
        matrix_t Q = matrix_t::Zero(num_particles, 3);

        for (int idx = 0; idx < num_particles; ++idx) {
            P.row(idx) = convert<real>(r[idx]);
            Q.row(idx) = convert<real>(ref_r[idx]);
        }

        Eigen::Matrix<real, 3, 3> H = P.transpose() * Q;
        auto svd = Eigen::JacobiSVD<decltype(H)>(H,
            Eigen::ComputeFullU & Eigen::ComputeFullV);

        auto U = svd.matrixU();
        auto V = svd.matrixV();
        auto d = (V * U.transpose()).determinant() > 0 ? 1 : -1;
        auto R = V * Eigen::DiagonalMatrix<real, 3>(1, 1, d) * U.transpose();

        auto D = (P - Q) * R;
        real rmsd_ = 0.0;
        for (int idx = 0; idx < num_particles; ++idx) {
            rmsd_ += D.row(idx).squaredNorm();
        }
        *rmsd = sqrt(rmsd_ / num_particles);
    }
}


// =========================================================================
// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
// =========================================================================

#include <fstream>
#include <iostream>
#include <iomanip>
#include <random>
#include <sstream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/SVD>

// -- Some typedefs
using TScalar = double;
using TMatrix = Eigen::Matrix< TScalar, Eigen::Dynamic, Eigen::Dynamic >;

// -- Helper functions
void read_file( TMatrix& X, const std::string& filename );

// -- Main function
int main( int argc, char** argv )
{
  // Check inputs and get them
  if( argc < 2 )
  {
    std::cerr
      << "Usage: " << argv[ 0 ] << " input.bin"
      << std::endl;
    return( 1 );
  } // end if
  std::string input = argv[ 1 ];

  // Read data
  TMatrix X;
  std::cout << "Read... " << std::flush;
  read_file( X, input );
  std::cout << "done! " << X.rows( ) << " " << X.cols( ) << std::endl;

  // Compute PCA
  std::cout << "Mean... " << std::flush;
  auto m = X.colwise( ).mean( );
  std::cout << "done! " << m.rows( ) << " " << m.cols( ) << std::endl;
  std::cout << "Centering... " << std::flush;
  auto c = X.rowwise( ) - m;
  std::cout << "done! " << c.rows( ) << " " << c.cols( ) << std::endl;
  std::cout << "Covariance... " << std::flush;
  auto S = ( c.transpose( ) * c ) / TScalar( X.cols( ) );
  std::cout << "done! " << S.rows( ) << " " << S.cols( ) << std::endl;
  std::cout << "Eigen analysis... " << std::flush;
  auto svd = S.bdcSvd( Eigen::ComputeFullU | Eigen::ComputeFullV );
  std::cout << "done!" << std::endl;
  TMatrix eR = svd.matrixU( );
  TMatrix ev = svd.singularValues( );
  TScalar sv = ev.sum( );

  unsigned long i = 0;
  double s = ev( i, 0 ) / sv;
  while( i < ev.rows( ) && s <= 0.95 )
  {
    i++;
    s += ev( i, 0 ) / sv;
  } // end while
  TMatrix Xp = ( X * eR ).block( 0, 0, X.rows( ), i );
  std::cout << i << " " << Xp.rows( ) << " " << Xp.cols( ) << std::endl;

  unsigned long xrows, xcols;
  xrows = Xp.rows( );
  xcols = Xp.cols( );

  std::ofstream out = std::ofstream( "out.bin", std::ios::binary );
  out.write( ( char* )( &xrows ), sizeof( unsigned long ) );
  out.write( ( char* )( &xcols ), sizeof( unsigned long ) );
  out.write( ( char* )( Xp.data( ) ), sizeof( TScalar ) * xrows * xcols );
  out.close( );

  /* TODO
     this->m_EigenValues.SetSize( ps );
     for( unsigned int d = 0; d < ps; ++d )
     this->m_EigenValues[ d ] = ev( d, 0 );
  */

  return( 0 );
}

// -------------------------------------------------------------------------
void read_file( TMatrix& X, const std::string& filename )
{
  std::ifstream Xreader = std::ifstream( filename, std::ios::binary );
  unsigned long xrows, xcols;
  Xreader.read( ( char* )( &xrows ), sizeof( unsigned long ) );
  Xreader.read( ( char* )( &xcols ), sizeof( unsigned long ) );
  X = TMatrix::Zero( xrows, xcols );
  Xreader.read( ( char* )( X.data( ) ), sizeof( TScalar ) * xrows * xcols );
  Xreader.close( );
}

// eof - pca.cxx


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

template <typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
toeplitz(const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& vec)
{
  index size = vec.size();

  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> mat(size, size);

  for (auto i = 0; i < size; i++)
  {
    for (auto j = 0; j < i; j++) mat(j, i) = vec(i - j);
    for (auto j = i; j < size; j++) mat(j, i) = vec(j - i);
  }

  return mat;
}

} // namespace algorithm
} // namespace fluid


/*
 * recon.cpp
 *
 *  Created on: Jul 25, 2019
 *      Author: dmarce1
 */
#include "../../octotiger/print.hpp"
#include <stdio.h>
#include <ctype.h>
#include <stdlib.h>
#include <map>
#include <vector>
#include <math.h>
#include <iostream>
#include <complex>
#include <boost/program_options.hpp>

auto band_filter(double t, double Ps, double Pc) {
	const auto x = 2.0 * M_PI * t / Pc;
	const auto y = 2.0 * M_PI * (t / Ps + 0.5);
	const auto sinc = x == 0 ? 1.0 : sin(x) / x;
	constexpr auto a0 = 7938.0 / 18608.0;
	constexpr auto a1 = 9240.0 / 18608.0;
	constexpr auto a2 = 1430.0 / 18608.0;
	const auto blackman = (a0 - a1 * cos(y) + a2 * cos(2 * y));
	if (blackman > 0.0) {
		return sinc * blackman;
	} else {
		return 0.0;
	}
}

void output_spectrogram(double Ps, double Pc) {
	double pmin = Pc / 100.0;
	double pmax = 2.0 * Ps;
	double dp = (pmax - pmin) / 1000.0;
	const auto dt = Ps / 100000.0;
	FILE *fp = fopen("filter.dat", "wt");
	for (auto p = pmin; p <= pmax; p += dp) {
		double w = 0.0;
		const auto omega = 2.0 * M_PI / p;
		double sum = 0.0;
		constexpr std::complex<double> I(0, 1);
		for (double t = dt / 2.0; t < Ps / 2.0; t += dt) {
			const auto bf = band_filter(t, Ps, Pc);
			w += 2.0 * bf * dt;
			sum += 2.0 * bf * cos(omega * t) * dt;
		}
		sum /= w;
		fprintf(fp, "%.12e %.12e\n", p, sum);
	}
	fclose(fp);

}

auto derivative(const std::vector<std::vector<double>> &f, double omega) {
	std::vector<std::vector<double>> g;
	std::vector<double> u(f[0].size());
	const auto P = 2.0 * M_PI / omega;
	for (int n = 1; n < f.size() - 1; n++) {
		const auto nm = n - 1;
		const auto np = n + 1;
		const auto h1 = (f[n][0] - f[nm][0]);
		const auto h2 = (f[np][0] - f[n][0]);
		for (int i = 0; i < u.size(); i++) {
			u[i] = P * (f[np][i] * h1 * h1 + f[n][i] * (h2 * h2 - h1 * h1) - f[nm][i] * h2 * h2) / (f[0][i] * h1 * h2 * (h1 + h2));
		}
		u[0] = f[n][0];
		g.push_back(u);
	}

	return g;

}

auto filter(const std::vector<std::vector<double>> &f, double omega, double Pc, double Ps) {
	std::vector<std::vector<double>> g;
	std::vector<double> u(f[0].size());

	const auto tmin = f[0][0];
	const auto tmax = f[f.size() - 1][0];
	const auto P = 2.0 * M_PI / omega;
	double rt = tmin / P;
	Pc *= P;
	Ps *= P;
	for (int n = 1; n < f.size() - 1; n++) {
		double t0 = f[n][0];
		double dt;
		double weight = 0.0;

		if (Pc / 2.0 + tmin < t0 && t0 < tmax - Pc / 2.0) {
			std::fill(u.begin(), u.end(), 0.0);
			for (int m = 1; m < f.size() - 1; m++) {
				double t = f[m][0];
				if (t0 - Pc / 2.0 < t && t < t0 + Pc / 2.0) {
					dt = (f[m + 1][0] - f[m - 1][0]) / 2.0;
					const auto h1 = f[m][0] - f[m - 1][0];
					const auto h2 = f[m + 1][0] - f[m][0];
					const auto w1 = (h1 + h2) * (2 * h1 - h2) / h1 / 6.0;
					const auto w2 = std::pow(h1 + h2, 3) / h2 / h1 / 6.0;
					const auto w3 = (h1 + h2) * (2 * h2 - h1) / h2 / 6.0;
					double y1 = band_filter(f[m - 1][0] - t0, Ps, Pc);
					double y2 = band_filter(f[m][0] - t0, Ps, Pc);
					double y3 = band_filter(f[m + 1][0] - t0, Ps, Pc);
					for (int i = 0; i < u.size(); i++) {
						u[i] += w1 * f[m - 1][i] * y1 * dt;
						u[i] += w2 * f[m][i] * y2 * dt;
						u[i] += w3 * f[m + 1][i] * y3 * dt;
					}
					weight += w1 * y1 * dt;
					weight += w2 * y2 * dt;
					weight += w3 * y3 * dt;
				}
			}
			for (int i = 0; i < u.size(); i++) {
				u[i] /= weight;
			}
			u[0] = rt;
			g.push_back(u);
		}
		dt = (f[n + 1][0] - f[n - 1][0]) / 2.0;
		rt += dt / P;
	}
	return g;
}

struct options {
	double pmin;
	double pmax;
	bool help;
	bool normalize;
	std::string input;

	int read_options(int argc, char *argv[]) {
		namespace po = boost::program_options;

		po::options_description command_opts("options");

		command_opts.add_options() //
		("input", po::value<std::string>(&input)->default_value("binary.dat"), "input filename")           //
		("pmin", po::value<double>(&pmin)->default_value(1.25), "minimum period to allow through filter")           //
		("pmax", po::value<double>(&pmax)->default_value(5), "period where filter allows 100%")           //
		("normalize", po::value<bool>(&normalize)->default_value(true), "normalize averages to t=0 value")           //
		("help", po::value<bool>(&help)->default_value(false), "show the help page")           //
				;
		boost::program_options::variables_map vm;
		po::store(po::parse_command_line(argc, argv, command_opts), vm);
		po::notify(vm);

		if (help) {
			std::cout << command_opts << "\n";
			return -1;
		}
		FILE *fp = fopen(input.c_str(), "rb");
		if (fp == NULL) {
			print("Unable to open %s\n", input.c_str());
			return -1;
		} else {
			fclose(fp);
		}

		return 0;
	}
};
int main(int argc, char *argv[]) {
	options opts;
	if (opts.read_options(argc, argv) == 0) {

		FILE *fp = fopen(opts.input.c_str(), "rt");
		static char buffer[100000];
		std::map<double, std::vector<double>> values;

		while (!feof(fp)) {
			const char* b = fgets(buffer, 100000, fp);
			bool done = false;
			char *ptr = buffer;
			double t;
			int col = 0;
			std::vector<double> these_values;
			do {
				while (isspace(*ptr)) {
					if (*ptr == '\n') {
						done = true;
						break;
					}
					ptr++;
				}
				if (!done) {
					double number = atof(ptr);
					if (col == 0) {
						t = number;
					}
					these_values.push_back(number);
					col++;
				}
				while (!isspace(*ptr)) {
					ptr++;
				}
			} while (!done);
			if (values.find(t) == values.end()) {
				values.insert(std::make_pair(t, std::move(these_values)));
			}
		}

		std::vector<std::vector<double>> v1;
		for (auto &v : values) {
			v1.push_back(std::move(v.second));
		}

		auto Pc = 2.0 * opts.pmax * opts.pmin / (opts.pmax + opts.pmin);
		auto Ps = 4.0 * opts.pmax * opts.pmin / (opts.pmax - opts.pmin);
		print("sinc period      = %e\n", Pc);
		print("Blackmann period = %e\n", Ps);
		const auto P = 2.0 * M_PI / v1[0][2];

		print("Window is +/- %.12e orbits\n", Pc / 2.0);

		auto v2 = filter(v1, v1[0][2], Pc, Ps);
		if (v2.size() == 0) {
			print("Not enough data to produce output\n");
		} else {
			if (opts.normalize) {
				for (auto &line : v2) {
					for (int i = 1; i < line.size(); i++) {
						line[i] /= v1[0][i];
					}
				}
			}
			const auto v4 = derivative(v1, v1[0][2]);
			const auto v3 = filter(v4, v1[0][2], Pc, Ps);
			FILE *fp1 = fopen("avg.dat", "wt");
			FILE *fp2 = fopen("drv.dat", "wt");
			for (const auto &i : v2) {
				for (const auto &j : i) {
					fprintf(fp1, " %.12e ", j);
				}
				fprintf(fp1, "\n");
			}
			for (const auto &i : v3) {
				for (const auto &j : i) {
					fprintf(fp2, " %.12e ", j);
				}
				fprintf(fp2, "\n");
			}
			fclose(fp1);
			fclose(fp2);
			fclose(fp);
		}

		output_spectrogram(Ps, Pc);
	}
	return 0;
}


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MIQP_COMMON_MATH_HPP_
#define MIQP_COMMON_MATH_HPP_

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

namespace miqp {
namespace common {
namespace math {

inline void WrapRadiantTo2Pi(double& x) {
  x = fmod(x, 2 * boost::math::constants::pi<double>());
  if (x < 0) {
    x += 2 * boost::math::constants::pi<double>();
  }
  assert(x >= 0);
  assert(x <= 2 * boost::math::constants::pi<double>());
}

inline void SwapIfNeeded(float& maxval, float& minval) {
  if (maxval < minval) {
    float tmp = maxval;
    maxval = minval;
    minval = tmp;
  }
}

inline double InterpolateWithBounds(const double x0, const double y0,
                                    const double x1, const double y1,
                                    const double xInterp) {
  double yInterp;
  if (xInterp > x1) {
    yInterp = y1;
  } else if (xInterp < x0) {
    yInterp = y0;
  } else {
    yInterp = (y1 - y0) / (x1 - x0) * (xInterp - x0) + y0;
  }
  return yInterp;
};

}  // namespace math
}  // namespace common
}  // namespace miqp

#endif  // MIQP_COMMON_MATH_HPP_


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearfeelementmatrix.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>

namespace ElementMatrixComputation {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 4, 4> MyLinearFEElementMatrix::Eval(
    const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());
  // Matrix for returning element matrix

  auto lapl_mat = lf::uscalfe::LinearFELaplaceElementMatrix().Eval(cell);

  if (ref_el == lf::base::RefElType::kQuad) {
    Eigen::Matrix<double, 4, 4> elem_mat;
    elem_mat << 4,2,1,2,
                2,4,2,1,
                1,2,4,2,
                2,1,2,4;

    double x0 = vertices.row(0).minCoeff();
    double x1 = vertices.row(0).maxCoeff();
    double y0 = vertices.row(1).minCoeff();
    double y1 = vertices.row(1).maxCoeff();

    double area = (x1 - x0) * (y1 - y0);

    return area * elem_mat / 36. + lapl_mat;
  }

  else if(ref_el == lf::base::RefElType::kTria) {
      Eigen::Matrix<double, 4, 4> elem_mat;
      elem_mat << 2,1,1,0,
                  1,2,1,0,
                  1,1,2,0,
                  0,0,0,0;

      Eigen::Matrix2d side_lengths;

      side_lengths.col(0) = vertices.col(1) - vertices.col(0);
      side_lengths.col(1) = vertices.col(2) - vertices.col(0);

      double area = 0.5 * side_lengths.determinant();

      return area * elem_mat / 12. + lapl_mat;

  }
  else {
    assert(0 && "unsupported cell type");
  }

}
/* SAM_LISTING_END_1 */
}  // namespace ElementMatrixComputation


//
// Created by TAADEJOM on 15.05.2021.
//

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

using namespace Eigen;

/**
 * Concatenates two matrices vertically. A at the top and B at the bottom
 */
MatrixXd vConcat(const MatrixXd& A, const MatrixXd& B) {
    MatrixXd D(A.rows()+B.rows(), A.cols()); // <-- D(A.rows() + B.rows(), ...)
    D << A, B;
    return D;
}

/**
 * See: https://gist.github.com/gocarlos/c91237b02c120c6319612e42fa196d77
 * Note: matrix index starts at 1 in matlab and at 0 in eigen -> never forget -1!
 */
QVector<double> DataSet::regression(int degree) const {
    QSharedPointer<QCPGraphDataContainer> data = graph->data();
    int m = data->size(); // rows
    int n = degree; // columns
    // xPoints = data(:,1);  %x -> A
    // yPoints = data(:,2); %y -> b
    MatrixXd xPoints(m, 1); // n rows, 1 column
    MatrixXd yPoints(m, 1); // n rows, 1 column
    for (int i = 0; i < m; ++i) {
        xPoints(i, 0) = data->at(i)->key;
        yPoints(i, 0) = data->at(i)->value;
    }

    // x0 = ones(m,1).*80.3e3;
    MatrixXd x1 = xPoints;
    MatrixXd x0 = Eigen::MatrixXd::Ones(m, 1) * 80300;

    MatrixXd z1 = (x1 - x0).array() / 50;

    // Set up vector A
    MatrixXd A(m, n);
    for (int i = 1; i <= n; ++i) {
        if (i == n) {
            // A(1:m, degree) = ones(m,1);
            A.col(i - 1) = Eigen::MatrixXd::Ones(m, 1);
        } else {
            // A(1:m, degree) = xPoints.^(n - degree);
            A.col(i - 1) = xPoints.array().pow(n - i);
        }
    }

    MatrixXd Im = Eigen::MatrixXd::Identity(m, m); // m*m identity matrix

    // QR-Zerlegung von A
    MatrixXd R = A;
    MatrixXd Q = Im;
    for (int k = 1; k <= n; ++k) {
        MatrixXd x = R.col(k - 1).bottomRows(R.rows() - k + 1); // we wanna show R.rows - k rows from the bottom // fixme: ErrorRowIndexOutOfBounds with 5 data points and degree 10

        // x is a matrix with 1 column and however many rows
        double lambda = x.coeff(k - 1, 0) < 0 ? x.norm() : -x.norm();

        MatrixXd e = Im.col(k - 1).bottomRows(R.rows() - k + 1);
        MatrixXd v = (x - lambda * e) / (x - lambda * e).norm(); // use normalize()
        MatrixXd z = Eigen::MatrixXd::Zero(k - 1, 1);

        // H = eye(m)-2.*[z;v]*[z;v]';
        MatrixXd H = Eigen::MatrixXd::Identity(m, m) - (2 * (vConcat(z, v) * vConcat(z, v).transpose()));

        Q = Q * H; // mXm * mXm matrix mul => VERY SLOW
        R = H * R;
    }
    // Rückwärtseinsetzen
    MatrixXd y = Q.transpose() * yPoints;

    MatrixXd x = Eigen::MatrixXd::Zero(1, n);
    // x(n) = y(n)/R(n,n);
    x.coeffRef(0, n - 1) = y.coeff(n  - 1, 0) / R.coeff(n - 1, n - 1);

    for (int i = (n - 1); i >= 1; --i) {
        double s = 0;
        for (int j = i + 1; j <= n; ++j) {
            // s = s + x(j)*R(i,j);
            s += x.coeff(0, j - 1) * R.coeff(i - 1, j - 1);
        }
        // x(i) = (y(i) - s)/R(i,i);
        x.coeffRef(0, i - 1) = (y.coeff(i - 1, 0) - s) / R.coeff(i - 1, i - 1);
    }

    return QVector<double>(x.data(), x.data() + x.rows() * x.cols());
}

/**
 * Makes a function string out of a list of coeffs
 */
QString DataSet::getFunctionString(const QVector<double>& coeffs) {
    QString regression = "";
    for (int i = 0; i < coeffs.size(); ++i) {
        regression += QString("%1*x^%2+").arg(QString::number(coeffs.at(i), 'E', 16)).arg(coeffs.length() - i - 1);
    }
    regression.truncate(regression.lastIndexOf('+'));
    return regression;
}

QDataStream &operator<<(QDataStream &out, const DataSet &dataSet) {
    out << dataSet.displayName << dataSet.functionString << dataSet.color << quint32(dataSet.graphWidth) << quint32(dataSet.pointDensity);
    return out;
}

QDataStream &operator>>(QDataStream &in, DataSet &dataSet) {
    QString displayName;
    QString functionString;
    QColor color;
    quint32 graphWidth;
    quint32 pointDensity;
    in >> displayName >> functionString >> color >> graphWidth >> pointDensity;
    dataSet.displayName = displayName;
    dataSet.functionString = functionString;
    dataSet.color = color;
    dataSet.graphWidth = graphWidth;
    dataSet.pointDensity = pointDensity;
    return in;
}


#pragma once

#include <Discregrid/common.hpp>
#include <Eigen/Dense>

class CubicKernel
{
public:
	 Discregrid::Real getRadius() { return m_radius; }
	 void setRadius(Discregrid::Real val)
	{
		m_radius = val;
		const Discregrid::Real pi = static_cast<Discregrid::Real>(M_PI);

		const Discregrid::Real h3 = m_radius*m_radius*m_radius;
		m_k = 8.0 / (pi*h3);
		m_l = 48.0 / (pi*h3);
		m_W_zero = W(Discregrid::Vector3r::Zero());
	}

public:
	Discregrid::Real W(Discregrid::Vector3r const& r)
	{
		Discregrid::Real res = 0.0;
		const Discregrid::Real rl = r.norm();
		const Discregrid::Real q = rl/m_radius;
		if (q <= 1.0)
		{
			if (q <= 0.5)
			{
				const Discregrid::Real q2 = q*q;
				const Discregrid::Real q3 = q2*q;
				res = m_k * (6.0*q3-6.0*q2+1.0);
			}
			else
			{
				auto _1mq = 1.0 - q;
				res = m_k * (2.0*_1mq*_1mq*_1mq);
			}
		}
		return res;
	}

	Discregrid::Vector3r gradW(const Discregrid::Vector3r &r)
	{
		using namespace Eigen;
		Discregrid::Vector3r res;
		const Discregrid::Real rl = r.norm();
		const Discregrid::Real q = rl / m_radius;
		if (q <= 1.0)
		{
			if (rl > 1.0e-6)
			{
				const Discregrid::Vector3r gradq = r * ((Discregrid::Real) 1.0 / (rl*m_radius));
				if (q <= 0.5)
				{
					res = m_l*q*((Discregrid::Real) 3.0*q - (Discregrid::Real) 2.0)*gradq;
				}
				else
				{
					const Discregrid::Real factor = 1.0 - q;
					res = m_l*(-factor*factor)*gradq;
				}
			}
		}
		else
			res.setZero();

		return res;
	}

	Discregrid::Real W_zero()
	{
		return m_W_zero;
	}

private:
	Discregrid::Real m_radius;
	Discregrid::Real m_k;
	Discregrid::Real m_l;
	Discregrid::Real m_W_zero;
};


#include "IntrinsicGeometry.h"
#include "MeshConnectivity.h"
#include <Eigen/Dense>

IntrinsicGeometry::IntrinsicGeometry(const MeshConnectivity &mesh, const std::vector<Eigen::Matrix2d> &abars) : abars(abars)
{
    int nedges = mesh.nEdges();
    int nfaces = mesh.nFaces();

    Js.resize(2 * nfaces, 2);
    for (int i = 0; i < nfaces; i++)
    {
        Eigen::Matrix2d Jeuclid;
        Jeuclid << 0, -1,
            1, 0;
        Js.block<2, 2>(2 * i, 0) = std::sqrt(abars[i].determinant()) * abars[i].inverse() * Jeuclid;
    }

    Ts.resize(2 * nedges, 4);
    Ts.setZero();
    for (int i = 0; i < nedges; i++)
    {
        int face1 = mesh.edgeFace(i, 0);
        int face2 = mesh.edgeFace(i, 1);
        if (face1 == -1 || face2 == -1)
            continue;

        int vert1 = mesh.edgeVertex(i, 0);
        int vert2 = mesh.edgeVertex(i, 1);

        // write edge vert1->vert2 in barycentric coordinates on each face
        Eigen::Vector2d barys[3] = { {0,0}, {1,0}, {0,1} };
        Eigen::Vector2d face1e(0, 0);
        Eigen::Vector2d face2e(0, 0);
        for (int j = 0; j < 3; j++)
        {
            if (vert1 == mesh.faceVertex(face1, j))
                face1e -= barys[j];
            else if (vert2 == mesh.faceVertex(face1, j))
                face1e += barys[j];
            if (vert1 == mesh.faceVertex(face2, j))
                face2e -= barys[j];
            else if (vert2 == mesh.faceVertex(face2, j))
                face2e += barys[j];
        }

        Eigen::Vector2d face1eperp = Js.block<2, 2>(2 * face1, 0) * face1e;
        Eigen::Vector2d face2eperp = Js.block<2, 2>(2 * face2, 0) * face2e;
        Eigen::Matrix2d face1basis;
        face1basis.col(0) = face1e;
        face1basis.col(1) = face1eperp;
        Eigen::Matrix2d face2basis;
        face2basis.col(0) = face2e;
        face2basis.col(1) = face2eperp;
        Ts.block<2, 2>(2 * i, 0) = face2basis * face1basis.inverse();
        Ts.block<2, 2>(2 * i, 2) = face1basis * face2basis.inverse();
    }
}


#include <blitz/array.h>
#include <blitz/array/stencil-et.h>

BZ_USING_NAMESPACE(blitz)

void setupInitialConditions(Array<float,3>& P1, Array<float,3>& P2,
    Array<float,3>& P3, Array<float,3>& c, int N);

BZ_DECLARE_STENCIL4(acoustic3D, P1, P2, P3, c)
    P3 = 2 * P2 + c * Laplacian3D_stencilop(P2) - P1;
BZ_END_STENCIL

float acoustic3D_BlitzStencil(int N, int niters)
{
    Array<float,3> P1, P2, P3, c;
    allocateArrays(shape(N,N,N), P1, P2, P3, c);

    setupInitialConditions(P1, P2, P3, c, N);

    for (int iter=0; iter < niters; ++iter)
    {
        applyStencil(acoustic3D(), P1, P2, P3, c);
        cycleArrays(P1, P2, P3);
    }

    return P1(N/2,N/2,N/2);
}


#include "space_vector_modulation.h"
#include "global_debug.h"
#include "util/clarke_transform.h"
#include "util/conversions.h"
#include "util/math_constants.h"
#include <Eigen/Dense>

const std::array<std::complex<Scalar>, 6> kSvmVectors = []() {
    std::array<std::complex<Scalar>, 6> result;
    result[0] = {1, 0};
    for (int i = 0; i < 6; ++i) {
        result[i] = {std::cos(i * 2 * kPI / 6), std::sin(i * 2 * kPI / 6)};
    }
    return result;
}();

int get_sector(const std::complex<Scalar>& voltage_ab) {
    const std::complex<Scalar> rot_60_deg = std::conj(kSvmVectors[1]);

    std::complex<Scalar> curr = voltage_ab;
    std::complex<Scalar> next = voltage_ab * rot_60_deg;
    int sector;
    for (sector = 0; sector < 5; ++sector) {
        if (curr.imag() >= 0 && next.imag() < 0) {
            // found it
            break;
        }
        // rotate
        curr = next;
        next *= rot_60_deg;
    }

    return sector;
}

std::array<Scalar, 3> get_pwm_duties(const Scalar bus_voltage,
                                     const std::complex<Scalar>& voltage_ab) {
    const int sector_x = get_sector(voltage_ab);
    const int sector_y = (sector_x + 1) % 6;

    const std::complex<Scalar> boundary_x =
        kSvmVectors[sector_x] * kClarkeScale * bus_voltage;
    const std::complex<Scalar> boundary_y =
        kSvmVectors[sector_y] * kClarkeScale * bus_voltage;

    Eigen::Matrix<Scalar, 2, 2> boundaries;
    boundaries <<
        // clang-format off
	boundary_x.real(), boundary_y.real(),
	boundary_x.imag(), boundary_y.imag()
        // clang-format on
        ;
    Eigen::Matrix<Scalar, 2, 1> v_ab;
    v_ab << voltage_ab.real(), voltage_ab.imag();

    Eigen::Matrix<Scalar, 2, 1> coeffs = boundaries.inverse() * v_ab;
    Scalar cx = coeffs(0);
    Scalar cy = coeffs(1);
    Scalar cx_p_cy = cx + cy;
    Scalar c0 = 1.0 - cx_p_cy;
    if (cx_p_cy > 1) {
        c0 = 0;
        cx /= cx_p_cy;
        cy /= cx_p_cy;
    }

    const auto& gx = kSvmStates[sector_x];
    const auto& gy = kSvmStates[sector_y];

    std::array<Scalar, 3> duties;
    for (int i = 0; i < 3; ++i) {
        duties[i] = c0 / 2 + gx[i] * cx + gy[i] * cy;
    }
    return duties;
}

std::complex<Scalar> get_avg_voltage_ab(const Scalar bus_voltage,
                                        const std::array<Scalar, 3>& duties) {
    // optimal sort 3 elements
    int first_gate_on = 0;
    int second_gate_on = 1;
    int third_gate_on = 2;
    if (duties[second_gate_on] > duties[first_gate_on]) {
        std::swap(first_gate_on, second_gate_on);
    }
    if (duties[third_gate_on] > duties[second_gate_on]) {
        std::swap(third_gate_on, second_gate_on);
    }
    // after these two swaps, duties[third_gate_on] is minimal
    if (duties[second_gate_on] > duties[first_gate_on]) {
        std::swap(first_gate_on, second_gate_on);
    }
    // duties[first_gate_on] is maximum

    Eigen::Matrix<Scalar, 3, 1> pole_voltages_x =
        Eigen::Matrix<Scalar, 3, 1>::Zero();
    pole_voltages_x(first_gate_on) = bus_voltage;

    Eigen::Matrix<Scalar, 3, 1> pole_voltages_y = pole_voltages_x;
    pole_voltages_y(second_gate_on) = bus_voltage;

    Eigen::Matrix<Scalar, 3, 1> pole_voltages_avg =
        pole_voltages_x * (duties[first_gate_on] - duties[second_gate_on]) +
        pole_voltages_y * (duties[second_gate_on] - duties[third_gate_on]);

    return to_complex<Scalar>(kClarkeTransform2x3 * pole_voltages_avg);
}


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

#ifndef RW_MATH_ROTATION2D_HPP
#define RW_MATH_ROTATION2D_HPP

/**
 * @file Rotation2D.hpp
 */

#if !defined(SWIG)
#include "Vector2D.hpp"

#include <rw/common/Serializable.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    template< class T > class Rotation2DVector;

    /** @addtogroup math */
    /* @{*/

#if !defined(SWIGJAVA)

    /**
     * @brief A 2x2 rotation matrix \f$ \mathbf{R}\in SO(2) \f$
     *
     * @f$
     *  \mathbf{R}=
     *  \left[
     *  \begin{array}{cc}
     *  {}^A\hat{X}_B & {}^A\hat{Y}_B
     *  \end{array}
     *  \right]
     *  =
     *  \left[
     *  \begin{array}{cc}
     *  r_{11} & r_{12} \\
     *  r_{21} & r_{22}
     *  \end{array}
     *  \right]
     * @f$
     */
#endif
    template< class T = double > class Rotation2D
    {
      public:
        //! Value type.
        typedef T value_type;

        //! The type of the internal Boost matrix implementation.
        typedef Eigen::Matrix< T, 2, 2 > EigenMatrix2x2;

        /**
           @brief A rotation matrix with uninitialized storage.
         */
        Rotation2D () {}

#if !defined(SWIGJAVA)

        /**
         * @brief Constructs an initialized 2x2 rotation matrix
         *
         * @param r11 \f$ r_{11} \f$
         * @param r12 \f$ r_{12} \f$
         * @param r21 \f$ r_{21} \f$
         * @param r22 \f$ r_{22} \f$
         *
         * @f$
         *  \mathbf{R} =
         *  \left[
         *  \begin{array}{cc}
         *  r_{11} & r_{12} \\
         *  r_{21} & r_{22}
         *  \end{array}
         *  \right]
         * @f$
         */

#endif
        Rotation2D (T r11, T r12, T r21, T r22)
        {
            _m[0][0] = r11;
            _m[0][1] = r12;
            _m[1][0] = r21;
            _m[1][1] = r22;
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs an initialized 2x2 rotation matrix
         * @f$ \robabx{a}{b}{\mathbf{R}} =
         * \left[
         *  \begin{array}{cc}
         *   \robabx{a}{b}{\mathbf{i}} & \robabx{a}{b}{\mathbf{j}}
         *  \end{array}
         * \right]
         * @f$
         *
         * @param i @f$ \robabx{a}{b}{\mathbf{i}} @f$
         * @param j @f$ \robabx{a}{b}{\mathbf{j}} @f$
         */

#endif
        Rotation2D (const rw::math::Vector2D< T >& i, const rw::math::Vector2D< T >& j)
        {
            _m[0][0] = i[0];
            _m[0][1] = j[0];
            _m[1][0] = i[1];
            _m[1][1] = j[1];
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs an initialized 2x2 rotation matrix
         * @f$ \robabx{a}{b}{\mathbf{R}} =
         * \left[
         *  \begin{array}{cc}
         *   \robabx{a}{b}{\mathbf{i}} & \robabx{a}{b}{\mathbf{j}}
         *  \end{array}
         * \right]
         * @f$
         *
         * @param theta
         */

#endif
        Rotation2D (const T theta)
        {
            _m[0][0] = cos (theta);
            _m[0][1] = -sin (theta);
            _m[1][0] = sin (theta);
            _m[1][1] = cos (theta);
        }

        /**
           @brief Construct an initialized 2x2 rotation matrix.

           The second of column of the matrix is deduced from the first column.

           @param i [in] The first column of the rotation matrix.
        */
        Rotation2D (const rw::math::Vector2D< T >& i)
        {
            _m[0][0] = i[0];
            _m[0][1] = -i[1];
            _m[1][0] = i[1];
            _m[1][1] = i[0];
        }

        /**
           @brief Construct a rotation matrix from an Eigen matrix.
         */
        template< class R > explicit Rotation2D (const EigenMatrix2x2& m)
        {
            _m[0][0] = m (0, 0);
            _m[0][1] = m (0, 1);
            _m[1][0] = m (1, 0);
            _m[1][1] = m (1, 1);
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs a 2x2 rotation matrix set to identity
         * @return a 2x2 identity rotation matrix
         *
         * @f$
         * \mathbf{R} =
         * \left[
         * \begin{array}{cc}
         * 1 & 0\\
         * 0 & 1
         * \end{array}
         * \right]
         * @f$
         */

#endif
        static const Rotation2D& identity ()
        {
            static Rotation2D id (1, 0, 0, 1);
            return id;
        }

#if !defined(SWIG)
        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        T& operator() (size_t row, size_t column) { return _m[row][column]; }

        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        const T& operator() (size_t row, size_t column) const { return _m[row][column]; }
#else
        MATRIXOPERATOR (T);
#endif
        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true only if all elements are equal.
         *
         * @param rhs [in] Rotation2D to compare with
         * @return True if equal.
         */
        bool operator== (const Rotation2D< T >& rhs) const
        {
            for (int i = 0; i < 2; ++i) {
                for (int j = 0; j < 2; ++j) {
                    if (!(_m[i][j] == rhs (i, j))) {
                        return false;
                    }
                }
            }
            return true;
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true if any of the elements are different.
         *
         * @param rhs [in] Rotation2D to compare with
         * @return True if not equal.
         */
        bool operator!= (const Rotation2D< T >& rhs) const { return !(*this == rhs); }

        /**
         * @brief Returns a boost 2x2 matrix @f$ \mathbf{M}\in SO(2)
         * @f$ that represents this rotation
         *
         * @return @f$ \mathbf{M}\in SO(2) @f$
         */
        EigenMatrix2x2 e ()
        {
            EigenMatrix2x2 matrix;
            matrix (0, 0) = _m[0][0];
            matrix (0, 1) = _m[0][1];
            matrix (1, 0) = _m[1][0];
            matrix (1, 1) = _m[1][1];
            return matrix;
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         *
         * @param bRc [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        const Rotation2D operator* (const Rotation2D& bRc) const
        {
            return Rotation2D ((*this) (0, 0) * bRc (0, 0) + (*this) (0, 1) * bRc (1, 0),
                               (*this) (0, 0) * bRc (0, 1) + (*this) (0, 1) * bRc (1, 1),
                               (*this) (1, 0) * bRc (0, 0) + (*this) (1, 1) * bRc (1, 0),
                               (*this) (1, 0) * bRc (0, 1) + (*this) (1, 1) * bRc (1, 1));
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{v}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{v}} \f$
         *
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         * @param bVc [in] \f$ \robabx{b}{c}{\mathbf{v}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{v}} \f$
         */
        const rw::math::Vector2D< T > operator* (const rw::math::Vector2D< T >& bVc) const
        {
            return rw::math::Vector2D< T > ((*this) (0, 0) * bVc (0) + (*this) (0, 1) * bVc (1),
                                            (*this) (1, 0) * bVc (0) + (*this) (1, 1) * bVc (1));
        }
#if !defined(SWIG)
        /**
         * @brief Writes rotation matrix to stream
         * @param os [in/out] output stream to use
         * @param r [in] rotation matrix to print
         * @return the updated output stream
         */
        friend std::ostream& operator<< (std::ostream& os, const Rotation2D& r)
        {
            return os << "Rotation2D {" << r (0, 0) << ", " << r (0, 1) << ", " << r (1, 0) << ", "
                      << r (1, 1) << "}";
        }
#else
        TOSTRING (rw::math::Rotation2D< T >);
#endif

      private:
        T _m[2][2];
        // Base _atrix;
    };

    /**
     * @brief Casts Rotation2D<T> to Rotation2D<Q>
     * @param rot [in] Rotation2D with type T
     * @return Rotation2D with type R
     */
    template< class R, class T > const Rotation2D< R > cast (const Rotation2D< T >& rot)
    {
        Rotation2D< R > res (Rotation2D< R >::identity ());
        for (size_t i = 0; i < 2; i++)
            for (size_t j = 0; j < 2; j++)
                res (i, j) = static_cast< R > (rot (i, j));
        return res;
    }

    /**
     * @brief The inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$ of a rotation matrix
     *
     * @relates Rotation2D
     *
     * @param aRb [in] the rotation matrix @f$ \robabx{a}{b}{\mathbf{R}} @f$
     *
     * @return the matrix inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$
     *
     * @f$ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} =
     * \robabx{a}{b}{\mathbf{R}}^T @f$
     */
    template< class T > const Rotation2D< T > inverse (const Rotation2D< T >& aRb)
    {
        return Rotation2D< T > (aRb (0, 0), aRb (1, 0), aRb (0, 1), aRb (1, 1));
    }

    /**
     * @brief Find the transpose of \b aRb.
     *
     * The transpose of a rotation matrix is the same as the inverse.
     */
    template< class T > const Rotation2D< T > transpose (const Rotation2D< T >& aRb)
    {
        return Rotation2D< T > (aRb (0, 0), aRb (1, 0), aRb (0, 1), aRb (1, 1));
    }
#if !defined(SWIG)
    extern template class rw::math::Rotation2D< double >;
    extern template class rw::math::Rotation2D< float >;
#else
    SWIG_DECLARE_TEMPLATE (Rotation2Dd, rw::math::Rotation2D< double >);
    SWIG_DECLARE_TEMPLATE (Rotation2Df, rw::math::Rotation2D< float >);
#endif
    /**@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Rotation2D
         */
        template<>
        void write (const rw::math::Rotation2D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Rotation2D
         */
        template<>
        void write (const rw::math::Rotation2D< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Rotation2D
         */
        template<>
        void read (rw::math::Rotation2D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Rotation2D
         */
        template<>
        void read (rw::math::Rotation2D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


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

#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

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

// Generic functor
// See http://eigen.tuxfamily.org/index.php?title=Functors
// C++ version of a function pointer that stores meta-data about the function
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{

  // Information that tells the caller the numeric type (eg. double) and size (input / output dim)
  typedef _Scalar Scalar;
  enum { // Required by numerical differentiation module
      InputsAtCompileTime = NX,
      ValuesAtCompileTime = NY
  };

  // Tell the caller the matrix sizes associated with the input, output, and jacobian
  typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
  typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
  typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

  // Local copy of the number of inputs
  int m_inputs, m_values;

  // Two constructors:
  Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
  Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

  // Get methods for users to determine function input and output dimensions
  int inputs() const { return m_inputs; }
  int values() const { return m_values; }

};

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

// https://en.wikipedia.org/wiki/Test_functions_for_optimization
// Booth Function
// Implement f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2
struct BoothFunctor : Functor<double>
{
  // Simple constructor
  BoothFunctor(): Functor<double>(2,3) {}

  // Implementation of the objective function
  int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const {
    double a = z(0);
	  double b = z(1);
	  double c = z(2);
	/*
     * Evaluate the Booth function.
     * Important: LevenbergMarquardt is designed to work with objective functions that are a sum
     * of squared terms. The algorithm takes this into account: do not do it yourself.
     * In other words: objFun = sum(fvec(i)^2)
     */
    fvec(0) = 3*a -cos(b*c) -0.5;
    fvec(1) = pow(a,2.0) -81*pow((b+0.1),2.0)+sin(c)+1.06;
	  fvec(2) = exp(-a*b)+20*c+(10*M_PI-3.0)/3.0;
    return 0;
  }
};

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

// https://en.wikipedia.org/wiki/Test_functions_for_optimization
// Himmelblau's Function
// Implement f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2
struct HimmelblauFunctor : Functor<double>
{
  // Simple constructor
  HimmelblauFunctor(): Functor<double>(2,2) {}
  // Implementation of the objective function
  int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const {
    double x = z(0);   double y = z(1);
    /*
     * Evaluate Himmelblau's function.
     * Important: LevenbergMarquardt is designed to work with objective functions that are a sum
     * of squared terms. The algorithm takes this into account: do not do it yourself.
     * In other words: objFun = sum(fvec(i)^2)
     */
    fvec(0) = x * x + y - 11;
    fvec(1) = x + y * y - 7;
    return 0;
  }
};

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

void testBoothFun() {
  std::cout << "Testing the Booth function..." << std::endl;
  Eigen::VectorXd zInit(3); zInit << 0.1, 0.1, -0.1;
  std::cout << "zInit: " << zInit.transpose() << std::endl;

  BoothFunctor functor;
  Eigen::NumericalDiff<BoothFunctor> numDiff(functor);
  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<BoothFunctor>,double> lm(numDiff);
  lm.parameters.maxfev = 1000;
  lm.parameters.xtol = 1.0e-10;
  std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl;
  std::cout << "x tol: " << lm.parameters.xtol << std::endl;

  Eigen::VectorXd z = zInit;
  int ret = lm.minimize(z);
  std::cout << "iter count: " << lm.iter << std::endl;
  std::cout << "return status: " << ret << std::endl;
  std::cout << "zSolver: " << z.transpose() << std::endl;
  std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}

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

void testHimmelblauFun() {
  std::cout << "Testing the Himmelblau function..." << std::endl;
  // Eigen::VectorXd zInit(2); zInit << 0.0, 0.0;  // soln 1
  // Eigen::VectorXd zInit(2); zInit << -1, 1;  // soln 2
  // Eigen::VectorXd zInit(2); zInit << -1, -1;  // soln 3
  Eigen::VectorXd zInit(2); zInit << 1, -1;  // soln 4
  std::cout << "zInit: " << zInit.transpose() << std::endl;
  std::cout << "soln 1: [3.0, 2.0]" << std::endl;
  std::cout << "soln 2: [-2.805118, 3.131312]" << std::endl;
  std::cout << "soln 3: [-3.77931, -3.28316]" << std::endl;
  std::cout << "soln 4: [3.584428, -1.848126]" << std::endl;

  HimmelblauFunctor functor;
  Eigen::NumericalDiff<HimmelblauFunctor> numDiff(functor);
  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<HimmelblauFunctor>,double> lm(numDiff);
  lm.parameters.maxfev = 1000;
  lm.parameters.xtol = 1.0e-10;
  std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl;
  std::cout << "x tol: " << lm.parameters.xtol << std::endl;

  Eigen::VectorXd z = zInit;
  int ret = lm.minimize(z);
  std::cout << "iter count: " << lm.iter << std::endl;
  std::cout << "return status: " << ret << std::endl;
  std::cout << "zSolver: " << z.transpose() << std::endl;
  std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}

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

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

	std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
	testBoothFun();
	testHimmelblauFun();
	return 0;
}


#ifndef RSVD_STANDARD_NORMAL_RANDOM_HPP_
#define RSVD_STANDARD_NORMAL_RANDOM_HPP_

#include <Eigen/Dense>
#include <cmath>
#include <complex>
#include <random>

namespace Rsvd {

namespace Internal {

namespace {
/// \brief Shortcut for the underlying \a real type used in the matrix.
///
/// \long E.g. if the matrix is of type \c std::complex<double>, this typedef equals to \c double.
template <typename MatrixType>
using RealType = typename Eigen::NumTraits<typename MatrixType::Scalar>::Real;
} // namespace

/// \brief Helper struct to do partial specialization in the matrix scalar type
///
/// \tparam MatrixType Eigen matrix type.
///
/// \tparam ScalarType Underlying scalar type used in \c MatrixType, can be complex or real.
///
/// \tparam RandomEngineType Type of the random engine, e.g. \c std::default_random_engine or \c
/// std::mt19937_64.
template <typename MatrixType, typename ScalarType, typename RandomEngineType>
struct StandardNormalRandomHelper {
  static inline MatrixType generate(Eigen::Index numRows, Eigen::Index numCols,
                                    RandomEngineType &engine);
};

/// \brief Partial specialization for real matrices.
template <typename MatrixType, typename RandomEngineType>
struct StandardNormalRandomHelper<MatrixType, RealType<MatrixType>, RandomEngineType> {
  static inline MatrixType generate(const Eigen::Index numRows, const Eigen::Index numCols,
                                    RandomEngineType &engine) {
    // Create a standard normal distribution with zero mean (mu = 0) and unity variance (sigma^2 =
    // 1)
    std::normal_distribution<RealType<MatrixType>> distribution{0, 1};
    const auto normal{[&](typename MatrixType::Scalar) { return distribution(engine); }};
    return MatrixType::NullaryExpr(numRows, numCols, normal);
  }
};

/// \brief Partial specialization for complex matrices.
template <typename MatrixType, typename RandomEngineType>
struct StandardNormalRandomHelper<MatrixType, std::complex<RealType<MatrixType>>,
                                  RandomEngineType> {
  static inline MatrixType generate(const Eigen::Index numRows, const Eigen::Index numCols,
                                    RandomEngineType &engine) {
    // A complex standard normal distribution has half of its variance in the real variable and
    // half in the complex. Hence, the each variance is 1/2 and each standard deviation is
    // 1/sqrt(2)
    constexpr RealType<MatrixType> stdDev{0.707106781186547};
    std::normal_distribution<RealType<MatrixType>> distribution{0, stdDev};
    const auto complexNormal{[&](typename MatrixType::Scalar) {
      return std::complex<RealType<MatrixType>>(distribution(engine), distribution(engine));
    }};
    return MatrixType::NullaryExpr(numRows, numCols, complexNormal);
  }
};

/// \brief Generate a matrix with iid elements from standard normal
/// distribution.
///
/// \long The matrix elements are drawn from the standard normal distribution (zero mean and unit
/// variance), i.e. \f$ [ \Omega ]_{i,j} \sim \mathcal{N}(0, 1) \f$ for the real case and \f$ [
/// \Omega ]_{i,j} \sim \mathcal{N}\left(0, \frac{1}{2} \right) + \mathcal{N}\left(0, \frac{1}{2}
/// \right) \cdot \mathrm{i}\f$ for the complex case.
///
/// \tparam MatrixType Eigen matrix type.
/// \tparam RandomEngineType Type of the random engine, e.g. \c std::default_random_engine or \c
/// std::mt19937_64.
///
/// \param numRows Number of matrix rows \f$ m \f$.
/// \param numCols Number of matrix columns \f$ n \f$.
/// \param engine Reference to the random engine.
///
/// \return Matrix \f$ \Omega \in \mathbb{F}^{m \times n} \f$ with \f$\mathbb{F} \in \{ \mathbb{R},
/// \mathbb{C} \} \f$ with normally distributed elements.
template <typename MatrixType, typename RandomEngineType>
inline MatrixType standardNormalRandom(const Eigen::Index numRows, const Eigen::Index numCols,
                                       RandomEngineType &engine) {
  return StandardNormalRandomHelper<MatrixType, typename MatrixType::Scalar,
                                    RandomEngineType>::generate(numRows, numCols, engine);
}

} // namespace Internal

} // namespace Rsvd

#endif


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd X = MatrixXd::Random(5,5);
MatrixXd A = X * X.transpose();
X = MatrixXd::Random(5,5);
MatrixXd B = X * X.transpose();

GeneralizedSelfAdjointEigenSolver<MatrixXd> es(A,B,EigenvaluesOnly);
cout << "The eigenvalues of the pencil (A,B) are:" << endl << es.eigenvalues() << endl;
es.compute(B,A,false);
cout << "The eigenvalues of the pencil (B,A) are:" << endl << es.eigenvalues() << endl;

  return 0;
}


#pragma once

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

#include <map>
#include <vector>

using std::vector;
using std::map;

namespace mcl_cpp
{
	class mcl
	{
	public:
		//! @brief MCL ctor. Register a callback function to return the cluster results
		mcl(const Eigen::MatrixXd& Min, std::function< void(size_t cluster_j, size_t member_i) > f) : M(Min), ClusterResultCallback(f) {}

		/*! @brief Apply Markov clustering algorithm with specified parameters and return clusters
			For each cluster, returns the list of node-ids that belong to that cluster
			*/
		Eigen::MatrixXd cluster_mcl(double expand_factor = 2, double inflate_factor = 2, double max_loop = 10, double mult_factor = 1)
		{
			Eigen::MatrixXd M_selfloop = M + mult_factor * Eigen::MatrixXd::Identity(M.cols(), M.rows());
			Eigen::MatrixXd M_normalized = normalize(M_selfloop);
			int i;
			for (i = 0; i < max_loop && !stop(M_normalized, i); i++)
			{
				inflate(M_normalized, inflate_factor);
				expand(M_normalized, expand_factor);
			}
			for (auto row = 0; row<M_normalized.rows(); row++)
			{
				if (M_normalized(row, row) > 0)
				{
					for (auto col = 0; col < M_normalized.cols(); col++)
					{
						if (M_normalized.coeff(row, col) > 0)
							ClusterResultCallback(row, col);
					}
				}
			}
			return std::move(M_normalized);
		}

	private:
		bool stop(const Eigen::MatrixXd& in, int i)
		{
			Eigen::MatrixXd m = in*in;
			Eigen::MatrixXd diff = (m - in);
			double mx = diff.maxCoeff(), mn = diff.minCoeff();
			return (diff.maxCoeff() == diff.minCoeff());
		}

		Eigen::MatrixXd normalize(Eigen::MatrixXd& in)
		{
			Eigen::MatrixXd one_over_col_sum = in.colwise().sum().cwiseInverse();
			Eigen::MatrixXd M_normalized = in * one_over_col_sum.asDiagonal();
			return std::move(M_normalized);
		}

		void expand(Eigen::MatrixXd& in, double expand_factor)
		{
			Eigen::MatrixPower<Eigen::MatrixXd> Apow(in);
			in = Apow(expand_factor);
		}

		void inflate(Eigen::MatrixXd& in, double inflate_factor)
		{
			auto lam = [inflate_factor](double x) -> double { return std::pow(x, inflate_factor); };
			in = in.unaryExpr(lam);
			in = normalize(in);
		}

		const Eigen::MatrixXd & M;
		std::function< void(size_t cls_i, size_t mem_j) > ClusterResultCallback;
	};

}


#include <Eigen/Dense>
#include <iostream>
#include <chrono.h>

void buildSystem(Eigen::MatrixXd& A, Eigen::VectorXd& b, const unsigned int dim) {
    A = Eigen::MatrixXd::Random(dim, dim);
    A.diagonal() += (dim + 1) * Eigen::VectorXd::Ones(dim);

    b = Eigen::VectorXd::Random(dim);
    assert(b.norm() > DBL_EPSILON);
}

Eigen::VectorXd GS(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const unsigned int nbIter) {
    auto x = b;
    for (size_t iter = 0; iter < nbIter; ++iter) {
        for (size_t i = 0; i < A.rows(); i++) {
            double sum = 0.0;
            for (size_t j = 0; j < A.cols(); j++) {
                if (j != i) {
                    sum += A(i, j) * x(j);
                }
                x(i) = (b(i) - sum) / A(i, i);
            }
        }
    }
    return x;
}

int main(int argc, char const *argv[]) {
    const unsigned int arraySize = 5;
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(arraySize, arraySize);
    Eigen::VectorXd b = Eigen::VectorXd::Random(arraySize);
    buildSystem(A, b, A.size());

    TP_CPP_IMAC2::Chrono chrono;

    // Jacobi
    chrono.start();
    Eigen::VectorXd x = GS(A, b, 28);
    chrono.stop();
    std::cout << "Jacobi : " << chrono.timeSpan() << std::endl;

    // Lu
    chrono.start();
    Eigen::PartialPivLU<Eigen::MatrixXd> lu(A);
    x = A * lu.solve(b);
    chrono.stop();
    std::cout << "Lu : " << chrono.timeSpan() << std::endl;

    // QR
    chrono.start();
    Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr(A);
    x = A * qr.solve(b);
    chrono.stop();
    std::cout << "QR : " << chrono.timeSpan() << std::endl;

    // SVD
    chrono.start();
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    x = A * svd.solve(b);
    chrono.stop();
    std::cout << "SVD : " << chrono.timeSpan() << std::endl;

    return 0;
}


#include <iostream>
#include <math.h>
#include <Eigen/Dense>

using namespace std;

struct Vertex {
    double x, y, z;
};

int main()
{
    Vertex v1, v2, v3;
    cin >> v1.x >> v1.y >> v1.z
        >> v2.x >> v2.y >> v2.z
        >> v3.x >> v3.y >> v3.z;

    Eigen::Vector3d vec12(v2.x-v1.x, v2.y-v1.y, v2.z-v1.z);
    Eigen::Vector3d vec23(v3.x-v2.x, v3.y-v2.y, v3.z-v2.z);

    Eigen::Vector3d veccross = vec12.cross(vec23);
    // veccross /= sqrt(veccross.array().pow(2).sum());
    veccross.normalize();

    Eigen::Vector4d faceEqn;
    faceEqn << veccross, -veccross[0]*v1.x -veccross[1]*v1.y -veccross[2]*v1.z;

    cout << veccross << endl << faceEqn << endl;

}


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

Eigen::Matrix3d OthMatrix(const Eigen::Matrix3d& input) {
    Eigen::Quaterniond q(input);
    q.normalize();
    return q.toRotationMatrix();
}

// 解决如下问题
// 1. Affine3d, Isomtry3d,  Matrix4d表示同一个欧式变换时，求逆是否不同
// 从输出可以看出来结果一致
int main(int argv, char** argc) {
    double theta_z = M_PI * 60 / 180;
    double theta_y = M_PI * 30 / 180;
    double theta_x = M_PI * 40 / 180;
    Eigen::Matrix3d rz =
        Eigen::AngleAxisd(theta_z, Eigen::Vector3d::UnitZ()).toRotationMatrix();
    Eigen::Matrix3d ry =
        Eigen::AngleAxisd(theta_y, Eigen::Vector3d::UnitY()).toRotationMatrix();
    Eigen::Matrix3d rx =
        Eigen::AngleAxisd(theta_x, Eigen::Vector3d::UnitX()).toRotationMatrix();

    Eigen::Quaterniond rotation(rz * ry * rx);
    std::cout << rotation.matrix() << std::endl;
    Eigen::Translation3d t(1, 2, 3);
    Eigen::Affine3d matrix_affine = t * rotation;
    std::cout <<"matrix_affine : \n" << matrix_affine.matrix() << std::endl;

    Eigen::Isometry3d isometry3d(t * rotation);
    std::cout << "isometry3 : \n" << isometry3d.matrix() << std::endl;

    Eigen::Matrix4d m4d = matrix_affine.matrix();
    std::cout << "matrix4d : \n" << m4d.matrix() << std::endl;

    std::cout <<"matrix_affine inverse: \n" <<  matrix_affine.inverse().matrix() << std::endl;
    std::cout << "matrix4d inverse : \n" << m4d.inverse() << std::endl;
    std::cout << "isometry3 inverse : \n" << isometry3d.inverse().matrix() << std::endl;

}


/*
 * two_dimensional_phase_lattice.cpp
 *
 * This example show how one can use matrices as state types in odeint.
 *
 * Copyright 2009-2012 Karsten Ahnert and Mario Mulansky.
 * 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)
 */

#include <iostream>
#include <map>
#include <string>
#include <fstream>

#ifndef M_PI //not there on windows
#define M_PI 3.1415927 //...
#endif

#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

//[ two_dimensional_phase_lattice_definition
typedef boost::numeric::ublas::matrix< double > state_type;

struct two_dimensional_phase_lattice
{
    two_dimensional_phase_lattice( double gamma = 0.5 )
    : m_gamma( gamma ) { }

    void operator()( const state_type &x , state_type &dxdt , double /* t */ ) const
    {
        size_t size1 = x.size1() , size2 = x.size2();

        for( size_t i=1 ; i<size1-1 ; ++i )
        {
            for( size_t j=1 ; j<size2-1 ; ++j )
            {
                dxdt( i , j ) =
                        coupling_func( x( i + 1 , j ) - x( i , j ) ) +
                        coupling_func( x( i - 1 , j ) - x( i , j ) ) +
                        coupling_func( x( i , j + 1 ) - x( i , j ) ) +
                        coupling_func( x( i , j - 1 ) - x( i , j ) );
            }
        }

        for( size_t i=0 ; i<x.size1() ; ++i ) dxdt( i , 0 ) = dxdt( i , x.size2() -1 ) = 0.0;
        for( size_t j=0 ; j<x.size2() ; ++j ) dxdt( 0 , j ) = dxdt( x.size1() -1 , j ) = 0.0;
    }

    double coupling_func( double x ) const
    {
        return sin( x ) - m_gamma * ( 1.0 - cos( x ) );
    }

    double m_gamma;
};
//]

struct write_for_gnuplot
{
    size_t m_every , m_count;

    write_for_gnuplot( size_t every = 10 )
    : m_every( every ) , m_count( 0 ) { }

    void operator()( const state_type &x , double t )
    {
        if( ( m_count % m_every ) == 0 )
        {
            clog << t << endl;
            cout << "sp '-'" << endl;
            for( size_t i=0 ; i<x.size1() ; ++i )
            {
                for( size_t j=0 ; j<x.size2() ; ++j )
                {
                    cout << i << "\t" << j << "\t" << sin( x( i , j ) ) << "\n";
                }
                cout << "\n";
            }
            cout << "e" << endl;
        }

        ++m_count;
    }
};

class write_snapshots
{
public:

    typedef std::map< size_t , std::string > map_type;

    write_snapshots( void ) : m_count( 0 ) { }

    void operator()( const state_type &x , double t )
    {
        map< size_t , string >::const_iterator it = m_snapshots.find( m_count );
        if( it != m_snapshots.end() )
        {
            ofstream fout( it->second.c_str() );
            for( size_t i=0 ; i<x.size1() ; ++i )
            {
                for( size_t j=0 ; j<x.size2() ; ++j )
                {
                    fout << i << "\t" << j << "\t" << x( i , j ) << "\t" << sin( x( i , j ) ) << "\n";
                }
                fout << "\n";
            }
        }
        ++m_count;
    }

    map_type& snapshots( void ) { return m_snapshots; }
    const map_type& snapshots( void ) const { return m_snapshots; }

private:

    size_t m_count;
    map_type m_snapshots;
};

int main( int argc , char **argv )
{
    size_t size1 = 128 , size2 = 128;
    state_type x( size1 , size2 , 0.0 );

    for( size_t i=(size1/2-10) ; i<(size1/2+10) ; ++i )
        for( size_t j=(size2/2-10) ; j<(size2/2+10) ; ++j )
            x( i , j ) = static_cast<double>( rand() ) / RAND_MAX * 2.0 * M_PI;

    write_snapshots snapshots;
    snapshots.snapshots().insert( make_pair( size_t( 0 ) , string( "lat_0000.dat" ) ) );
    snapshots.snapshots().insert( make_pair( size_t( 100 ) , string( "lat_0100.dat" ) ) );
    snapshots.snapshots().insert( make_pair( size_t( 1000 ) , string( "lat_1000.dat" ) ) );
    observer_collection< state_type , double > obs;
    obs.observers().push_back( write_for_gnuplot( 10 ) );
    obs.observers().push_back( snapshots );

    cout << "set term x11" << endl;
    cout << "set pm3d map" << endl;

    integrate_const( runge_kutta4<state_type>() , two_dimensional_phase_lattice( 1.2 ) ,
                     x , 0.0 , 1001.0 , 0.1 , boost::ref( obs ) );

    // controlled steppers work only after ublas bugfix
    //integrate_const( make_dense_output< runge_kutta_dopri5< state_type > >( 1E-6 , 1E-6 ) , two_dimensional_phase_lattice( 1.2 ) ,
    //        x , 0.0 , 1001.0 , 0.1 , boost::ref( obs ) );

    return 0;
}


/**
\file   quintic_spline.cpp
\brief  quintic_spline parameterization
 *
\author  Mahmoud Ali
\date    21/6/2019
*/

#include<vector>
#include<iostream>
#include "ros/ros.h"
#include "std_msgs/Float64MultiArray.h"

//#include<normal_toppra_traj_instant_1.h>
#include<normal_toppra_traj_instant_3.h>
#include <Eigen/Dense>
#include <math.h>

#include <python2.7/Python.h>
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
using namespace Eigen;
using namespace std;
using Eigen::MatrixXd;

const double sm=180, vm= 130, am=250, jm=1000; // pos, vel, acc, jrk max limits

// finds in which segment time instant tg belons to
int find_seg_number (double tg, std::vector<double> T_vec){
    // segment changes from 0 to n-2 which mean n-1 seg
    int seg = 0;
    if(tg<= T_vec[0]) //less than tstart
        return seg;
    else if(tg>= T_vec[T_vec.size() -2 ] ) //if tg is greater than the starting time of the last segment
        return T_vec.size() - 2;
    else {
        for (int i=0; i< T_vec.size()-2 ; i++) { // between tstart and tend
            if(tg>= T_vec[i] && tg< T_vec[i+1])
                seg = i;
        }
        return seg;
    }
}

// to sample trajectory, find state(pos, vel, acc, jrk) at a given time tg, T_vec is time vector of waypoints, cof is coef matrix of quintic spline
void sample (const std::vector<double>& T_vec, const double& tg, const std::vector<std::vector<double>> &cof, std::vector<double> & state){//, bool pos_plt=true, bool vel_plt=true, bool acc_plt=true, bool jrk_plt=true ){
    std::vector<double> a=cof[0], b=cof[1], c=cof[2], d=cof[3], e=cof[4], f=cof[5];
    double pos=0, vel=0, acc=0, jrk=0;
    int n = T_vec.size(), seg=0; // n:number of points
    double t=0; // to represent tg-tseg
    //check for vector sizes
    //    std::cout<<"\nsize of a= "<< a.size() << " size of T= "<< T_vec.size();
    if( a.size() != T_vec.size()-1 )
        std::cout<<"error: coef and time have different size\n";

    // check for tg inside T_vec or not, if yes find corrresponding segment
    if(tg < T_vec[0]){
        std::cout<<"Warn: request time instant is before start time of trajectory\n";
        pos= a[0];
        vel= b[0];
        acc= 2*c[0];
        jrk= 6*d[0];
    }
    else if(tg > T_vec[n-1] ){
        std::cout<<"Warn: request time instant is after the end time of trajectory\n ";
        t = tg - T_vec[n-1];
        pos=   a[n-2] +   b[n-2]*t +         c[n-2]*pow( t, 2) +        d[n-2]*pow( t, 3) +    e[n-2]*pow( t, 4) +    f[n-2]* pow( t, 5)  ;
        vel=              b[n-2]        +  2*c[n-2]*t          +      3*d[n-2]*pow( t, 2) +  4*e[n-2]*pow( t, 3) +  5*f[n-2]* pow( t, 4)  ;
        acc=                               2*c[n-2]                 + 6*d[n-2]*t          + 12*e[n-2]*pow( t, 2) + 20*f[n-2]* pow( t, 3)  ;
        jrk=                                                          6*d[n-2]                 + 24*e[n-2]*t          + 60*f[n-2]* pow( t, 2)  ;
    }
    else{
        seg= find_seg_number(tg, T_vec);
        t = tg - T_vec[seg];
        //        std::cout<<"      tg= " <<tg<< "   seg_number= "<<  seg<<"   t= "<<t<<std::endl;
        pos=   a[seg] +   b[seg]*t +    c[seg]*pow( t, 2) +   d[seg]*pow( t, 3) +    e[seg]*pow( t, 4) +    f[seg]* pow( t, 5) ;
        vel=              b[seg]        +  2*c[seg]*t          + 3*d[seg]*pow( t, 2) +  4*e[seg]*pow( t, 3) +  5*f[seg]* pow( t, 4) ;
        acc=                               2*c[seg]                 + 6*d[seg]*t          + 12*e[seg]*pow( t, 2) + 20*f[seg]* pow( t, 3) ;
        jrk=                                                          6*d[seg]                 + 24*e[seg]*t          + 60*f[seg]* pow( t, 2) ;
    }
    // assign state to state vector
    state[0] = pos;
    state[1] = vel;
    state[2] = acc;
    state[3] = jrk;

}

// generate C1 matrix which is depends on the time of each segment, idx is segment indx, h vectore contains segments times
void generate_coef_matrix_c1(const int idx, const VectorXd &h, MatrixXd &C1){
    MatrixXd C(5,6);
    C<< 1,  h(idx),  pow(h(idx), 2),    pow(h(idx), 3),    pow(h(idx), 4),     pow(h(idx), 5),
            0,       1,        2*h(idx),  3*pow(h(idx), 2),  4*pow(h(idx), 3),   5*pow(h(idx), 4),
            0,       0,               1,          3*h(idx),  6*pow(h(idx), 2),  10*pow(h(idx), 3),
            0,       0,               0,                 1,          4*h(idx),  10*pow(h(idx), 2),
            0,       0,               0,                 0,                 1,           5*h(idx);
    C1 = C;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "quintic_isp");
    ros::NodeHandle nh;

    ROS_INFO("initializing path .... ");
    //============ read trajectory ==========
    trajectory_msgs::JointTrajectory  traj;
    traj = generate_traj();
    int n_jts = traj.joint_names.size();
    int n_pts = traj.points.size();

    // get waypoints from trajectory
    std::vector< std::vector<double> > P_jt_wpt;
    P_jt_wpt.resize( n_jts);
    for(int jt=0; jt<n_jts; jt++){
        for(int pt=0; pt<n_pts; pt++){
            P_jt_wpt[jt].push_back( traj.points[pt].positions[jt] );
        }
    }

    //pos vector
    VectorXd pos(n_pts);
    for (int pt=0; pt< n_pts; pt++) {
        pos(pt) = P_jt_wpt[4][pt];
    }
    //    for (int pt=0; pt< n_pts; pt++) {
    //        pos(pt+n_pts) = P_jt_wpt[5][pt];
    //    }
    // put equations in form of AX=B
    int n = pos.size(); //number of waypoints
    int n_eqs = 6*(n-1); // number of equations/variables
    double vel0=0, veln=0, acc0=0, accn=0; // initial/final  conditions(vel, acc)

    // store stop point at which trajectory reverse direction
    std::vector<int> stp_pts_idx;
    for (int i=1; i<n-1; i++) {
        if( (  (pos(i+1) - pos(i))> 0 && (pos(i) - pos(i-1))> 0  ) || ( (pos(i+1) - pos(i))< 0 && (pos(i) - pos(i-1))< 0 ) )
            continue;
        stp_pts_idx.push_back( i );
    }
    //    ROS_INFO_STREAM("stp_pts_idx"); //check
    //    for (int i=0; i< stp_pts_idx.size(); i++)
    //        ROS_INFO_STREAM(" "<< stp_pts_idx[i] );

    //from pos vector: generate initial time for way points. t: time vector of waypoinys times, h is vector for segments times
    VectorXd t(n), h(n-1); //t0 ==> tn-1  ...   h0 ==> hn-2 (#n of t and #n-1 of h)
    for (int i = 0; i < n-1; ++i) {
        h(i) = abs( (abs(pos(i+1)) - abs(pos(i))) ) /vm;
    }
    //  ROS_INFO_STREAM("h vector: \n"<<h);
    t(0)=0; //starting time of trajectory
    for (int i = 0; i < n-1; ++i) {
        t(i+1) = h(i) + t(i);
    }
    //  ROS_INFO_STREAM("t vector: \n"<<t);

    // define coef matrix and B matrix: AX=B
    VectorXd B(n_eqs), X(n_eqs);
    MatrixXd A =   MatrixXd::Zero(n_eqs, n_eqs);
    MatrixXd C1 =  MatrixXd::Zero(5, 6);
    MatrixXd C2 =  MatrixXd::Zero(5, 6);

    // C1 and C2 store coef of 5(n-2) equations
    for (int i=0; i<5; i++)
        C2(i,i)=-1.0; // C2(0,0)  to   C2(4,4)

    for (int i=0; i<n-1; i++) {
        generate_coef_matrix_c1(i, h, C1);
        ROS_INFO_STREAM(" C1("<<i <<") initialization:\n "<< C1);
    }

    //>>>>>>>>>>>step_1:  3 boundary conditions for first point (pos0, vel0, acc0), 3 rows
    A(0,0) = 1;
    A(1,1) = 1;
    A(2,2) = 1;  // 3 rows are filled so far, so next r=3

    //>>>>>>>>>>> step_2:  pos condition for all waypoints except first and last, (n-2) rows
    int col_idx= 6;
    for (int r = 3; r < 3+(n-2); ++r) {  //start filling from 4th row, #rows= n-2
        A(r,col_idx) = 1;
        col_idx+=6;
    }  // n-2 rows are filled so far, so total rows are n-2+3= n+1

    int count =0; // count no of iteration
    std::vector<int> seg_idx;  //store segments indices that violate jerk
    bool update_all_seg_time= false; // select: true=> updates times of all segments, false: update time for segments that voilate jerk
    //================================================= loop: update time till the jerk become within the limit ================================
    // bif while loop
    bool lmtd_jrk = false; //will be set to true when all jrk values are in the limits
    while (!lmtd_jrk){
        if(count>0){ //no update in the first loop
            if(update_all_seg_time) // update all segment time
                for (int idx = 0; idx < n-1; ++idx)
                    h(idx)=h(idx)+0.01;

            else{  // only update the time of segment that violate jerk limit
                for (int idx = 0; idx < seg_idx.size(); ++idx){
                    ROS_INFO_STREAM("seg_idx["<<idx<< "]:  "<<seg_idx[idx]);
                    if (seg_idx[idx] == n-1)
                        seg_idx[idx] = seg_idx[idx] -1;
                    h(seg_idx[idx])=h(seg_idx[idx])+0.01;
                }
            }
        }
        seg_idx.clear();
        for (int i = 0; i < n-1; ++i) {
            t(i+1) = h(i) + t(i);
        }

        // checks for velocity and acc limits

        //>>>>>>>>>>>step_3: boundary conditions for last point (posn, veln, accn), 3 rows (depends on time so it is inside the loop)
        // ROS_INFO("3 boundary conditions for last poit");
        for (int r = n+1 ; r < 3+(n+1); ++r) {// for 3 row
            for (int c = n_eqs-6 ; c < n_eqs; ++c) {  // last 6 columns
                generate_coef_matrix_c1( n-2, h, C1);
                A(r, c) = C1(r-(n+1), c-(n_eqs-6));     // last h ==> hn-2
            }
        }//3+(n-2)+3 = n+4 rows are filled so far,  so next row to fill is (n+5)th row [its is index= n+4]
        //>>>>>>>>>>>step_4: 5(n-2) coninuity conditions for each waypoints except first and last, 5(n-2) rows (depend on time so it is inside the loop)
        //   ROS_INFO(" 5(n-2) continuity conditions for midle poits");
        int row_idx = n+4; col_idx=0;  //start filling from (n+2)th row [its is index= n+1],
        for (int blk=0; blk< n-2; blk++) { // n-2 blks, each blk is 5 equations with 6 coef
            //       ROS_INFO_STREAM("row_idx: "<< row_idx <<"   col_idx:"<< col_idx);
            for (int r = 0 ; r < 5 ; ++r) {  // 5(n-2) rows
                for (int c = 0 ; c < 6; ++c) {  // last 6 columns
                    //               ROS_INFO_STREAM("r: "<< r <<"   c:"<< c);
                    generate_coef_matrix_c1( blk, h, C1);
                    A(row_idx+r, col_idx+c)   = C1(r,c);     // last h ==> hn-2
                    A(row_idx+r, col_idx+c+6) = C2(r,c);
                }
            }
            col_idx += 6;
            row_idx += 5;
        } //all row s are filled

        //====================================check for stop points:==================================

        //>>>>>>>>>>>step_5: fill B vector B
        B(0) = pos(0);
        B(1) = vel0;
        B(2) = acc0; //  3 rows so far
        for (int i=1; i< n-1; i++) // all pos from pos1 to pos(n-1)= (n-2) rows
            B(i+2)= pos(i); // so far 3+n-2 = n+1

        for (int i=n+4; i< n_eqs; i++) // last 5(n-2) ros
            B(i)= 0; // so far 3+n-2 = n+1

        B(n+1) = pos(n-1);
        B(n+2) = veln;
        B(n+3) = accn;
        ROS_INFO_STREAM("B vector: \n"<<B);

        //find the solution: using matirx inverse
        //   MatrixXd A_inv = A.inverse();
        //   X = A.inverse()*B;  // some times gives nan or inf
        //   ROS_INFO_STREAM("X vector: \n"<<X);

        HouseholderQR<MatrixXd> qr(A);
        X = qr.solve(B); // computes A^-1 * b
        ROS_INFO_STREAM("X vector: \n"<<X );

        // extract spline coefs from the vector  ========================================================
        std::vector<double> a, b, c, d, e, f;
        for (int i = 0; i < n_eqs; i+=6) {
            a.push_back( X(i) );
            b.push_back( X(i+1) );
            c.push_back( X(i+2) );
            d.push_back( X(i+3) );
            e.push_back( X(i+4) );
            f.push_back( X(i+5) );
        }

        // sample trajectory using sample func: create variables,  sampling T_vec, Coef_mtrx
        std::vector<double> T_vec;
        T_vec.resize(n);
        for (int i=0; i< n; i++) {
            T_vec[i] = t(i);
        }
        std::vector<double> POS, VEL, ACC, JRK, T;
        std::vector<std::vector<double>> cof_mtrx;
        cof_mtrx.push_back(a);
        cof_mtrx.push_back(b);
        cof_mtrx.push_back(c);
        cof_mtrx.push_back(d);
        cof_mtrx.push_back(e);
        cof_mtrx.push_back(f);
        std::vector<double> state;
        state.resize(4);
        // states at each intermdiate times
        double frq =125, tg=0;
        while (tg< T_vec[n-1]) {
            sample(T_vec, tg, cof_mtrx, state);
            //    std::cout<< "state:"<< "p= "<< state[0]<<"   v= "<< state[1]<<"   a= "<< state[2]<<"  j= "<< state[3] ;
            T.push_back(tg);
            POS.push_back( state[0]);
            VEL.push_back( state[1]);
            ACC.push_back( state[2]);
            JRK.push_back( state[3]);
            tg += 1/frq;
        }

        // states at waypoints and check limit conditions
        lmtd_jrk = true;
        std::vector<std::vector<double>> wpt_states;
        wpt_states.resize(4);
        for (int pt = 0; pt < n; ++pt) {
            tg= T_vec[pt];
            sample(T_vec, tg, cof_mtrx, state);
            wpt_states[0].push_back( state[0] );
            wpt_states[1].push_back( state[1] );
            wpt_states[2].push_back( state[2] );
            wpt_states[3].push_back( state[3] );
            std::cout<< "jrk["<<pt<<"]= "<< state[3] ;
            if(  state[3] > 1000 || state[3] < -1000 ){
                seg_idx.push_back( pt);
                lmtd_jrk = false;
            }
        }
        // ROS_INFO( "waypoints times:    ");
        // for (int i=0; i< n; i++ )
        //    ROS_INFO_STREAM( "    "<< T_vec[i] );
        // ROS_INFO( "segment peroids:    ");
        // for (int i=0; i< n-1; i++ )
        //    ROS_INFO_STREAM( "    "<< h[i] );

        // plot section =================================
        count++;
        int plot_per_loop = 3; //to plot 9 iteration to fit on sbplts number
        int sbplt_rows= 3, sbplt_cols= 3;
        bool pos_plt=false, vel_plt=true, acc_plt=true, jrk_plt = false;// select states to plot
        pos_plt=false, vel_plt=false, acc_plt=false, jrk_plt = true;
        std::string  sbplt_name;

        //   here choose few iterations, plot them  in subplot
        if(  (count)%plot_per_loop == 0 ){
            plt::figure(1);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop   );
            plt::plot( T, POS);
            plt::plot( T_vec, wpt_states[0],"r*");
            sbplt_name= "pos_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);
            plt::figure(2);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop   );
            plt::plot(  T, VEL);
            plt::plot( T_vec, wpt_states[1],"r*");
            sbplt_name= "vel_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);
            plt::figure(3);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop   );
            plt::plot(  T, ACC);
            plt::plot( T_vec, wpt_states[2],"r*");
            sbplt_name= "acc_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);
            plt::figure(4);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop   );
            plt::plot(  T, JRK);
            plt::plot( T_vec, wpt_states[3],"r*");
            sbplt_name= "jrk_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);
            //    plt::legend(); // Enable legend.

        }
        if(lmtd_jrk){ // to plot last iteration
            plt::figure(1);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop  +1);
            plt::plot( T, POS);
            plt::plot( T_vec, wpt_states[0],"r*");
            sbplt_name= "pos_last_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);

            plt::figure(2);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop    +1);
            plt::plot(  T, VEL);
            plt::plot( T_vec, wpt_states[1],"r*");
            sbplt_name= "vel_last_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);

            plt::figure(3);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop    +1);
            plt::plot(  T, ACC);
            plt::plot( T_vec, wpt_states[2],"r*");
            sbplt_name= "acc_last_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);

            plt::figure(4);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop    +1);
            plt::plot(  T, JRK);
            plt::plot( T_vec, wpt_states[3],"r*");
            sbplt_name= "jrk_last_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);

            //    plt::legend(); // Enable legend.
        }
        ROS_INFO_STREAM("#iteration= "<<count <<"is_lmtd_jrk: "<< lmtd_jrk);
    }

    ROS_INFO_STREAM("#loops= "<<count);
    plt::show();

}


//
// Created by Vlad Argunov on 10/01/2022.
//

#include "HestonOption.h"

#include <Eigen/Dense>
#include <random>
#include <string>
#include <cmath>
#include <tuple>
#include <algorithm>
#include "Simulation.h"

HestonOption::HestonOption(std::string type_option_, double tn_, int n_processes_, int n_steps_, double strike_) {
    sim = Simulation();
    type_option = type_option_;
    tn = tn_;
    n_processes = n_processes_;
    n_steps = n_steps_;
    strike = strike_;
}

void HestonOption::set_params_stock_process(double stock_price_start_, double mu_) {
    stock_price_start = stock_price_start_;
    mu = mu_;
}

void HestonOption::set_prams_variance_process(double variance_start_, double kappa_, double theta_, double sigma_,
                                              double rho_) {
    variance_start = variance_start_;
    kappa = kappa_;
    theta = theta_;
    sigma = sigma_;
    rho = rho_;
}

double HestonOption::compute_price() {
    auto [stocks, _] = sim.generate_heston_process(n_processes, tn, stock_price_start, mu, variance_start, kappa, theta, sigma, rho, n_steps);
    Eigen::Index stocks_cols = stocks.cols();
    double total_payoffs = 0;
    for (int row = 0; row < stocks.rows(); ++row) {
        if (type_option == "Call"){
            total_payoffs += std::max(stocks(row,stocks_cols - 1) - strike, 0.0);
        }
        if (type_option == "Put"){
            total_payoffs += std::max(strike - stocks(row,stocks_cols), 0.0);
        }
    }
    return exp(- mu * tn) * total_payoffs / n_processes;
}