#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <unsupported/Eigen/FFT>
#include <gravitysolvers.hpp>

// ************************************************************************* //
// ******************************** DATA IO ******************************** //
// ************************************************************************* //

Gravitysolver::DataIO::DataIO()
{
  epsilon = 0.0;
}

bool Gravitysolver::DataIO::readDataOld(const std::string &filename)
{
  std::ifstream infile(filename);
  int numParticles, numGasParticles, numStarParticles;

  if(infile.is_open()) {
    infile >> numParticles >> numGasParticles >> numStarParticles;

    particles = Eigen::MatrixXf::Zero(MATRIX_DATA_ROWS, numParticles);

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(0, i); // m
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(1, i); // x
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(2, i); // y
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(3, i); // z
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(4, i); // vx
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(5, i); // vy
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(6, i); // vz
    }

    return true;
  }

  std::cout << "Gravitysolver::DataIO::readDataOld(\"" << filename << "\") " << "could not read file" << std::endl;
  return false;
}

bool Gravitysolver::DataIO::readData(const std::string &filename)
{
  std::ifstream infile(filename);
  int N;

  if(infile.is_open()) {
    infile >> N >> epsilon;
    particles = Eigen::MatrixXf::Zero(MATRIX_DATA_ROWS, N);

    float m, x, y, z, vx, vy, vz, fx, fy, fz;

    for(int j = 0; j < N; j++) {
      infile >> m >> x >> y >> z >> vx >> vy >> vz >> fx >> fy >> fz;
      particles(0, j) = m;
      particles(1, j) = x;
      particles(2, j) = y;
      particles(3, j) = z;
      particles(4, j) = vx;
      particles(5, j) = vy;
      particles(6, j) = vz;
      particles(7, j) = fx;
      particles(8, j) = fy;
      particles(9, j) = fz;
    }

    return true;
  }

  std::cout << "Gravitysolver::DataIO::readData(\"" << filename << "\") " << "could not read file" << std::endl;
  return false;
}

bool Gravitysolver::DataIO::writeData(const std::string &filename)
{
  std::ofstream outfile(filename);
  int N = particles.cols();

  if(outfile.is_open()) {
    outfile << N << " " << epsilon << "\n";

    for(int j = 0; j < N; j++) {
      outfile << particles(0, j) << "\n"; // m
      outfile << particles(1, j) << "\n"; // x
      outfile << particles(2, j) << "\n"; // y
      outfile << particles(3, j) << "\n"; // z
      outfile << particles(4, j) << "\n"; // vx
      outfile << particles(5, j) << "\n"; // vy
      outfile << particles(6, j) << "\n"; // vz
      outfile << particles(7, j) << "\n"; // fx
      outfile << particles(8, j) << "\n"; // fy
      outfile << particles(9, j) << "\n"; // fz
    }

    return true;
  }

  std::cout << "Gravitysolver::DataIO::writeData(\"" << filename << "\") " << "could not write file" << std::endl;
  return false;
}

// ************************************************************************* //
// ***************************** DIRECT SOLVER ***************************** //
// ************************************************************************* //

Gravitysolver::Direct::Direct()
{

}

float Gravitysolver::Direct::softening()
{
  return epsilon;
}

void Gravitysolver::Direct::setSoftening(float eps)
{
  epsilon = eps;
}

void Gravitysolver::Direct::solve()
{
  for(int i = 0; i < particles.cols(); i++) {
    particles(7, i) = .0;
    particles(8, i) = .0;
    particles(9, i) = .0;
  }

  float mi, mj, dx, dy, dz, sqrtInvDist, sqrtInvDist3, fx, fy, fz;

  for(int i = 0; i < particles.cols(); i++) {
    for(int j = i + 1; j < particles.cols(); j++) {
      mi = particles(0, i);
      mj = particles(0, j);
      dx = particles(1, i) - particles(1, j);
      dy = particles(2, i) - particles(2, j);
      dz = particles(3, i) - particles(3, j);

      sqrtInvDist = 1.0 / std::sqrt(dx * dx + dy * dy + dz * dz + epsilon * epsilon);
      sqrtInvDist3 = sqrtInvDist * sqrtInvDist * sqrtInvDist;

      // Assumption: G = 1
      fx = -mi * mj * dx * sqrtInvDist3;
      fy = -mi * mj * dy * sqrtInvDist3;
      fz = -mi * mj * dz * sqrtInvDist3;

      particles(7, i) += fx;
      particles(8, i) += fy;
      particles(9, i) += fz;
      particles(7, j) -= fx;
      particles(8, j) -= fy;
      particles(9, j) -= fz;
    }
  }
}

const MatrixData &Gravitysolver::Direct::data()
{
  return particles;
}

// ************************************************************************* //
// ******************************* PM SOLVER ******************************* //
// ************************************************************************* //

Gravitysolver::PM::PM(int numGridCells)
{
  // The Tensor in Eigen experiences a strange case of std::bad_alloc()
  // if it has more than 776 = 2*388 complex valued elements per dimension
  if(numGridCells > 0 && numGridCells <= 388) {
    Ng = numGridCells;
  }
  else {
    std::cout << "Gravitysolver::PM::PM(" << numGridCells << ") numGridCells must be > 0" << std::endl;
    Ng = 1;
  }

  h = .0;
}

/**
* Returns indices for a mesh cell for the galaxy given world coordinates.
* World coordinates have the center of the galaxy at (0, 0, 0).
* The mapping to grid coordinates is an affine mapping where the galaxy center
* (0, 0, 0) is in the center of the mesh as well.
*/
Vector3i Gravitysolver::PM::worldToGrid(float x, float y, float z)
{
  Vector3i gridCoor;

  gridCoor(0) = std::floor((x + (worldLen / 2)) / worldLen * Ng);
  gridCoor(1) = std::floor((y + (worldLen / 2)) / worldLen * Ng);
  gridCoor(2) = std::floor((z + (worldLen / 2)) / worldLen * Ng);

  return gridCoor;
}

/**
* Returns position at the center of a mesh cell in world coordinates given cell indices
* The position is mapped in a fashin such that (0, 0, 0) is the center of the galaxy
* in world coordinates.
*/
Vector3f Gravitysolver::PM::gridToWorld(int i, int j, int k)
{
  Vector3f worldCoor;

  worldCoor(0) = (i * worldLen / Ng) - (worldLen / 2) + (h / 2);
  worldCoor(1) = (j * worldLen / Ng) - (worldLen / 2) + (h / 2);
  worldCoor(2) = (k * worldLen / Ng) - (worldLen / 2) + (h / 2);

  return worldCoor;
}

void Gravitysolver::PM::fft3d(FieldTensorCF &t)
{
  const int x = t.dimension(0);
  const int y = t.dimension(1);
  const int z = t.dimension(2);

  Eigen::FFT<float> fft;

  for(int k = 0; k < z; k++) { // for each 2d sheet make a 2d fft
    for(int j = 0; j < y; j++) { // fft in x-dir
      VectorXcf tv(x);
      for(int i = 0; i < x; i++)
          tv(i) = t(i, j, k);

      VectorXcf fv = fft.fwd(tv);
      for(int i = 0; i < x; i++)
          t(i, j, k) = fv(i);
    }

    for(int i = 0; i < x; i++) { // fft in y-dir
      VectorXcf tv(y);
      for(int j = 0; j < y; j++)
          tv(j) = t(i, j, k);

      VectorXcf fv = fft.fwd(tv);
      for(int j = 0; j < y; j++)
          t(i, j, k) = fv(j);
    }
  }

  for(int i = 0; i < x; i++) { // and for each of the x*y spikes pointing upwards in z-dir do a 1D fft
    for(int j = 0; j < y; j++) {

      VectorXcf tv(z);
      for(int k = 0; k < z; k++)
        tv(k) = t(i, j, k);

      VectorXcf fv = fft.fwd(tv);
      for(int k = 0; k < z; k++)
        t(i, j, k) = fv(k);
    }
  }
}

void Gravitysolver::PM::ifft3d(FieldTensorCF &t)
{
  const int x = t.dimension(0);
  const int y = t.dimension(1);
  const int z = t.dimension(2);

  const float invXYZ = 1.0 / (x*y*z);

  for(int i = 0; i < x; i++) {
    for(int j = 0; j < y; j++) {
      for(int k = 0; k < z; k++) {
        t(i,j,k) = std::conj(t(i,j,k));
      }
    }
  }

  fft3d(t);

  for(int i = 0; i < x; i++) {
    for(int j = 0; j < y; j++) {
      for(int k = 0; k < z; k++) {
        t(i,j,k) = std::conj(t(i,j,k)) * invXYZ;
      }
    }
  }
}

void Gravitysolver::PM::conv3d(FieldTensorCF &out, FieldTensorCF &in, FieldTensorCF &kernel)
{
  int x = in.dimension(0);
  int y = in.dimension(1);
  int z = in.dimension(2);

  fft3d(in);
  fft3d(kernel);

  for(int i = 0; i < x; i++) {
    for(int j = 0; j < y; j++) {
      for(int k = 0; k < z; k++) {
        out(i,j,k) = in(i,j,k) * kernel(i,j,k);
      }
    }
  }

  ifft3d(out);
}

void Gravitysolver::PM::solve()
{
  const int N = particles.cols();
  Vector3f maxPos = particles.block(1, 0, 3, N).rowwise().maxCoeff();
  Vector3f minPosAbs = particles.block(1, 0, 3, N).rowwise().minCoeff().cwiseAbs();

  worldLen = std::max(maxPos.maxCoeff(), minPosAbs.maxCoeff()) * 2;

  // adds one layer of cells in each dimension so particles at the edge will
  // contribute with their entire mass to the density field
  worldLen += (worldLen / Ng);
  h = worldLen / Ng;

  std::cout << "PM solver global parameters" << std::endl;
  std::cout << "---------------------------" << std::endl;
  std::cout << "worldLen:   " << worldLen << std::endl;
  std::cout << "Ng:         " << Ng << std::endl;
  std::cout << "h:          " << h << std::endl;
  std::cout << "---------------------------" << std::endl;

  density.resize(2*Ng, 2*Ng, 2*Ng);
  greenFunction.resize(2*Ng, 2*Ng, 2*Ng);
  potential.resize(2*Ng, 2*Ng, 2*Ng);

  ax.resize(Ng, Ng, Ng);
  ay.resize(Ng, Ng, Ng);
  az.resize(Ng, Ng, Ng);

  // density field construction
  for(int col = 0; col < particles.cols(); col++) {
    float m = particles(0, col);
    float px = particles(1, col);
    float py = particles(2, col);
    float pz = particles(3, col);

    // (NGP)
    Vector3i ti = worldToGrid(px, py, pz);
    density(ti(0), ti(1), ti(2)) += (m / (h*h*h));
  }

  // green's function construction
  for(int i = 0; i < Ng; i++) {
    for(int j = 0; j < Ng; j++) {
      for(int k = 0; k < Ng; k++) {
        Vector3f worldPos = gridToWorld(i, j, k);

        // Assumption: G = 1
        float res = -1.0 / worldPos.norm();

        // make the function symmetric accross all axes
        greenFunction(i,               j,            k) = res;
        greenFunction(i,               j,     2*Ng-k-1) = res;
        greenFunction(i,        2*Ng-j-1,            k) = res;
        greenFunction(i,        2*Ng-j-1,     2*Ng-k-1) = res;
        greenFunction(2*Ng-i-1,        j,            k) = res;
        greenFunction(2*Ng-i-1,        j,     2*Ng-k-1) = res;
        greenFunction(2*Ng-i-1, 2*Ng-j-1,            k) = res;
        greenFunction(2*Ng-i-1, 2*Ng-j-1,     2*Ng-k-1) = res;
      }
    }
  }

  // solve the poisson equation to get the potential
  conv3d(potential, density, greenFunction);

  // calculate acceleration field from potential
  for(int i = 1; i < Ng - 1; i++) {
    for(int j = 1; j < Ng - 1; j++) {
      for(int k = 1; k < Ng - 1; k++) {
        ax(i, j, k) = -(potential(i+1,j,k).real() - potential(i-1,j,k).real()) / (2*h);
        ay(i, j, k) = -(potential(i,j+1,k).real() - potential(i,j-1,k).real()) / (2*h);
        az(i, j, k) = -(potential(i,j,k+1).real() - potential(i,j,k-1).real()) / (2*h);
      }
    }
  }

  // interpolate acceleration field back from mesh to particles
  for(int col = 0; col < particles.cols(); col++) {
    float m = particles(0, col);
    float px = particles(1, col);
    float py = particles(2, col);
    float pz = particles(3, col);

    // (NGP)
    Vector3i ti = worldToGrid(px, py, pz);

    // F = m*a
    particles(7, col) = m * ax(ti(0), ti(1), ti(2));
    particles(8, col) = m * ay(ti(0), ti(1), ti(2));
    particles(9, col) = m * az(ti(0), ti(1), ti(2));
  }
}

const MatrixData &Gravitysolver::PM::data()
{
  return particles;
}


#pragma once
#include <memory>
#include <Eigen/Dense>
#include "Bound3Intersect.hpp"
#include "../Ray.hpp"

class Bound3
{
public:
    Bound3(Eigen::Vector3f min, Eigen::Vector3f max) : _min(min), _max(max){};

    Bound3Intersect intersect_ray(const Ray &ray) const
    {
        const auto &origin = ray.get_origin();
        const auto &dir = ray.get_direction();

        auto tIn = (_min - origin).cwiseQuotient(dir).minCoeff();
        auto tOut = (_max - origin).cwiseQuotient(dir).maxCoeff();

        if (tIn < std::numeric_limits<float>::epsilon())
            return Bound3Intersect(0.0f, tOut);

        if (tOut - tIn > -std::numeric_limits<float>::epsilon())
            return Bound3Intersect(tIn, tOut);

        return Bound3Intersect();
    };

    static std::unique_ptr<Bound3> union_bound3(const std::unique_ptr<Bound3> &box1, const std::unique_ptr<Bound3> &box2)
    {
        auto min = box1->_min.cwiseMin(box2->_min);
        auto max = box1->_max.cwiseMax(box2->_max);
        return std::make_unique<Bound3>(min, max);
    };

    const Eigen::Vector3f &min() { return _min; };

    const Eigen::Vector3f &max() { return _max; };

private:
    Eigen::Vector3f _min;

    Eigen::Vector3f _max;
};


/*
 * SWE_Plane_Normal_Modes.hpp
 *
 *  Created on: 17 Nov 2019
 *      Author: Pedro Peixoto <pedrosp@ime.usp.br>
 *
 *      based on previous implementation by Martin Schreiber in swe_plane.cpp
 *
 */

#ifndef SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_
#define SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_

#include <rexi/EXPFunctions.hpp>
#include <sweet/plane/PlaneData_Physical.hpp>
#include <sweet/plane/PlaneData_Spectral.hpp>
#include <sweet/plane/PlaneData_SpectralComplex.hpp>
#include <sweet/SimulationVariables.hpp>
#include <sweet/plane/PlaneOperators.hpp>
#include <functional>
#if SWEET_EIGEN
#include <Eigen/Eigenvalues>
#endif
/**
 * SWE Plane normal mode
 */
class SWE_Plane_Normal_Modes
{
public:

	template <typename TCallbackClass>
	static
	void normal_mode_analysis(
			PlaneData_Spectral &io_prog_h_pert, // h: surface height (perturbation)
			PlaneData_Spectral &io_prog_u, // u: velocity in x-direction
			PlaneData_Spectral &io_prog_v, // v: velocity in y-direction
			int number_of_prognostic_variables,
			SimulationVariables &i_simVars, // Simulation variables
			TCallbackClass *i_class,
			void(TCallbackClass::* const i_run_timestep_method)(void)
	)
	{

		const PlaneDataConfig *planeDataConfig = io_prog_h_pert.planeDataConfig;

		// dummy time step to get time step size
		if (i_simVars.timecontrol.current_timestep_size <= 0)
			SWEETError("Normal mode analysis requires setting fixed time step size");

		/*
		 *
		 * Mode-wise normal mode analysis
		 *
		 *
		 */

		if (i_simVars.misc.normal_mode_analysis_generation == 4)
		{
#if SWEET_EIGEN
#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			SWEETError("SWE_Plane_Normal_Modes: This test was build for linear or linearized models, so please compile without dealising --plane-spectral-dealiasing=disable.");
#endif

			/*
			 * Setup all output files
			 */
			const char* filename; //general filename
			char buffer_real[1024];

			if (i_simVars.iodata.output_file_name == "")
				filename = "output_%s_t%020.8f.csv";
			else
				filename = i_simVars.iodata.output_file_name.c_str();

			sprintf(buffer_real, filename, "normal_modes_plane", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file(buffer_real, std::ios_base::trunc);
			std::cout << "Writing normal mode analysis to files of the form '" << buffer_real << "'" << std::endl;

			//Positive inertia-gravity modes
			sprintf(buffer_real, filename, "normal_modes_plane_igpos", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_igpos(buffer_real, std::ios_base::trunc);

			//Negative inertia-gravity modes
			sprintf(buffer_real, filename, "normal_modes_plane_igneg", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_igneg(buffer_real, std::ios_base::trunc);

			//Geostrophic modes
			sprintf(buffer_real, filename, "normal_modes_plane_geo", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_geo(buffer_real, std::ios_base::trunc);

			//std::cout << "WARNING: OUTPUT IS TRANSPOSED!" << std::endl;

			// use very high precision
			file << std::setprecision(20);
			file_igpos << std::setprecision(20);
			file_igneg << std::setprecision(20);
			file_geo << std::setprecision(20);

			file << "# dt " << i_simVars.timecontrol.current_timestep_size << std::endl;
			file << "# g " << i_simVars.sim.gravitation << std::endl;
			file << "# h " << i_simVars.sim.h0 << std::endl;
			file << "# r " << i_simVars.sim.sphere_radius << std::endl;
			file << "# f " << i_simVars.sim.plane_rotating_f0 << std::endl;

#if SWEET_USE_PLANE_SPECTRAL_SPACE
			int specmodes = planeDataConfig->get_spectral_iteration_range_area(0)+planeDataConfig->get_spectral_iteration_range_area(1);
			file << "# specnummodes " << specmodes << std::endl;
			file << "# specrealresx " << planeDataConfig->spectral_real_modes[0] << std::endl;
			file << "# specrealresy " << planeDataConfig->spectral_real_modes[1] << std::endl;
#endif

			file << "# physresx " << planeDataConfig->physical_res[0] << std::endl;
			file << "# physresy " << planeDataConfig->physical_res[1] << std::endl;
			file << "# normalmodegeneration " << i_simVars.misc.normal_mode_analysis_generation << std::endl;
			file << "# antialiasing ";
#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			file << 1;
#else
			file << 0;
#endif
			file << std::endl;

			PlaneData_Spectral* prog[3] = {&io_prog_h_pert, &io_prog_u, &io_prog_v};

			int number_of_prognostic_variables = 3;
			//The basic state is with zero in all variables
			// The only non zero variable in the basic state is the total height
			//    for which the constant is added within run_timestep()
			io_prog_h_pert.spectral_set_zero();
			io_prog_u.spectral_set_zero();
			io_prog_v.spectral_set_zero();

			//int num_timesteps = 1;

			// Timestep and perturbation
			double dt = i_simVars.timecontrol.current_timestep_size;
			double eps = dt;

			//Matrix representing discrete linear operator in spectral space
			Eigen::MatrixXcf A(3,3) ;
			//Eigen solver
			Eigen::ComplexEigenSolver<Eigen::MatrixXcf> ces;
			//Final eigenvalues
			std::complex<double> eval[3];

			//For each spectral mode
			//for (int r = 0; r < 2; r++) //only required to get the symmetric half of the spectrum
			//{
			int r = 0;

			for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
			{
				std::cout << "." << std::flush;
				for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
				{
					//This is the mode to be analysed
					//std::cout << "Mode (i,j)= (" << i << " , " << j <<")"<< std::endl;

					for (int outer_prog_id = 0; outer_prog_id < number_of_prognostic_variables; outer_prog_id++)
					{

						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog[inner_prog_id]->spectral_set_zero();

						// activate mode via real coefficient
						prog[outer_prog_id]->spectral_set(j, i, 1.0);
						//Activate the symetric couterpart of the mode (only needed if j>0 )
						if (j > 0)
							prog[outer_prog_id]->spectral_set(planeDataConfig->spectral_data_size[1]-j, i, 1.0);

						/*
						 * RUN timestep
						 */
						////prog[outer_prog_id]->request_data_physical();
						(i_class->*i_run_timestep_method)();

						/*
						 * compute
						 * 1/dt * (U(t+1) - U(t))
						 */
						///////prog[outer_prog_id]->request_data_spectral();

						std::complex<double> val = prog[outer_prog_id]->spectral_get(j, i);
						val = val - 1.0; //subtract U(0) from mode
						prog[outer_prog_id]->spectral_set(j, i, val);

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							(*prog[inner_prog_id]) /= eps;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							A(inner_prog_id,outer_prog_id)=prog[inner_prog_id]->spectral_get(j, i);;
						}

					}

					//std::cout << "Lik matrix" << std::endl;
					//std::cout << A << std::endl;

					//std::cout<<"Normal modes" << std::endl;
					ces.compute(A);
					for(int i=0; i<3; i++)
					{
						eval[i]=ces.eigenvalues()[i];
						//std::cout << "Eigenvalue "<< i << " : " << eval[i].real() <<" "<<eval[i].imag() << std::endl;

					}
					/* We will try to separate the modes in 3 types:
					 * -positive inertia-gravity (imag>f) - we will adopt coriolis f to test as if > zero, since the exact freq is sqrt(f^2+cK*K)
					 * -negative inertia-gravity (imag<-f)
					 * -negative inertia-gravity (imag aprox 0) - we will fit all other modes here
					 */
					int count_igpos=0;
					int count_igneg=0;
					int count_geo=0;
					for(int i=0; i<3; i++)
					{
						if(eval[i].imag() > 0.5 * i_simVars.sim.plane_rotating_f0)
						{
							//std::cout<< "IG pos mode: " << eval[i].imag() << std::endl;
							//file_igpos << eval[i].imag();
							file_igpos << eval[i].real()<< "\t" << eval[i].imag();
							file_igpos << "\t";
							count_igpos++;
						}
						if(eval[i].imag() < - 0.5 * i_simVars.sim.plane_rotating_f0)
						{
							//std::cout<< "IG neg mode: " << eval[i].imag() << std::endl;
							//file_igneg << eval[i].imag();
							file_igneg << eval[i].real()<< "\t" << eval[i].imag();
							file_igneg << "\t";
							count_igneg++;
						}
						if(eval[i].imag() >= - 0.5 * i_simVars.sim.plane_rotating_f0 && eval[i].imag() <=  0.5 * i_simVars.sim.plane_rotating_f0 )
						{
							//std::cout<< "IG geo mode: " << eval[i].imag() << std::endl;
							//file_geo << eval[i].imag();
							file_geo << eval[i].real()<< "\t" << eval[i].imag();
							file_geo << "\t";
							count_geo++;
						}
					}
					//Check if we got the correct modes
					if ( count_igpos * count_igneg * count_geo > 0 )
					{
						count_igpos=0;
						count_igneg=0;
						count_geo=0;
					}
					else
					{
						SWEETError("SWE_Plane_Normal_Modes: Could not separate modes!!");
					}

					//std::cout<<"-------------------------" << std::endl;
				}
				file_igpos << std::endl;
				file_igneg << std::endl;
				file_geo << std::endl;
			}

			//}
			//std::cout<<"-------------------------" << std::endl;
			//SWEETError("still needs work...");
#else
			SWEETError("SWE_Plane_Normal_Modes: Cannot test this without Eigen library. Please compile with --eigen=enable");
#endif
		}
		/*
		 * Do a normal mode analysis using perturbation, see
		 * Hillary Weller, John Thuburn, Collin J. Cotter,
		 * "Computational Modes and Grid Imprinting on Five Quasi-Uniform Spherical C Grids"
		 */
		else
		{

			//run_timestep();
			const char* filename;
			char buffer_real[1024];

			if (i_simVars.iodata.output_file_name == "")
				filename = "output_%s_normalmodes.csv";
			else
				filename = i_simVars.iodata.output_file_name.c_str();

			sprintf(buffer_real, filename, "normal_modes_physical", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file(buffer_real, std::ios_base::trunc);
			std::cout << "Writing normal mode analysis to file '" << buffer_real << "'" << std::endl;

			std::cout << "WARNING: OUTPUT IS TRANSPOSED!" << std::endl;

			// use very high precision
			file << std::setprecision(20);

			PlaneData_Spectral* prog[3] = {&io_prog_h_pert, &io_prog_u, &io_prog_v};

			/*
			 * Maximum number of prognostic variables
			 *
			 * Advection e.g. has only one
			 */
			if (number_of_prognostic_variables <= 0)
				SWEETError("simVars.pde.number_of_prognostic_variables must be set!");

			if (number_of_prognostic_variables == 3)
			{
				io_prog_h_pert.spectral_set_zero();
				io_prog_u.spectral_set_zero();
				io_prog_v.spectral_set_zero();
			}
			else if (number_of_prognostic_variables == 1)
			{
				io_prog_h_pert.spectral_set_zero();
			}
			else
			{
				SWEETError("Not yet supported");
			}

#if 0
			if (i_simVars.disc.timestepping_method == SimulationVariables::Discretization::LEAPFROG_EXPLICIT)
			{
				SWEETError("Not yet tested and supported");
				std::cout << "WARNING: Leapfrog time stepping doesn't make real sense since 1st step is based on RK-like method" << std::endl;
				std::cout << "We'll do two Leapfrog time steps here to take the LF errors into account!" << std::endl;
				std::cout << "Therefore, we also halve the time step size here" << std::endl;

				i_simVars.timecontrol.current_timestep_size = 0.5*i_simVars.sim.CFL;
				i_simVars.sim.CFL = -i_simVars.timecontrol.current_timestep_size;
			}
#endif

			int num_timesteps = 1;
			if (i_simVars.misc.normal_mode_analysis_generation >= 10)
			{
				if (i_simVars.timecontrol.max_timesteps_nr > 0)
					num_timesteps = i_simVars.timecontrol.max_timesteps_nr;
			}

			if (i_simVars.timecontrol.max_simulation_time > 0)
				file << "# t " << i_simVars.timecontrol.max_simulation_time << std::endl;
			else
				file << "# t " << (num_timesteps*(-i_simVars.timecontrol.current_timestep_size)) << std::endl;

			file << "# g " << i_simVars.sim.gravitation << std::endl;
			file << "# h " << i_simVars.sim.h0 << std::endl;
//			file << "# r " << i_simVars.sim.sphere_radius << std::endl;
			file << "# f " << i_simVars.sim.plane_rotating_f0 << std::endl;

#if SWEET_USE_PLANE_SPECTRAL_SPACE
			int specmodes = planeDataConfig->get_spectral_iteration_range_area(0)+planeDataConfig->get_spectral_iteration_range_area(1);
			file << "# specnummodes " << specmodes << std::endl;
			file << "# specrealresx " << planeDataConfig->spectral_real_modes[0] << std::endl;
			file << "# specrealresy " << planeDataConfig->spectral_real_modes[1] << std::endl;
#endif

			file << "# physresx " << planeDataConfig->physical_res[0] << std::endl;
			file << "# physresy " << planeDataConfig->physical_res[1] << std::endl;
			file << "# normalmodegeneration " << i_simVars.misc.normal_mode_analysis_generation << std::endl;
			file << "# antialiasing ";

#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			file << 1;
#else
			file << 0;
#endif

			file << std::endl;

			// iterate over all prognostic variables
			for (int outer_prog_id = 0; outer_prog_id < number_of_prognostic_variables; outer_prog_id++)
			{
				if (i_simVars.misc.normal_mode_analysis_generation == 1 || i_simVars.misc.normal_mode_analysis_generation == 11)
				{
					// iterate over physical space
					for (std::size_t outer_i = 0; outer_i < planeDataConfig->physical_array_data_number_of_elements; outer_i++)
					{
						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						std::cout << "." << std::flush;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog[inner_prog_id]->spectral_set_zero();

						// activate mode
						///prog[outer_prog_id]->request_data_physical();
						///prog[outer_prog_id]->physical_space_data[outer_i] = 1;
						PlaneData_Physical tmp = prog[outer_prog_id]->toPhys();
						tmp.physical_space_data[outer_i] = 1;
						prog[outer_prog_id]->loadPlaneDataPhysical(tmp);

						/*
						 * RUN timestep
						 */

						(i_class->*i_run_timestep_method)();

						if (i_simVars.misc.normal_mode_analysis_generation == 1)
						{
							/*
							 * compute
							 * 1/dt * (U(t+1) - U(t))
							 */
							///////prog[outer_prog_id]->request_data_physical();
							///////prog[outer_prog_id]->physical_space_data[outer_i] -= 1.0;
							PlaneData_Physical tmp2 = prog[outer_prog_id]->toPhys();
							tmp2.physical_space_data[outer_i] -= 1.0;

							for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								tmp2 /= i_simVars.timecontrol.current_timestep_size;
								//(*prog[inner_prog_id]) /= i_simVars.timecontrol.current_timestep_size;

							prog[outer_prog_id]->loadPlaneDataPhysical(tmp2);
						}

						tmp = prog[outer_prog_id]->toPhys();
						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							///prog[inner_prog_id]->request_data_physical();
							for (std::size_t k = 0; k < planeDataConfig->physical_array_data_number_of_elements; k++)
							{
								///file << prog[inner_prog_id]->physical_space_data[k];
								file << tmp.physical_space_data[k];
								if (inner_prog_id != number_of_prognostic_variables-1 || k != planeDataConfig->physical_array_data_number_of_elements-1)
									file << "\t";
								else
									file << std::endl;
							}
						}
					}
				}
#if 1
				else if (i_simVars.misc.normal_mode_analysis_generation == 3 || i_simVars.misc.normal_mode_analysis_generation == 13)
				{
#if !SWEET_USE_PLANE_SPECTRAL_SPACE
					SWEETError("Only available with if plane spectral space is activated during compile time!");
#else

					// iterate over spectral space
					for (int r = 0; r < 2; r++)
					{

						for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
						{
							for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
							{
								// reset time control
								i_simVars.timecontrol.current_timestep_nr = 0;
								i_simVars.timecontrol.current_simulation_time = 0;

								std::cout << "." << std::flush;

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
									prog[inner_prog_id]->spectral_set_zero();

								// activate mode via real coefficient
								prog[outer_prog_id]->spectral_set(j, i, 1.0);

								/*
								 * RUN timestep
								 */
								(i_class->*i_run_timestep_method)();

								if (i_simVars.misc.normal_mode_analysis_generation == 3)
								{
									/*
									 * compute
									 * 1/dt * (U(t+1) - U(t))
									 */
									///prog[outer_prog_id]->request_data_spectral();

									std::complex<double> val = prog[outer_prog_id]->spectral_get(j, i);
									val = val - 1.0;
									prog[outer_prog_id]->spectral_set(j, i, val);

									for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
										(*prog[inner_prog_id]) /= i_simVars.timecontrol.current_timestep_size;
								}

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								{
									///prog[inner_prog_id]->request_data_spectral();

									/*
									 * REAL
									 */

									for (int r = 0; r < 2; r++)
									{
										for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
										{
											for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
											{
												file << prog[inner_prog_id]->spectral_get(j, i).real();
												file << "\t";
											}
										}
									}
								}

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								{
									/*
									 * IMAG
									 */
									int c = 0;
									for (int r = 0; r < 2; r++)
									{
										for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
										{
											for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
											{
												file << prog[inner_prog_id]->spectral_get(j, i).imag();

												if (inner_prog_id != number_of_prognostic_variables-1 || c != specmodes-1)
													file << "\t";
												else
													file << std::endl;

												c++;
											}
										}
									}
								}
							}
						}
					}
#endif
				}
#else
				else if (i_simVars.misc.normal_mode_analysis_generation == 3 || i_simVars.misc.normal_mode_analysis_generation == 13)
				{
					PlaneData_SpectralComplex t1(planeDataConfig);
					PlaneData_SpectralComplex t2(planeDataConfig);
					PlaneData_SpectralComplex t3(planeDataConfig);
					PlaneDataComplex* prog_cplx[3] = {&t1, &t2, &t3};

					// iterate over spectral space
					for (std::size_t outer_i = 0; outer_i < planeDataConfig->spectral_complex_array_data_number_of_elements; outer_i++)
					{
						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						std::cout << "." << std::flush;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog_cplx[inner_prog_id]->spectral_set_zero();

						// activate mode via real coefficient
						prog_cplx[outer_prog_id]->request_data_spectral();
						prog_cplx[outer_prog_id]->spectral_space_data[outer_i].real(1);

						// convert PlaneData_SpectralComplex to PlaneData
						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							*prog[inner_prog_id] = Convert_PlaneDataSpectralComplex_To_PlaneDataSpectral::physical_convert(*prog_cplx[inner_prog_id]);
							prog[inner_prog_id]->spectral_zeroAliasingModes();
						}

						/*
						 * RUN timestep
						 */
						(i_class->*i_run_timestep_method)();

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							prog[inner_prog_id]->spectral_zeroAliasingModes();
#warning "update this physical_convert maybe to spectral_convert"

							*prog_cplx[inner_prog_id] = Convert_PlaneDataSpectral_To_PlaneDataSpectralComplex::physical_convert(*prog[inner_prog_id]);

							prog_cplx[inner_prog_id]->request_data_spectral();
						}

						if (i_simVars.misc.normal_mode_analysis_generation == 3)
						{
							/*
							 * compute
							 * 1/dt * (U(t+1) - U(t))
							 */
							prog_cplx[outer_prog_id]->request_data_spectral();
							prog_cplx[outer_prog_id]->spectral_space_data[outer_i] -= 1.0;

							for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								prog_cplx[inner_prog_id]->operator*=(1.0/i_simVars.timecontrol.current_timestep_size);
						}

						// convert PlaneData_SpectralComplex to PlaneData
						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							prog_cplx[inner_prog_id]->request_data_spectral();

							/*
							 * REAL
							 */
							for (std::size_t k = 0; k < planeDataConfig->spectral_complex_array_data_number_of_elements; k++)
							{
								file << prog_cplx[inner_prog_id]->spectral_space_data[k].real();
								file << "\t";
							}

							/*
							 * IMAG
							 */
							for (std::size_t k = 0; k < planeDataConfig->spectral_complex_array_data_number_of_elements; k++)
							{
								file << prog_cplx[inner_prog_id]->spectral_space_data[k].imag();

								if (inner_prog_id != number_of_prognostic_variables-1 || k != planeDataConfig->spectral_complex_array_data_number_of_elements-1)
									file << "\t";
								else
									file << std::endl;
							}
						}
					}
				}
#endif
			}
		}
	}

	~SWE_Plane_Normal_Modes()
	{

	}
};

#endif /* SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_ */


#include <boost/gil.hpp>
#include <boost/gil/extension/io/jpeg.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>
#include <boost/gil/extension/numeric/resample.hpp>
#include <boost/gil/extension/numeric/kernel.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>

#include <cmath>

using namespace std;
using namespace boost::gil;

// value of strong edge pixel
static const int TOP_VALUE = 255;
// value of weak (maybe edge) pixel
static const int MIDDLE_VALUE = 150;
// value of definitely not-edge pixel
static const int BOTTOM_VALUE = 0;

// Example how to detect edges via Canny algorithm
// Read more about Canny edge detection algorithm on
// https://docs.opencv.org/3.1.0/da/d22/tutorial_py_canny.html

template <typename SrcView>
void gaussian_blur(const SrcView &src)
{
    //1-D Gaussian blur kernel with kernel size 5, Sigma 1.0
    float gaussian[] = {0.06136f, 0.24477f, 0.38774f, 0.24477f, 0.06136f};
    kernel_1d<float> kernel(gaussian, 5, 2);

    convolve_rows<gray32f_pixel_t>(src, kernel, src, convolve_option_output_ignore);
    convolve_cols<gray32f_pixel_t>(src, kernel, src, convolve_option_output_ignore);
}

// Function calculates image gradient (intensity and direction)
// Smoothened image is filtered with a Sobel kernels in both horizontal and vertical
// direction to get first derivative in horizontal direction (Gx) and vertical direction (Gy).
// Gradient manitude is found as G = sqrt(Gx^2 + Gy^2)
// Direction is found as angle = arctan(Gy / Gx)
// More information about sobel filtering on https://en.wikipedia.org/wiki/Sobel_operator
void sobel_filtering(const gray8c_view_t &src,
                     const gray8_view_t &magnitude, const gray32f_view_t &slope)
{
    // This we used 1-D convolution
    // Gx = second * (first * img)
    // Gy = first * (second * img)
    // where * is convolution
    float first_sobel[] = {1.f, 0.f, -1.f};
    float second_sobel[] = {1.f, 2.f, 1.f};

    kernel_1d<float> first_sobel_kernel(first_sobel, 3, 1);
    kernel_1d<float> second_sobel_kernel(second_sobel, 3, 1);

    //16 bits signed matrix is used in order to avoid char overflow
    gray16s_image_t vertical(src.dimensions());
    gray16s_image_t horizontal(src.dimensions());

    convolve_rows<gray32f_pixel_t>(src, first_sobel_kernel,
                                   view(vertical), convolve_option_output_zero);
    convolve_cols<gray32f_pixel_t>(const_view(vertical), second_sobel_kernel,
                                   view(vertical), convolve_option_output_zero);

    convolve_rows<gray32f_pixel_t>(src, second_sobel_kernel, view(horizontal),
                                   convolve_option_output_zero);
    convolve_cols<gray32f_pixel_t>(const_view(horizontal), first_sobel_kernel,
                                   view(horizontal), convolve_option_output_zero);

    // Magnitude and angle calculation
    auto ver_it = view(vertical).begin();
    auto hor_it = view(horizontal).begin();
    auto slope_it = slope.begin();

    for (auto mag_it = magnitude.begin(); mag_it != magnitude.end();
            ++mag_it, ++ver_it, ++hor_it, ++slope_it) {

        // std::min was used to be sure, that pixel max value less than 256
        *mag_it = std::min(UINT8_MAX,
                           (int)std::sqrt(
                               std::pow((int)(*ver_it), 2) + std::pow((int)(*hor_it), 2)));

        *slope_it = (float)std::atan2((int)(*hor_it), (int)(*ver_it));
    }
}

// Non-maximum suppression method is perfomed to thin out edges of the image
// Method goes throug all the points on the gradient intensity matrix and
// finds the pixels with maximum value in the edge direction
void non_maximal_suppression(const gray8c_view_t &magnitude, const gray32fc_view_t &angle,
                             const gray8_view_t &dst)
{
    // Dst matrix fills with 0
    fill_pixels(dst, int8_t(0));

    auto mag_loc = magnitude.xy_at(1, 1);
    int q, r;
    int index;
    int curVal;
    for (int y = 1; y < magnitude.height() - 1; ++y) {
        auto dst_it = dst.row_begin(y);
        auto angle_it = angle.row_begin(y);

        for (int x = 1; x < dst.width() - 1; ++x, ++angle_it, ++dst_it, ++mag_loc.x()) {
            // Index helps to find direction of the edge
            // Pixel has 8 neighbors and 4 possible directions
            // (-1,-1) (0,-1) (1,-1)
            // (-1, 0) (0, 0) (1, 0)
            // (-1, 1) (0, 1) (1, 1)
            // On each direction 2 possible neighbors
            // If both of them is smaller than current value (0, 0),
            // then current value is saved in output image
            index = (int)((*angle_it)[0] * 8 / M_PI);
            index = (index < 0) ? index + 8 : index;

            curVal = mag_loc(0, 0);

            switch (index) {
            // Horizontal direction
            case 0:
            case 7:
            case 8:
                q = mag_loc(1, 0);
                r = mag_loc(-1, 0);
                break;
            // 45 degree
            case 1:
            case 2:
                q = mag_loc(1, 1);
                r = mag_loc(-1, -1);
                break;
            // Vertical direction
            case 3:
            case 4:
                q = mag_loc(0, 1);
                r = mag_loc(0, -1);
                break;
            // 135 degree
            case 5:
            case 6:
                q = mag_loc(1, -1);
                r = mag_loc(-1, 1);
                break;
            }

            if ((curVal >= q) && (curVal >= r)) {
                *dst_it = curVal;
            }
        }
        mag_loc += point2<std::ptrdiff_t>(-dst.width() + 2, 1);
    }
}

void hysteresis_threshold(const gray8_view_t &dst, int minVal, int maxVal)
{
    // Histogram calculation of 3 groups: strong pixel, weak pixel, definitely not-edge pixel
    for_each_pixel(dst, [minVal, maxVal](gray8_pixel_t &pixel) {
        pixel = (pixel < minVal) ? BOTTOM_VALUE :
                ((pixel > maxVal) ? TOP_VALUE : MIDDLE_VALUE);
    });

    // Weak pixels check
    // If weak pixel has a strong neighbor, then it is strong one
    // otherwise it is definitely not-edge pixel
    auto dst_loc = dst.xy_at(1, 1);
    for (int i = 1; i < dst.height() - 1; ++i) {
        for (int j = 1; j < dst.width() - 1; ++j, ++dst_loc.x()) {
            if (dst_loc(0, 0) == MIDDLE_VALUE) {
                if (dst_loc(-1, -1) == TOP_VALUE || dst_loc(-1, 0) == TOP_VALUE ||
                        dst_loc(-1, 1) == TOP_VALUE || dst_loc(1, -1) == TOP_VALUE ||
                        dst_loc(1, 0) == TOP_VALUE || dst_loc(1, 1) == TOP_VALUE ||
                        dst_loc(0, -1) == TOP_VALUE || dst_loc(0, 1) == TOP_VALUE)
                    dst_loc(0, 0) = TOP_VALUE;
                else
                    dst_loc(0, 0) = BOTTOM_VALUE;
            }
        }
        dst_loc += point2<std::ptrdiff_t>(-dst.width() + 2, 1);
    }
}

void canny_edge_detection(const rgb8c_view_t &src,
                          const gray8_view_t &dst, int minVal, int maxVal)
{
    // Canny edge detection algorithm works on grayscale images only
    gray8_image_t gray_img(src.dimensions());
    copy_pixels(color_converted_view<gray8_pixel_t>(src), view(gray_img));

    gaussian_blur(view(gray_img));

    gray32f_image_t angle(src.dimensions());
    gray8_image_t magnitude(src.dimensions());
    sobel_filtering(view(gray_img), view(magnitude), view(angle));

    non_maximal_suppression(view(magnitude), view(angle), dst);

    hysteresis_threshold(dst, minVal, maxVal);
}

int main(int argc, char *argv[])
{
    char *input = "test.jpg";
    char *output = "canny.jpg";
    int min_threshold_value = 30;
    int max_threshold_value = 70;

    if (argc >= 2 && (argv[1][0] == '-') && (argv[1][1] == 'H' || argv[1][1] == 'h')) {
        printf("canny [path_to_input [path_to_output "
               "[min_threshold_value [max_threshold_value]]]]\n");
        return 0;
    }
    if (argc >= 2) input = argv[1];
    if (argc >= 3) output = argv[2];
    if (argc >= 4) min_threshold_value = atoi(argv[3]);
    if (argc >= 5) max_threshold_value = atoi(argv[4]);

    if (min_threshold_value > max_threshold_value)
        std::swap(min_threshold_value, max_threshold_value);

    rgb8_image_t img;
    read_image(input, img, jpeg_tag{});

    gray8_image_t res_img(img.dimensions());
    canny_edge_detection(const_view(img), view(res_img),
                         min_threshold_value, max_threshold_value);
    write_view(output, view(res_img), jpeg_tag{});
    return 0;
}


/**
 * @file
 * @brief Solution of source-free heat equation and computation of H1
 *  	  seminorms on different triangular meshes and refinement levels
 * @author Julien Gacon, Amélie Loher
 * @date   March 2019
 */

#include "unstablebvp.h"
// General includes
#include <array>
#include <fstream>
#include <memory>
#include <string>
// Eigen
#include <Eigen/Core>
#include <Eigen/SparseLU>
// Lehrfempp
#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/uscalfe/uscalfe.h>

namespace UnstableBVP {

std::shared_ptr<lf::refinement::MeshHierarchy> createMeshHierarchy(
    const int reflevels, const std::string &mesh_type) {
  // Helper object: mesh factory
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Decide where the triangular domain should be located in x_2 direction
  // by adding an offset to the x_2 coordinate of the nodes
  double offset = 0;
  if (mesh_type == "top") {
    offset = 1.5;
  } else if (mesh_type == "bottom") {
    offset = -1.5;
  } else {
    // already at 0
  }

  // Define the nodes
  std::array<std::array<double, 2>, 3> node_coord{
      std::array<double, 2>({0.5, -0.5 + offset}),
      std::array<double, 2>({0, 0.5 + offset}),
      std::array<double, 2>({1, 0.5 + offset})};

  for (const auto &node : node_coord) {
    mesh_factory_ptr->AddPoint(Eigen::Vector2d({node[0], node[1]}));
  }

  // Initialize triangle
  mesh_factory_ptr->AddEntity(lf::base::RefEl::kTria(),
                              std::vector<lf::base::size_type>({0, 1, 2}),
                              std::unique_ptr<lf::geometry::Geometry>(nullptr));

  // Get a pointer to the mesh
  std::shared_ptr<lf::mesh::Mesh> mesh_p = mesh_factory_ptr->Build();

  // (optional) Print information about the mesh
  // std::cout << "   Mesh info\n" << *mesh_p;

  // Ask LehrFEM++ to create a hierarchy of nested meshes
  std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh_p,
                                                              reflevels);

  return multi_mesh_p;
}

double solveTemperatureDistribution(
    std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  // **********************************************************************
  // Stage 0: provide all coefficient functions mainly through lambda
  //          functions and derived MeshFunctions
  // **********************************************************************

  // The boundary condition
  auto bc = [](Eigen::Vector2d x) -> double {
    return x[1] <= 0 ? 1 - x[1] : 0;
  };
  // Wrap into a MeshFunction
  lf::mesh::utils::MeshFunctionGlobal mf_bc{bc};

  // We use lowest-order (p.w. linear Lagrangian finite elements), for which
  // LehrFEM++ provides a built-in description according to the paradigm of
  // parametric finite elements.
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  // Reference to current mesh
  const lf::mesh::Mesh &mesh{*(fe_space->Mesh())};
  // Obtain local->global index mapping for current finite element space
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};

  // **********************************************************************
  // Stage 1: Assemble finite element Galerkin matrix
  // **********************************************************************

  // Dimension of finite element space`
  const lf::base::size_type N_dofs(dofh.NumDofs());
  // Matrix in triplet format holding Galerkin matrix, zero initially.
  lf::assemble::COOMatrix<double> A(N_dofs, N_dofs);

  // Element matrix builder for the negative Laplacian
  lf::uscalfe::LinearFELaplaceElementMatrix elmat_builder{};

  // Invoke assembly on cells (co-dimension = 0 as first argument)
  // Information about the mesh and the local-to-global map is passed through
  // a Dofhandler object, argument 'dofh'. This function call adds triplets to
  // the internal COO-format representation of the sparse matrix A.
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elmat_builder, A);

  // **********************************************************************
  // Stage 2: Right-hand side vector
  // **********************************************************************

  // Define RHS vector
  // No source, hence it is simply zero
  Eigen::Matrix<double, Eigen::Dynamic, 1> phi(N_dofs);
  phi.setZero();

  // **********************************************************************
  // Stage 3: Fixing solution components according to essential (Dirichlet)
  //          boundary conditions
  // **********************************************************************

  // Obtain specification for shape functions on edges
  std::shared_ptr<const lf::uscalfe::ScalarReferenceFiniteElement<double>>
      rsf_edge_p = fe_space->ShapeFunctionLayout(lf::base::RefEl::kSegment());
  LF_ASSERT_MSG(rsf_edge_p != nullptr, "FE specification for edges missing");

  // Obtain an array of boolean flags for the edges (codim 1) of the mesh,
  // `true` indicates that the edge lies on the boundary
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(fe_space->Mesh(), 1)};

  // Fetch flags and values for degrees of freedom located on Dirichlet
  // edges.
  auto ess_bdc_flags_values{lf::uscalfe::InitEssentialConditionFromFunction(
      dofh, *rsf_edge_p,
      [&bd_flags](const lf::mesh::Entity &edge) -> bool {
        return (bd_flags(edge));
      },
      mf_bc)};

  // Eliminate Dirichlet dofs from linear system
  lf::assemble::FixFlaggedSolutionComponents<double>(
      [&ess_bdc_flags_values](lf::assemble::glb_idx_t gdof_idx) {
        return ess_bdc_flags_values[gdof_idx];
      },
      A, phi);

  // **********************************************************************
  // Stage 4: Solve LSE
  // **********************************************************************

  // Assembly completed: Convert COO matrix A into CRS format using Eigen's
  // internal conversion routines.
  Eigen::SparseMatrix<double> A_crs = A.makeSparse();

  // Solve linear system using Eigen's sparse direct elimination
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(A_crs);
  LF_VERIFY_MSG(solver.info() == Eigen::Success, "LU decomposition failed");
  Eigen::VectorXd sol_vec = solver.solve(phi);
  LF_VERIFY_MSG(solver.info() == Eigen::Success, "Solving LSE failed");

  // **********************************************************************
  // Stage 5: Compute H1 seminorm
  // **********************************************************************

  // Compute the difference to a function that's zero everywhere, hence
  // just the gradient of the solution (which is encapsulated in the fe_space).
  // We use this trick to avoid the manual computation and make use of the
  // LehrFEM facilities :)
  lf::uscalfe::MeshFunctionL2GradientDifference loc_comp(
      fe_space,
      lf::mesh::utils::MeshFunctionConstant(Eigen::Vector2d(0.0, 0.0)), 2);

  // Compute the norm of the ``difference'' (i.e. the norm of the gradient of
  // the solution)
  const double norm = lf::uscalfe::NormOfDifference(dofh, loc_comp, sol_vec);

  return norm;
}

}  // namespace UnstableBVP


#include "sv/util/eigen.h"

#include <Eigen/Cholesky>

#include "sv/util/logging.h"

namespace sv {

void StableRotateBlockTopLeft(MatrixXdRef H,
                              VectorXdRef b,
                              int block_ind,
                              int block_size) {
  CHECK_EQ(H.rows(), b.size());
  CHECK_EQ(H.cols(), b.size());
  CHECK_GE(block_ind, 0);
  CHECK_GT(block_size, 0);
  CHECK_LT(block_ind * block_size, H.rows());

  if (block_ind == 0) return;
  const auto n = block_size;

  // Permute block of rows up gradually, also need to permute b
  for (int i = block_ind; i > 0; --i) {
    const auto r = i * block_size;
    // swap current rows with above rows
    H.middleRows(r, n).swap(H.middleRows(r - n, n));
    b.segment(r, n).swap(b.segment(r - n, n));
  }

  // Permute block of cols
  for (int j = block_ind; j > 0; --j) {
    const auto c = j * block_size;
    // swap current cols with left cols
    H.middleCols(c, n).swap(H.middleCols(c - n, n));
  }
}

void FillLowerTriangular(MatrixXdRef M) {
  CHECK_EQ(M.rows(), M.cols());
  M.triangularView<Eigen::Lower>() =
      M.triangularView<Eigen::Upper>().transpose();
}

void FillUpperTriangular(MatrixXdRef M) {
  CHECK_EQ(M.rows(), M.cols());
  M.triangularView<Eigen::Upper>() =
      M.triangularView<Eigen::Lower>().transpose();
}

void MakeSymmetric(MatrixXdRef M) {
  CHECK_EQ(M.rows(), M.cols());
  M += M.transpose().eval();
  M.array() /= 2.0;
}

void MargTopLeftBlock(const MatrixXdCRef& Hf,
                      const VectorXdCRef& bf,
                      MatrixXdRef Hm,
                      VectorXdRef bm,
                      int dim) {
  // Pre-condition
  // 1. Hf is square and match bsc and symmetric
  // 2. Hm is quare and match bpr
  const auto nf = bf.size();
  const auto nm = bm.size();
  CHECK_GT(dim, 0);
  CHECK_EQ(nm + dim, nf);
  CHECK_EQ(Hf.rows(), nf);
  CHECK_EQ(Hf.cols(), nf);
  CHECK_EQ(Hm.rows(), nm);
  CHECK_EQ(Hm.cols(), nm);
  CHECK_EQ(Hf, Hf.transpose()) << "\n" << Hf;

  // Hf                     bf
  // [ H00 H01 ] [ x0 ] = [ b0 ]
  // [ H10 H11 ] [ x1 ] = [ b1 ]
  // Hm = H11 - H10 * H00^-1 * H01
  // bm =  b1 - H10 * H00^-1 * b0
  const auto H01 = Hf.topRightCorner(dim, nm);
  const auto H10 = Hf.bottomLeftCorner(nm, dim);

  // Benchmark shows that simply inverse has similar speed as llt
  // However to account for rank-deficiency we use ldlt to inverse
  const auto H00_inv = Hf.topLeftCorner(dim, dim)
                           .selfadjointView<Eigen::Lower>()
                           .ldlt()
                           .solve(Eigen::MatrixXd::Identity(dim, dim))
                           .eval();

  Hm = Hf.bottomRightCorner(nm, nm);
  Hm.noalias() -= H10 * H00_inv * H01;

  const auto b0 = bf.head(dim);
  bm = bf.tail(nm);  // b1
  bm.noalias() -= H10 * (H00_inv * b0);

  // Make sure Hpr is symmetric
  MakeSymmetric(Hm);

  // Post-condition
  // 1. Hpr shape doesn't change
  // 2. Hpr is symmetric
  CHECK_EQ(Hm.rows(), nm);
  CHECK_EQ(Hm.cols(), nm);
  CHECK_EQ(Hm, Hm.transpose()) << "\n" << Hm;
}

}  // namespace sv


/*
 * Copyright (c) 2013-2015 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef NEWTON_HPP
#define NEWTON_HPP

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <kv/autodif.hpp>
#include <limits>
#include <boost/random.hpp>
#include <ctime>
#include <cmath>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
bool
newton(F f, ub::vector<T>& x, T epsilon = std::numeric_limits<T>::epsilon(), int maxloop = 100)
{
	int s = x.size();
	int i, j, r;
	ub::vector<T> fx;
	ub::matrix<T> fdx;
	T norm1, norm2;

	for (i=0; i<maxloop; i++) {
		try {
			autodif<T>::split(f(autodif<T>::init(x)), fx, fdx);
			ub::permutation_matrix<> pm(s);
			r = ub::lu_factorize(fdx, pm);
			if (r != 0) return false;
			ub::lu_substitute(fdx, pm, fx);
		}
		catch (...) {
			return false;
		}

		norm1 = 1.;
		norm2 = 0.;
		for (j=0; j<s; j++) {
			using std::abs;
			norm1 = std::max(norm1, abs(x(j)));
			norm2 = std::max(norm2, abs(fx(j)));
		}

		x = x - fx;
		if (norm2 <= norm1 * epsilon) return true;
	}
	return false;
}

template <class T, class F>
bool
newton_random(F f, ub::vector<T>& x, T epsilon = std::numeric_limits<T>::epsilon(), int maxloop = 100)
{
	int s = x.size();
	int i;

	using namespace boost;
	// use "static" to be "randomized" only once
	static variate_generator< mt19937, normal_distribution<> > rand (mt19937(time(0)), normal_distribution<>(0., 10.));

	for (i=0; i<s; i++) x(i) = rand();
	return newton(f, x, epsilon, maxloop);
}

} // namespace kv

#endif // NEWTON_HPP


// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  C++ sensor fusion/filtering code that uses flexkalman
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_tracking
 */

#pragma once

#ifndef __cplusplus
#error "This header is C++-only."
#endif

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

#include "flexkalman/AugmentedProcessModel.h"
#include "flexkalman/AugmentedState.h"
#include "flexkalman/BaseTypes.h"
#include "flexkalman/PoseState.h"

namespace xrt::auxiliary::tracking {

namespace types = flexkalman::types;
using flexkalman::types::Vector;

//! For things like accelerometers, which on some level measure the local vector
//! of a world direction.
template <typename State>
class WorldDirectionMeasurement : public flexkalman::MeasurementBase<WorldDirectionMeasurement<State>>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	static constexpr size_t Dimension = 3;
	using MeasurementVector = types::Vector<Dimension>;
	using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
	WorldDirectionMeasurement(types::Vector<3> const &direction,
	                          types::Vector<3> const &reference,
	                          types::Vector<3> const &variance)
	    : direction_(direction.normalized()), reference_(reference.normalized()), covariance_(variance.asDiagonal())
	{}

	MeasurementSquareMatrix const &
	getCovariance(State const & /*s*/)
	{
		return covariance_;
	}

	types::Vector<3>
	predictMeasurement(State const &s) const
	{
		return s.getCombinedQuaternion() * reference_;
	}

	MeasurementVector
	getResidual(MeasurementVector const &predictedMeasurement, State const &s) const
	{
		return predictedMeasurement - reference_;
	}

	MeasurementVector
	getResidual(State const &s) const
	{
		return getResidual(predictMeasurement(s), s);
	}

private:
	types::Vector<3> direction_;
	types::Vector<3> reference_;
	MeasurementSquareMatrix covariance_;
};
#if 0
//! For things like accelerometers, which on some level measure the local vector
//! of a world direction.
class LinAccelWithGravityMeasurement
    : public flexkalman::MeasurementBase<LinAccelWithGravityMeasurement>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	static constexpr size_t Dimension = 3;
	using MeasurementVector = types::Vector<Dimension>;
	using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
	LinAccelWithGravityMeasurement(types::Vector<3> const &direction,
	                          types::Vector<3> const &reference,
	                          types::Vector<3> const &variance)
	    : direction_(direction), reference_(reference),
	      covariance_(variance.asDiagonal())
	{}

	// template <typename State>
	MeasurementSquareMatrix const &
	getCovariance(State const & /*s*/)
	{
		return covariance_;
	}

	// template <typename State>
	types::Vector<3>
	predictMeasurement(State const &s) const
	{
		return reference_;
	}

	// template <typename State>
	MeasurementVector
	getResidual(MeasurementVector const &predictedMeasurement,
	            State const &s) const
	{
		s.getQuaternion().conjugate() *
		        predictedMeasurement return predictedMeasurement -
		    reference_.normalized();
	}

	template <typename State>
	MeasurementVector
	getResidual(State const &s) const
	{
		MeasurementVector residual =
		    direction_ - reference_ * s.getQuaternion();
		return getResidual(predictMeasurement(s), s);
	}

private:
	types::Vector<3> direction_;
	types::Vector<3> reference_;
	MeasurementSquareMatrix covariance_;
};
#endif

class BiasedGyroMeasurement : public flexkalman::MeasurementBase<BiasedGyroMeasurement>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	static constexpr size_t Dimension = 3;
	using MeasurementVector = types::Vector<Dimension>;
	using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
	BiasedGyroMeasurement(types::Vector<3> const &angVel, types::Vector<3> const &variance)
	    : angVel_(angVel), covariance_(variance.asDiagonal())
	{}

	template <typename State>
	MeasurementSquareMatrix const &
	getCovariance(State const & /*s*/)
	{
		return covariance_;
	}

	template <typename State>
	types::Vector<3>
	predictMeasurement(State const &s) const
	{
		return s.b().stateVector() + angVel_;
	}

	template <typename State>
	MeasurementVector
	getResidual(MeasurementVector const &predictedMeasurement, State const &s) const
	{
		return predictedMeasurement - s.a().angularVelocity();
	}

	template <typename State>
	MeasurementVector
	getResidual(State const &s) const
	{
		return getResidual(predictMeasurement(s), s);
	}

private:
	types::Vector<3> angVel_;
	MeasurementSquareMatrix covariance_;
};
/*!
 * For PS Move-like things, where there's a directly-computed absolute position
 * that is not at the tracked body's origin.
 */
class AbsolutePositionLeverArmMeasurement : public flexkalman::MeasurementBase<AbsolutePositionLeverArmMeasurement>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	using State = flexkalman::pose_externalized_rotation::State;
	static constexpr size_t Dimension = 3;
	using MeasurementVector = types::Vector<Dimension>;
	using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;

	/*!
	 * @todo the point we get from the camera isn't the center of the ball,
	 * but the center of the visible surface of the ball - a closer
	 * approximation would be translation along the vector to the center of
	 * projection....
	 */
	AbsolutePositionLeverArmMeasurement(MeasurementVector const &measurement,
	                                    MeasurementVector const &knownLocationInBodySpace,
	                                    MeasurementVector const &variance)
	    : measurement_(measurement), knownLocationInBodySpace_(knownLocationInBodySpace),
	      covariance_(variance.asDiagonal())
	{}

	MeasurementSquareMatrix const &
	getCovariance(State const & /*s*/)
	{
		return covariance_;
	}

	types::Vector<3>
	predictMeasurement(State const &s) const
	{
		return s.getIsometry() * knownLocationInBodySpace_;
	}

	MeasurementVector
	getResidual(MeasurementVector const &predictedMeasurement, State const & /*s*/) const
	{
		return measurement_ - predictedMeasurement;
	}

	MeasurementVector
	getResidual(State const &s) const
	{
		return getResidual(predictMeasurement(s), s);
	}

private:
	MeasurementVector measurement_;
	MeasurementVector knownLocationInBodySpace_;
	MeasurementSquareMatrix covariance_;
};

} // namespace xrt::auxiliary::tracking


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2006, 2007, 2015 Ferdinando Ametrano
 Copyright (C) 2006 Cristina Duminuco
 Copyright (C) 2007 Giorgio Facchinetti
 Copyright (C) 2015 Paolo Mazzocchi

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

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

#ifndef quantlib_abcdcalibration_hpp
#define quantlib_abcdcalibration_hpp

#include <ql/math/optimization/endcriteria.hpp>
#include <ql/math/optimization/projectedcostfunction.hpp>
#include <ql/math/array.hpp>

#include <boost/shared_ptr.hpp>

#include <vector>

namespace QuantLib {

    class Quote;
    class OptimizationMethod;
    class ParametersTransformation;

    class AbcdCalibration {
      private:
        class AbcdError : public CostFunction {
          public:
            AbcdError(AbcdCalibration* abcd) : abcd_(abcd) {}

            Real value(const Array& x) const {
                const Array y = abcd_->transformation_->direct(x);
                abcd_->a_ = y[0];
                abcd_->b_ = y[1];
                abcd_->c_ = y[2];
                abcd_->d_ = y[3];
                return abcd_->error();
            }
            Disposable<Array> values(const Array& x) const {
                const Array y = abcd_->transformation_->direct(x);
                abcd_->a_ = y[0];
                abcd_->b_ = y[1];
                abcd_->c_ = y[2];
                abcd_->d_ = y[3];
                return abcd_->errors();
            }
          private:
            AbcdCalibration* abcd_;
        };

        class AbcdParametersTransformation : public ParametersTransformation {
          public:
            AbcdParametersTransformation() : y_(Array(4)) {}
            // to constrained <- from unconstrained
            Array direct(const Array& x) const;
            // to unconstrained <- from constrained
            Array inverse(const Array& x) const;
          private:
            mutable Array y_;
        };

      public:
        AbcdCalibration() {};
        AbcdCalibration(
             const std::vector<Real>& t,
             const std::vector<Real>& blackVols,
             Real aGuess = -0.06,
             Real bGuess =  0.17,
             Real cGuess =  0.54,
             Real dGuess =  0.17,
             bool aIsFixed = false,
             bool bIsFixed = false,
             bool cIsFixed = false,
             bool dIsFixed = false,
             bool vegaWeighted = false,
             const boost::shared_ptr<EndCriteria>& endCriteria
                      = boost::shared_ptr<EndCriteria>(),
             const boost::shared_ptr<OptimizationMethod>& method
                      = boost::shared_ptr<OptimizationMethod>());
        //! adjustment factors needed to match Black vols
        std::vector<Real> k(const std::vector<Real>& t,
                            const std::vector<Real>& blackVols) const;
        void compute();
        //calibration results
        Real value(Real x) const;
        Real error() const;
        Real maxError() const;
        Disposable<Array> errors() const;
        EndCriteria::Type endCriteria() const;
        Real a() const { return a_; }
        Real b() const { return b_; }
        Real c() const { return c_; }
        Real d() const { return d_; }
        bool aIsFixed_, bIsFixed_, cIsFixed_, dIsFixed_;
        Real a_, b_, c_, d_;
        boost::shared_ptr<ParametersTransformation> transformation_;
      private:
        // optimization method used for fitting
        mutable EndCriteria::Type abcdEndCriteria_;
        boost::shared_ptr<EndCriteria> endCriteria_;
        boost::shared_ptr<OptimizationMethod> optMethod_;
        mutable std::vector<Real> weights_;
        bool vegaWeighted_;
        //! Parameters
        std::vector<Real> times_, blackVols_;
    };

}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2006, 2015 Ferdinando Ametrano
 Copyright (C) 2006 Cristina Duminuco
 Copyright (C) 2005, 2006 Klaus Spanderen
 Copyright (C) 2007 Giorgio Facchinetti
 Copyright (C) 2015 Paolo Mazzocchi

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

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

#include <ql/math/optimization/method.hpp>
#include <ql/math/optimization/constraint.hpp>
#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/termstructures/volatility/abcd.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/interpolations/abcdinterpolation.hpp>

namespace QuantLib {

    // to constrained <- from unconstrained
    inline Array AbcdCalibration::AbcdParametersTransformation::direct(const Array& x) const {
        y_[1] = x[1];
        y_[2] = std::exp(x[2]);
        y_[3] = std::exp(x[3]);
        y_[0] = std::exp(x[0]) - y_[3];
        return y_;
    }

    // to unconstrained <- from constrained
    inline Array AbcdCalibration::AbcdParametersTransformation::inverse(const Array& x) const {
        y_[1] = x[1];
        y_[2] = std::log(x[2]);
        y_[3] = std::log(x[3]);
        y_[0] = std::log(x[0] + x[3]);
        return y_;
    }

    // to constrained <- from unconstrained

    inline AbcdCalibration::AbcdCalibration(
               const std::vector<Real>& t,
               const std::vector<Real>& blackVols,
               Real a, Real b, Real c, Real d,
               bool aIsFixed, bool bIsFixed, bool cIsFixed, bool dIsFixed,
               bool vegaWeighted,
               const boost::shared_ptr<EndCriteria>& endCriteria,
               const boost::shared_ptr<OptimizationMethod>& optMethod)
    : aIsFixed_(aIsFixed), bIsFixed_(bIsFixed),
      cIsFixed_(cIsFixed), dIsFixed_(dIsFixed),
      a_(a), b_(b), c_(c), d_(d),
      abcdEndCriteria_(EndCriteria::None), endCriteria_(endCriteria),
      optMethod_(optMethod), weights_(blackVols.size(), 1.0/blackVols.size()),
      vegaWeighted_(vegaWeighted),
      times_(t), blackVols_(blackVols) {

        AbcdMathFunction::validate(a, b, c, d);

        QL_REQUIRE(blackVols.size()==t.size(),
                       "mismatch between number of times (" << t.size() <<
                       ") and blackVols (" << blackVols.size() << ")");

        // if no optimization method or endCriteria is provided, we provide one
        if (!optMethod_) {
            Real epsfcn = 1.0e-8;
            Real xtol = 1.0e-8;
            Real gtol = 1.0e-8;
            bool useCostFunctionsJacobian = false;
            optMethod_ = boost::shared_ptr<OptimizationMethod>(new
                LevenbergMarquardt(epsfcn, xtol, gtol, useCostFunctionsJacobian));
        }
        if (!endCriteria_) {
            Size maxIterations = 10000;
            Size maxStationaryStateIterations = 1000;
            Real rootEpsilon = 1.0e-8;
            Real functionEpsilon = 0.3e-4;     // Why 0.3e-4 ?
            Real gradientNormEpsilon = 0.3e-4; // Why 0.3e-4 ?
            endCriteria_ = boost::shared_ptr<EndCriteria>(new
                EndCriteria(maxIterations, maxStationaryStateIterations,
                            rootEpsilon, functionEpsilon, gradientNormEpsilon));
        }
    }

    inline void AbcdCalibration::compute() {
        if (vegaWeighted_) {
            Real weightsSum = 0.0;
            for (Size i=0; i<times_.size() ; i++) {
                Real stdDev = std::sqrt(blackVols_[i]* blackVols_[i]* times_[i]);
                // when strike==forward, the blackFormulaStdDevDerivative becomes
                weights_[i] = CumulativeNormalDistribution().derivative(.5*stdDev);
                weightsSum += weights_[i];
            }
            // weight normalization
            for (Size i=0; i<times_.size() ; i++) {
                weights_[i] /= weightsSum;
            }
        }

        // there is nothing to optimize
        if (aIsFixed_ && bIsFixed_ && cIsFixed_ && dIsFixed_) {
            abcdEndCriteria_ = EndCriteria::None;
            //error_ = interpolationError();
            //maxError_ = interpolationMaxError();
            return;
        } else {

            AbcdError costFunction(this);
            transformation_ = boost::shared_ptr<ParametersTransformation>(new
                AbcdParametersTransformation);

            Array guess(4);
            guess[0] = a_;
            guess[1] = b_;
            guess[2] = c_;
            guess[3] = d_;

            std::vector<bool> parameterAreFixed(4);
            parameterAreFixed[0] = aIsFixed_;
            parameterAreFixed[1] = bIsFixed_;
            parameterAreFixed[2] = cIsFixed_;
            parameterAreFixed[3] = dIsFixed_;

            Array inversedTransformatedGuess(transformation_->inverse(guess));

            ProjectedCostFunction projectedAbcdCostFunction(costFunction,
                            inversedTransformatedGuess, parameterAreFixed);

            Array projectedGuess
                (projectedAbcdCostFunction.project(inversedTransformatedGuess));

            NoConstraint constraint;
            Problem problem(projectedAbcdCostFunction, constraint, projectedGuess);
            abcdEndCriteria_ = optMethod_->minimize(problem, *endCriteria_);
            Array projectedResult(problem.currentValue());
            Array transfResult(projectedAbcdCostFunction.include(projectedResult));

            Array result = transformation_->direct(transfResult);
            AbcdMathFunction::validate(a_, b_, c_, d_);
            a_ = result[0];
            b_ = result[1];
            c_ = result[2];
            d_ = result[3];

        }
    }

    inline Real AbcdCalibration::value(Real x) const {
        return abcdBlackVolatility(x,a_,b_,c_,d_);
    }

    inline std::vector<Real> AbcdCalibration::k(const std::vector<Real>& t,
                                         const std::vector<Real>& blackVols) const {
        QL_REQUIRE(blackVols.size()==t.size(),
               "mismatch between number of times (" << t.size() <<
               ") and blackVols (" << blackVols.size() << ")");
        std::vector<Real> k(t.size());
        for (Size i=0; i<t.size() ; i++) {
            k[i]=blackVols[i]/value(t[i]);
        }
        return k;
    }

    inline Real AbcdCalibration::error() const {
        Size n = times_.size();
        Real error, squaredError = 0.0;
        for (Size i=0; i<times_.size() ; i++) {
            error = (value(times_[i]) - blackVols_[i]);
            squaredError += error * error * weights_[i];
        }
        return std::sqrt(n*squaredError/(n-1));
    }

    inline Real AbcdCalibration::maxError() const {
        Real error, maxError = QL_MIN_REAL;
        for (Size i=0; i<times_.size() ; i++) {
            error = std::fabs(value(times_[i]) - blackVols_[i]);
            maxError = std::max(maxError, error);
        }
        return maxError;
    }

    // calculate weighted differences
    inline Disposable<Array> AbcdCalibration::errors() const {
        Array results(times_.size());
        for (Size i=0; i<times_.size() ; i++) {
            results[i] = (value(times_[i]) - blackVols_[i])* std::sqrt(weights_[i]);
        }
        return results;
    }

    inline EndCriteria::Type AbcdCalibration::endCriteria() const{
        return abcdEndCriteria_;
    }

}

#endif


//  (C) Copyright John Maddock 2005.
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_COMPLEX_ASIN_INCLUDED
#define BOOST_MATH_COMPLEX_ASIN_INCLUDED

#ifndef BOOST_MATH_COMPLEX_DETAILS_INCLUDED
#  include <boost/math/complex/details.hpp>
#endif
#ifndef BOOST_MATH_LOG1P_INCLUDED
#  include <boost/math/special_functions/log1p.hpp>
#endif
#include <boost/assert.hpp>

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::sqrt; using ::fabs; using ::acos; using ::asin; using ::atan; using ::atan2; }
#endif

namespace boost{ namespace math{

template<class T>
inline std::complex<T> asin(const std::complex<T>& z)
{
   //
   // This implementation is a transcription of the pseudo-code in:
   //
   // "Implementing the complex Arcsine and Arccosine Functions using Exception Handling."
   // T E Hull, Thomas F Fairgrieve and Ping Tak Peter Tang.
   // ACM Transactions on Mathematical Software, Vol 23, No 3, Sept 1997.
   //

   //
   // These static constants should really be in a maths constants library:
   //
   static const T one = static_cast<T>(1);
   //static const T two = static_cast<T>(2);
   static const T half = static_cast<T>(0.5L);
   static const T a_crossover = static_cast<T>(1.5L);
   static const T b_crossover = static_cast<T>(0.6417L);
   static const T s_pi = boost::math::constants::pi<T>();
   static const T half_pi = s_pi / 2;
   static const T log_two = boost::math::constants::ln_two<T>();
   static const T quarter_pi = s_pi / 4;
#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif
   //
   // Get real and imaginary parts, discard the signs as we can
   // figure out the sign of the result later:
   //
   T x = std::fabs(z.real());
   T y = std::fabs(z.imag());
   T real, imag;  // our results

   //
   // Begin by handling the special cases for infinities and nan's
   // specified in C99, most of this is handled by the regular logic
   // below, but handling it as a special case prevents overflow/underflow
   // arithmetic which may trip up some machines:
   //
   if((boost::math::isnan)(x))
   {
      if((boost::math::isnan)(y))
         return std::complex<T>(x, x);
      if((boost::math::isinf)(y))
      {
         real = x;
         imag = std::numeric_limits<T>::infinity();
      }
      else
         return std::complex<T>(x, x);
   }
   else if((boost::math::isnan)(y))
   {
      if(x == 0)
      {
         real = 0;
         imag = y;
      }
      else if((boost::math::isinf)(x))
      {
         real = y;
         imag = std::numeric_limits<T>::infinity();
      }
      else
         return std::complex<T>(y, y);
   }
   else if((boost::math::isinf)(x))
   {
      if((boost::math::isinf)(y))
      {
         real = quarter_pi;
         imag = std::numeric_limits<T>::infinity();
      }
      else
      {
         real = half_pi;
         imag = std::numeric_limits<T>::infinity();
      }
   }
   else if((boost::math::isinf)(y))
   {
      real = 0;
      imag = std::numeric_limits<T>::infinity();
   }
   else
   {
      //
      // special case for real numbers:
      //
      if((y == 0) && (x <= one))
         return std::complex<T>(std::asin(z.real()), z.imag());
      //
      // Figure out if our input is within the "safe area" identified by Hull et al.
      // This would be more efficient with portable floating point exception handling;
      // fortunately the quantities M and u identified by Hull et al (figure 3),
      // match with the max and min methods of numeric_limits<T>.
      //
      T safe_max = detail::safe_max(static_cast<T>(8));
      T safe_min = detail::safe_min(static_cast<T>(4));

      T xp1 = one + x;
      T xm1 = x - one;

      if((x < safe_max) && (x > safe_min) && (y < safe_max) && (y > safe_min))
      {
         T yy = y * y;
         T r = std::sqrt(xp1*xp1 + yy);
         T s = std::sqrt(xm1*xm1 + yy);
         T a = half * (r + s);
         T b = x / a;

         if(b <= b_crossover)
         {
            real = std::asin(b);
         }
         else
         {
            T apx = a + x;
            if(x <= one)
            {
               real = std::atan(x/std::sqrt(half * apx * (yy /(r + xp1) + (s-xm1))));
            }
            else
            {
               real = std::atan(x/(y * std::sqrt(half * (apx/(r + xp1) + apx/(s+xm1)))));
            }
         }

         if(a <= a_crossover)
         {
            T am1;
            if(x < one)
            {
               am1 = half * (yy/(r + xp1) + yy/(s - xm1));
            }
            else
            {
               am1 = half * (yy/(r + xp1) + (s + xm1));
            }
            imag = boost::math::log1p(am1 + std::sqrt(am1 * (a + one)));
         }
         else
         {
            imag = std::log(a + std::sqrt(a*a - one));
         }
      }
      else
      {
         //
         // This is the Hull et al exception handling code from Fig 3 of their paper:
         //
         if(y <= (std::numeric_limits<T>::epsilon() * std::fabs(xm1)))
         {
            if(x < one)
            {
               real = std::asin(x);
               imag = y / std::sqrt(-xp1*xm1);
            }
            else
            {
               real = half_pi;
               if(((std::numeric_limits<T>::max)() / xp1) > xm1)
               {
                  // xp1 * xm1 won't overflow:
                  imag = boost::math::log1p(xm1 + std::sqrt(xp1*xm1));
               }
               else
               {
                  imag = log_two + std::log(x);
               }
            }
         }
         else if(y <= safe_min)
         {
            // There is an assumption in Hull et al's analysis that
            // if we get here then x == 1.  This is true for all "good"
            // machines where :
            //
            // E^2 > 8*sqrt(u); with:
            //
            // E =  std::numeric_limits<T>::epsilon()
            // u = (std::numeric_limits<T>::min)()
            //
            // Hull et al provide alternative code for "bad" machines
            // but we have no way to test that here, so for now just assert
            // on the assumption:
            //
            BOOST_ASSERT(x == 1);
            real = half_pi - std::sqrt(y);
            imag = std::sqrt(y);
         }
         else if(std::numeric_limits<T>::epsilon() * y - one >= x)
         {
            real = x/y; // This can underflow!
            imag = log_two + std::log(y);
         }
         else if(x > one)
         {
            real = std::atan(x/y);
            T xoy = x/y;
            imag = log_two + std::log(y) + half * boost::math::log1p(xoy*xoy);
         }
         else
         {
            T a = std::sqrt(one + y*y);
            real = x/a; // This can underflow!
            imag = half * boost::math::log1p(static_cast<T>(2)*y*(y+a));
         }
      }
   }

   //
   // Finish off by working out the sign of the result:
   //
   if((boost::math::signbit)(z.real()))
      real = (boost::math::changesign)(real);
   if((boost::math::signbit)(z.imag()))
      imag = (boost::math::changesign)(imag);

   return std::complex<T>(real, imag);
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif
}

} } // namespaces

#endif // BOOST_MATH_COMPLEX_ASIN_INCLUDED


#include <opencv2/opencv.hpp>
#include <sophus/se3.h>
#include <Eigen/Core>
#include <vector>
#include <string>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>

using namespace std;
using namespace Eigen;
using namespace cv;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
// 内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// 基线
double baseline = 0.573;
// paths
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// TODO implement this function
/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3 &T21
);

// TODO implement this function
/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3 &T21
);

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
            (1 - xx) * (1 - yy) * data[0] +
            xx * (1 - yy) * data[1] +
            (1 - xx) * yy * data[img.step] +
            xx * yy * data[img.step + 1]
    );
}

inline bool OutOfImg(float u, float v, int col, int row){
	if(u >= 0 && u < col && v >= 0 && v < row)
		return false;
	else{
		return true;
	}
}

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

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 1000;
    int boarder = 40;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3 T_cur_ref;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
       // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);    // first you need to test single layer
        DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    }
}

void DirectPoseEstimationSingleLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3 &T21
) {

    // parameters
    int half_patch_size = 4;
    int iterations = 100;

    double cost = 0, lastCost = 0;
    int nGood = 0;  // good projections
    VecVector2d goodProjection;

    for (int iter = 0; iter < iterations; iter++) {
        nGood = 0;
        goodProjection.clear();

        // Define Hessian and bias
        Matrix6d H = Matrix6d::Zero();  // 6x6 Hessian
        Vector6d b = Vector6d::Zero();  // 6x1 bias

        for (size_t i = 0; i < px_ref.size(); i++) {

            // compute the projection in the second image
            // TODO START YOUR CODE HERE
            float u = px_ref[i](0), v = px_ref[i](1);
			double zCam1 = depth_ref[i];
			double xCam1 = double(zCam1 / fx * (u - cx));
			double yCam1 = double(zCam1 / fy * (v - cy));
			Vector3d pCam1(xCam1, yCam1, zCam1);
			Vector3d pCam2 = T21 * pCam1;
			double xCam2 = pCam2[0], yCam2 = pCam2[1], zCam2 = pCam2[2];
			float u2 = float(fx * xCam2 / zCam2 + cx);
			float v2 = float(fy * yCam2 / zCam2 + cy);

			if(OutOfImg(u-half_patch_size, v-half_patch_size, img1.cols, img1.rows)
			  || OutOfImg(u+half_patch_size-1, v+half_patch_size-1, img1.cols, img1.rows)
			  || OutOfImg(u2-half_patch_size, v2-half_patch_size, img2.cols, img2.rows)
			  || OutOfImg(u2+half_patch_size-1, v2+half_patch_size-1, img2.cols, img2.rows)
			  ){
			//	cout << "[DM] pixel out of img " << endl;
			//	cout << u << "  " << v << " " << img1.cols << " " << img1.rows << endl;
			//	cout << u2 << "  " << v2 << " " << img2.cols << " " << img2.rows << endl;
				continue;
			}

//			cout << "uv in cam1:" << u << " " << v << endl;
//			cout << "uv in cam2:" << u2 << " " << v2 << endl;
//			cout << "p in cam2: " << pCam2 << endl;
//			cout << "p in cam1: " << pCam1 << endl;
            nGood++;
            goodProjection.push_back(Eigen::Vector2d(u2, v2));

			double xCam2_2 = xCam2 * xCam2, yCam2_2 = yCam2 * yCam2, zCam2_2 = zCam2 * zCam2;
			Matrix26d J_pixel_xi;   // pixel to \xi in Lie algebra
			J_pixel_xi(0,0) = fx / zCam2;
			J_pixel_xi(0,1) = 0;
		    J_pixel_xi(0,2) = - fx * xCam2 / zCam2_2;
		    J_pixel_xi(0,3) = - fx * xCam2 * yCam2 / zCam2_2;
		    J_pixel_xi(0,4) = fx + fx * xCam2_2 / zCam2_2;
		    J_pixel_xi(0,5) = - fx * yCam2 / zCam2;
		    J_pixel_xi(1,0) = 0;
		    J_pixel_xi(1,1) = fy / zCam2;
		    J_pixel_xi(1,2) = -fy * yCam2 / zCam2_2;
		    J_pixel_xi(1,3) = -fy - fy * yCam2_2 / zCam2_2;
		    J_pixel_xi(1,4) = fy * xCam2 * yCam2 / zCam2_2;
		    J_pixel_xi(1,5) = fy * xCam2 / zCam2;

			// and compute error and jacobian
            for (int x = -half_patch_size; x < half_patch_size; x++)
                for (int y = -half_patch_size; y < half_patch_size; y++) {

                    double error = 0;
                    Eigen::Vector2d J_img_pixel;    // image gradients

                    float u1_patch = u + x, v1_patch = v + y;
					float u2_patch = u2 + x, v2_patch = v2 + y;
					error = GetPixelValue(img1, u1_patch, v1_patch) - GetPixelValue(img2, u2_patch, v2_patch);
		    		J_img_pixel[0] = (GetPixelValue(img2, u2_patch + 1, v2_patch) - GetPixelValue(img2, u2_patch - 1, v2_patch)) / 2;
		  			J_img_pixel[1] = (GetPixelValue(img2, u2_patch, v2_patch + 1) - GetPixelValue(img2, u2_patch, v2_patch - 1)) / 2 ;
					// total jacobian
                    Vector6d J = -J_pixel_xi.transpose() * J_img_pixel;

                    H += J * J.transpose();
                    b += -error * J;
                    cost += error * error;
                }
            // END YOUR CODE HERE
        }

        // solve update and put it into estimation
        // TODO START YOUR CODE HERE
        Vector6d update;
		update = H.ldlt().solve(b);
        T21 = Sophus::SE3::exp(update) * T21;
        // END YOUR CODE HERE

        cost /= nGood;

        if (std::isnan(update[0])) {
            // sometimes occurred when we have a black or white patch and H is irreversible
            cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost) {
//            cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        lastCost = cost;
//        cout << "cost = " << cost << ", good = " << nGood << endl;
    }
//    cout << "good projection: " << nGood << endl;
//    cout << "T21 = \n" << T21.matrix() << endl;

    // in order to help you debug, we plot the projected pixels here
//    cv::Mat img1_show, img2_show;
//    cv::cvtColor(img1, img1_show, CV_GRAY2BGR);
//    cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
//    for (auto &px: px_ref) {
//        cv::rectangle(img1_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
//                      cv::Scalar(0, 250, 0));
//    }
//    for (auto &px: goodProjection) {
//        cv::rectangle(img2_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
//                      cv::Scalar(0, 250, 0));
//    }
//    cv::imshow("reference", img1_show);
//    cv::imshow("current", img2_show);
//    cv::waitKey();
}

void DirectPoseEstimationMultiLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3 &T21
) {

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector<cv::Mat> pyr1, pyr2; // image pyramids
    // TODO START YOUR CODE HERE
	for(int i = 0; i < pyramids; i++){
		Mat img1_resize, img2_resize;
		cv::resize(img1, img1_resize, Size(img1.cols * scales[i], img1.rows * scales[i]));
		cv::resize(img2, img2_resize, Size(img2.cols * scales[i], img2.rows * scales[i]));
		pyr1.push_back(img1_resize);
		pyr2.push_back(img2_resize);
	}
    // END YOUR CODE HERE

    double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
    for (int level = pyramids - 1; level >= 0; level--) {
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px: px_ref) {
            px_ref_pyr.push_back(scales[level] * px);
        }

        // TODO START YOUR CODE HERE
        // scale fx, fy, cx, cy in different pyramid levels
		fx = fxG * scales[level];
		fy = fyG * scales[level];
		cx = cxG * scales[level];
		cy = cyG * scales[level];

        // END YOUR CODE HERE
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);//T21 have add before's cal result.
    }
	cout << "Multi T21 = \n" << T21.matrix() << endl;
}


#include <Eigen/Eigen>
#include <boost/geometry.hpp>
#include <limits>
#include <lmlib/intersection.h>

namespace lm
{
box2f get_box(Eigen::Vector2f extents) { return box2f{-extents, extents}; }

float distance(Eigen::Vector2f point, Eigen::Vector2f box_extents)
{
    box2f box = get_box(box_extents);

    if (boost::geometry::within(point, box))
    {
        Eigen::Vector2f a{-box_extents[0], -box_extents[1]},
          b{-box_extents[0], box_extents[1]}, c{box_extents[0], box_extents[1]},
          d{box_extents[0], -box_extents[1]};

        std::initializer_list<segment2f> segments{
          {a, b}, {b, c}, {c, d}, {d, a}};

        float distance{std::numeric_limits<float>::max()};

        for (auto segment : segments)
        {
            distance = std::min(
              (float)boost::geometry::distance(segment, point), distance);
        }
        return -distance;
    }

    return boost::geometry::distance(point, box);
}

std::pair<segment2f, float>
  closest_edge(Eigen::Vector2f point, Eigen::Vector2f box_extents)
{
    Eigen::Vector2f a{-box_extents[0], -box_extents[1]},
      b{-box_extents[0], box_extents[1]}, c{box_extents[0], box_extents[1]},
      d{box_extents[0], -box_extents[1]};

    std::initializer_list<segment2f> segments{{a, b}, {b, c}, {c, d}, {d, a}};

    float min_distance{std::numeric_limits<float>::max()};
    segment2f closest;

    for (auto segment : segments)
    {
        double segment_distance = boost::geometry::distance(segment, point);
        if (segment_distance < min_distance)
        {
            min_distance = segment_distance;
            closest = segment;
        }
    }

    return {closest, min_distance};
}
} // namespace lm