#ifndef MIRRORPLASMA_HPP
#define MIRRORPLASMA_HPP

#include "PlasmaPhysics.hpp"
#include "Species.hpp"
#include "NetCDFIO.hpp"
#include <toml.hpp>

#include <boost/math/interpolators/barycentric_rational.hpp>

#include <memory>
#include <cmath>

class MirrorPlasma {
	public:
		class VacuumMirrorConfiguration {
			public:
				VacuumMirrorConfiguration( toml::value const& );
				/*
				VacuumMirrorConfiguration( const& VacuumMirrorConfiguration other ) :
					IonSpecies( other.IonSpecies )
				{
					MirrorRatio = other.MirrorRatio;
					PlasmaColumnWidth = other.PlasmaColumnWidth;
					PlasmaLength = other.PlasmaLength;
					AxialGapDistance = other.AxialGapDistance;
					WallRadius = other.WallRadius;
					CentralCellFieldStrength = other.CentralCellFieldStrength;
					AuxiliaryHeating = other.AuxiliaryHeating;
				}
				*/
				Species_t IonSpecies;
				double MirrorRatio;
				double PlasmaColumnWidth;
				double PlasmaLength;
				double AxialGapDistance;
				double WallRadius;
				double CentralCellFieldStrength;
				double AuxiliaryHeating;

				double ParallelFudgeFactor;
				double PerpFudgeFactor;
				bool AmbipolarPhi;

				bool AlphaHeating;
				bool ReportNuclearDiagnostics;
				bool ReportThrust;

				double InitialTemp;
				double InitialMach;
				bool Collisional;

				std::string OutputFile;
				std::string NetcdfOutputFile;

				double PlasmaVolume() const {
					return M_PI * ( PlasmaColumnWidth + 2 * AxialGapDistance ) * PlasmaColumnWidth * PlasmaLength;
				};
				double WallArea() const {
					return 2.0 * M_PI * WallRadius * WallRadius * PlasmaLength;
				};

				double ImposedVoltage;
				double PlasmaInnerRadius() const { return AxialGapDistance; };
				double PlasmaOuterRadius() const { return AxialGapDistance + PlasmaColumnWidth; };
				double PlasmaCentralRadius() const { return AxialGapDistance + PlasmaColumnWidth / 2.0; };
			private:

		};

		// Copy Constructor
		/*
		MirrorPlasma( MirrorPlasma const& other ) :
			pVacuumConfig( other.pVacuumConfig )
		{
			FuellingRate = other.FuellingRate;
			IonDensity = other.IonDensity;
			IonTemperature = other.IonTemperature;
			ElectronDensity = other.ElectronDensity;
			ElectronTemperature = other.ElectronTemperature;
		   NeutralDensity = other.NeutralDensity;
			NeutralSource = other.NeutralSource;
			Zeff = other.Zeff;
		   MachNumber = other.MachNumber;
		};
		*/

		MirrorPlasma( MirrorPlasma const& ) = delete;

		MirrorPlasma( toml::value const& configSection );

		double FuellingRate;
		double IonDensity,IonTemperature;
		double ElectronDensity,ElectronTemperature;
		double NeutralDensity,NeutralSource;
		double Zeff;

		double MachNumber; // Sonic Mach number defined with c_s^2 = Z T_e/m_i

		double SoundSpeed() const {
			// We *define* c_s^2 = Z_i T_e / m_i.
			double cs = ::sqrt( pVacuumConfig->IonSpecies.Charge * ElectronTemperature*ReferenceTemperature / ( pVacuumConfig->IonSpecies.Mass * ProtonMass ) );
			return cs;
		};

		// Defining (3/2) d( n_i T_i )/dt = IonHeating - IonHeatLosses
		//
		double ElectronHeatLosses() const;
		double IonHeatLosses() const;
		double ElectronHeating() const;
		double IonHeating() const;

		double EnergyConfinementTime() const {
			double StoredEnergy = 1.5 * ( ElectronDensity * ElectronTemperature + IonDensity * IonTemperature ) * ReferenceDensity * ReferenceTemperature;
			double TotalHeatLosses = ElectronHeatLosses() + IonHeatLosses();
			return StoredEnergy / TotalHeatLosses;
		};

		// RHS of dn/dt = <stuff>
		double IonParticleLosses() const;
		double ElectronParticleLosses() const;

		double ElectricPotential() const {
			return pVacuumConfig->CentralCellFieldStrength * MachNumber * SoundSpeed() * pVacuumConfig->PlasmaColumnWidth;
		};
		double IonLarmorRadius() const {
			return 1.02 * ::sqrt( pVacuumConfig->IonSpecies.Mass * IonTemperature * 1000 ) / ( pVacuumConfig->IonSpecies.Charge * pVacuumConfig->CentralCellFieldStrength * 10000 );
		};

		double Beta() const;
		double DebyeLength() const;

		double NuStar() const;
		double KineticEnergy() const;
		double ThermalEnergy() const;

		void PrintReport();

		void InitialiseNetCDF();
		void WriteTimeslice( double T );
		void FinaliseNetCDF();

		std::shared_ptr< VacuumMirrorConfiguration > pVacuumConfig;

		void SetMachFromVoltage();
		double AmbipolarPhi() const;

		void ComputeSteadyStateNeutrals();

		double ParallelMomentumLossRate() const;
		double initialTemperature() const { return pVacuumConfig->InitialTemp; };
		double initialMach() const { return pVacuumConfig->InitialMach; };
	private:
		NetCDFIO nc_output;

		double LogLambdaElectron() const;
		double LogLambdaIon() const;
		double LogLambdaAlphaElectron() const;

		double ElectronCollisionTime() const;
		double IonCollisionTime() const;
		double CollisionalTemperatureEquilibrationTime() const;

		double SlowingDownTime() const;

		double IonToElectronHeatTransfer() const;

		double BremsstrahlungLosses() const;
		double NeutralLosses() const;
		double ClassicalHeatLosses() const;
		double ParallelHeatLosses() const;

		double ParallelElectronPastukhovLossRate( double Phi ) const;
		double ParallelElectronParticleLoss() const;
		double ParallelElectronHeatLoss() const;

		double Chi_i( double ) const;
		double Chi_i() const;

		double ParallelIonPastukhovLossRate( double Phi ) const;
		double ParallelIonParticleLoss() const;
		double ParallelIonHeatLoss() const;

		double ParallelKineticEnergyLoss() const;

		double ClassicalIonHeatLoss() const;
		double ClassicalElectronHeatLoss() const;

		double ClassicalElectronParticleLosses() const;
		double ClassicalIonParticleLosses() const;

		double ClassicalViscosity() const;
		double AlfvenMachNumber() const;

		double ViscousHeating() const;

		double CentrifugalPotential() const;

		double IonCyclotronFrequency() const
		{
			double MagneticField = pVacuumConfig->CentralCellFieldStrength;
			return pVacuumConfig->IonSpecies.Charge * ElectronCharge * MagneticField / ( pVacuumConfig->IonSpecies.Mass * ProtonMass );
		};

		double ElectronCyclotronFrequency() const
		{
			double MagneticField = pVacuumConfig->CentralCellFieldStrength;
			return ElectronCharge * MagneticField / ElectronMass;
		};

		double FusionAlphaPowerDensity() const;
		double AlphaProductionRate() const;
		double NeutronOutput() const;
		double ThermalPowerOutput() const;
		double NeutronWallLoading() const;
		double DDNeutronRate() const;

		using interpolant = boost::math::barycentric_rational<double>;
		std::unique_ptr<interpolant> VoltageFunction;
		void ReadVoltageFile( std::string const& );
		double time;
		bool isTimeDependent;
	public:
		double AlphaHeating() const;
		double PromptAlphaLossFraction() const;
		double PromptAlphaThrust() const;
		double AlphaPromptLosses() const;
		double AlphaParallelLossRate() const;
		double ViscousTorque() const;
		double ParallelAngularMomentumLossRate() const;
		double RadialCurrent() const;
		double ParallelIonThrust() const;
		double ParallelCurrent(double) const;
		void UpdateVoltage();
		void SetTime( double );
};

#endif // MIRRORPLASMA_HPP


/*
 * DiscretizedFunction.cpp
 *
 *  Created on: 16.06.2017
 *      Author: thies
 */

#include <deal.II/base/data_out_base.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/utilities.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <random>
#include <utility>

#include <base/DiscretizedFunction.h>

using namespace dealii;

namespace wavepi {
namespace base {

inline double square(const double x) { return x * x; }

inline double pow4(const double x) { return x * x * x * x; }

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(std::shared_ptr<SpaceTimeMesh<dim>> mesh,
                                              std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm,
                                              bool store_derivative)
    : mesh(mesh), norm_(norm), store_derivative(store_derivative), cur_time_idx(0) {
  Assert(mesh && norm, ExcNotInitialized());

  function_coefficients.reserve(mesh->length());

  if (store_derivative) derivative_coefficients.reserve(mesh->length());

  for (size_t i = 0; i < mesh->length(); i++) {
    function_coefficients.emplace_back(mesh->n_dofs(i));

    if (store_derivative) derivative_coefficients.emplace_back(mesh->n_dofs(i));
  }
}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(std::shared_ptr<SpaceTimeMesh<dim>> mesh,
                                              std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm)
    : DiscretizedFunction(mesh, norm, false) {}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(std::shared_ptr<SpaceTimeMesh<dim>> mesh)
    : DiscretizedFunction(mesh, std::make_shared<InvalidNorm<DiscretizedFunction<dim>>>()) {}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(std::shared_ptr<SpaceTimeMesh<dim>> mesh, Function<dim>& function,
                                              std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm)
    : mesh(mesh), norm_(norm), store_derivative(false), cur_time_idx(0) {
  Assert(mesh && norm, ExcNotInitialized());

  function_coefficients.reserve(mesh->length());

  for (size_t i = 0; i < mesh->length(); i++) {
    auto dof_handler = mesh->get_dof_handler(i);

    Vector<double> tmp(dof_handler->n_dofs());
    function.set_time(mesh->get_time(i));

    VectorTools::interpolate(*dof_handler, function, tmp);
    mesh->get_constraint_matrix(i)->distribute(tmp);

    function_coefficients.push_back(std::move(tmp));
  }
}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(std::shared_ptr<SpaceTimeMesh<dim>> mesh, Function<dim>& function)
    : DiscretizedFunction(mesh, function, std::make_shared<InvalidNorm<DiscretizedFunction<dim>>>()) {}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(DiscretizedFunction<dim>&& o)
    : LightFunction<dim>(),
      mesh(std::move(o.mesh)),
      norm_(o.norm_),
      store_derivative(o.store_derivative),
      cur_time_idx(o.cur_time_idx),
      function_coefficients(std::move(o.function_coefficients)),
      derivative_coefficients(std::move(o.derivative_coefficients)) {
  Assert(mesh, ExcNotInitialized());

  o.mesh = std::shared_ptr<SpaceTimeMesh<dim>>();
}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(const DiscretizedFunction<dim>& o)
    : LightFunction<dim>(),
      mesh(o.mesh),
      norm_(o.norm_),
      store_derivative(o.store_derivative),
      cur_time_idx(o.cur_time_idx),
      function_coefficients(o.function_coefficients),
      derivative_coefficients(o.derivative_coefficients) {
  Assert(mesh, ExcNotInitialized());
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator=(DiscretizedFunction<dim>&& o) {
  mesh                    = std::move(o.mesh);
  norm_                   = o.norm_;
  store_derivative        = o.store_derivative;
  cur_time_idx            = o.cur_time_idx;
  function_coefficients   = std::move(o.function_coefficients);
  derivative_coefficients = std::move(o.derivative_coefficients);

  o.mesh = std::shared_ptr<SpaceTimeMesh<dim>>();

  Assert(mesh, ExcNotInitialized());
  return *this;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator=(const DiscretizedFunction<dim>& o) {
  mesh                    = o.mesh;
  norm_                   = o.norm_;
  store_derivative        = o.store_derivative;
  cur_time_idx            = o.cur_time_idx;
  function_coefficients   = o.function_coefficients;
  derivative_coefficients = o.derivative_coefficients;

  Assert(mesh, ExcNotInitialized());
  return *this;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::derivative() const {
  AssertThrow(store_derivative, ExcInternalError());
  AssertThrow(mesh, ExcNotInitialized());

  DiscretizedFunction<dim> result(mesh);
  result.function_coefficients = this->derivative_coefficients;

  return result;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::calculate_derivative() const {
  AssertThrow(mesh, ExcNotInitialized());
  AssertThrow(mesh->length() > 1, ExcInternalError());
  // AssertThrow(!store_derivative, ExcInternalError()); // why would you want to calculate it in this case?

  DiscretizedFunction<dim> result(mesh, norm_);

  /* implementation for constant mesh */
  /*
   for (size_t i = 0; i < mesh->length(); i++) {
   if (i < mesh->length() - 1)
   AssertThrow(function_coefficients[i + 1].size() == function_coefficients[i].size(),
   ExcNotImplemented());

   if (i == 0) {
   result.function_coefficients[i] = function_coefficients[i + 1];
   result.function_coefficients[i] -= function_coefficients[i];
   result.function_coefficients[i] /= mesh->get_time(i + 1) - mesh->get_time(i);
   } else if (i == mesh->length() - 1) {
   result.function_coefficients[i] = function_coefficients[i];
   result.function_coefficients[i] -= function_coefficients[i - 1];
   result.function_coefficients[i] /= mesh->get_time(i) - mesh->get_time(i - 1);
   } else {
   result.function_coefficients[i] = function_coefficients[i + 1];
   result.function_coefficients[i] -= function_coefficients[i - 1];
   result.function_coefficients[i] /= mesh->get_time(i + 1) - mesh->get_time(i - 1);
   }
   }
   */

  /* naive, but working implementation for non-constant mesh */
  /*
   for (size_t i = 0; i < mesh->length(); i++) {
   if (i == 0) {
   Vector<double> next_coefficients = function_coefficients[i + 1];
   mesh->transfer(i + 1, i, { &next_coefficients });

   result.function_coefficients[i] = next_coefficients;
   result.function_coefficients[i] -= function_coefficients[i];
   result.function_coefficients[i] /= mesh->get_time(i + 1) - mesh->get_time(i);
   } else if (i == mesh->length() - 1) {
   Vector<double> last_coefficients = function_coefficients[i - 1];
   mesh->transfer(i - 1, i, { &last_coefficients });

   result.function_coefficients[i] = function_coefficients[i];
   result.function_coefficients[i] -= last_coefficients;
   result.function_coefficients[i] /= mesh->get_time(i) - mesh->get_time(i - 1);
   } else {
   Vector<double> last_coefficients = function_coefficients[i - 1];
   Vector<double> next_coefficients = function_coefficients[i + 1];

   mesh->transfer(i - 1, i, { &last_coefficients });
   mesh->transfer(i + 1, i, { &next_coefficients });

   result.function_coefficients[i] = next_coefficients;
   result.function_coefficients[i] -= last_coefficients;
   result.function_coefficients[i] /= mesh->get_time(i + 1) - mesh->get_time(i - 1);
   }
   }
   */

  /* better: forward- and backward sweep */
  // forward sweep
  for (size_t i = 0; i < mesh->length(); i++) {
    if (i == 0) {
      result.function_coefficients[i].equ(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), function_coefficients[i]);
    } else if (i == mesh->length() - 1) {
      Vector<double> last_coefficients = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&last_coefficients});

      result.function_coefficients[i] = function_coefficients[i];
      result.function_coefficients[i] -= last_coefficients;
      result.function_coefficients[i] /= mesh->get_time(i) - mesh->get_time(i - 1);
    } else {
      Vector<double> last_coefficients = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&last_coefficients});

      result.function_coefficients[i].equ(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i - 1)), last_coefficients);
    }
  }

  // backward sweep
  for (size_t j = 0; j < mesh->length(); j++) {
    size_t i = mesh->length() - 1 - j;

    if (i == 0) {
      Vector<double> next_coefficients = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&next_coefficients});

      result.function_coefficients[i].add(1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), next_coefficients);
    } else if (i == mesh->length() - 1) {
      // nothing to be done
    } else {
      Vector<double> next_coefficients = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&next_coefficients});

      result.function_coefficients[i].add(1.0 / (mesh->get_time(i + 1) - mesh->get_time(i - 1)), next_coefficients);
    }
  }

  return result;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::calculate_second_derivative() const {
  AssertThrow(mesh, ExcNotInitialized());
  AssertThrow(mesh->length() > 1, ExcInternalError());

  DiscretizedFunction<dim> result(mesh, norm_);

  /* implementation for constant mesh */

  for (size_t i = 0; i < mesh->length(); i++) {
    if (i < mesh->length() - 1)
      AssertThrow(function_coefficients[i + 1].size() == function_coefficients[i].size(), ExcNotImplemented());

    if (i == 0) {
      result.function_coefficients[i] = function_coefficients[i];
      result.function_coefficients[i].add(-2, function_coefficients[i + 1]);
      result.function_coefficients[i].add(1, function_coefficients[i + 2]);
      result.function_coefficients[i] /= square(mesh->get_time(i + 1) - mesh->get_time(i));
    } else if (i == mesh->length() - 1) {
      result.function_coefficients[i] = function_coefficients[i];
      result.function_coefficients[i].add(-2, function_coefficients[i - 1]);
      result.function_coefficients[i].add(1, function_coefficients[i - 2]);
      result.function_coefficients[i] /= square(mesh->get_time(i) - mesh->get_time(i - 1));
    } else {
      result.function_coefficients[i].equ(-2, function_coefficients[i]);
      result.function_coefficients[i].add(1, function_coefficients[i + 1]);
      result.function_coefficients[i].add(1, function_coefficients[i - 1]);
      result.function_coefficients[i] /= square(mesh->get_time(i + 1) - mesh->get_time(i - 1)) / 4;
    }
  }

  return result;
}

template <int dim>
double DiscretizedFunction<dim>::absolute_error(DiscretizedFunction<dim>& other, double* norm_out) const {
  LogStream::Prefix p("calculate_error");
  AssertThrow(other.mesh == mesh, ExcInternalError());

  DiscretizedFunction<dim> tmp = other;
  tmp.set_norm(norm_);

  if (norm_out) *norm_out = tmp.norm();

  tmp -= *this;

  return tmp.norm();
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::calculate_derivative_transpose() const {
  AssertThrow(mesh, ExcNotInitialized());

  // because of the special cases
  AssertThrow(mesh->length() > 3, ExcInternalError());

  DiscretizedFunction<dim> result(mesh, norm_);

  /* implementation for constant mesh */
  /*
   for (size_t i = 0; i < mesh->length(); i++) {
   auto dest = &result.function_coefficients[i];

   if (i < mesh->length() - 1)
   AssertThrow(function_coefficients[i + 1].size() == function_coefficients[i].size(),
   ExcNotImplemented());

   if (i == 0) {
   *dest = function_coefficients[i + 1];
   dest->sadd(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)),
   -1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), function_coefficients[i]);
   } else if (i == 1) {
   *dest = function_coefficients[i + 1];
   dest->sadd(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)),
   1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)), function_coefficients[i - 1]);
   } else if (i == mesh->length() - 1) {
   *dest = function_coefficients[i];
   dest->sadd(1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)),
   1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
   } else if (i == mesh->length() - 2) {
   *dest = function_coefficients[i + 1];
   dest->sadd(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)),
   1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
   } else {
   *dest = function_coefficients[i + 1];
   dest->sadd(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)),
   1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
   }
   }
   */

  /* naive, but working implementation for non-constant mesh */
  /*for (size_t i = 0; i < mesh->length(); i++) {
   auto& dest = result.function_coefficients[i];

   if (i == 0) {
   Vector<double> tmp = function_coefficients[i + 1];
   mesh->transfer(i + 1, i, {&tmp});
   dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);

   dest.add(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), function_coefficients[i]);
   } else if (i == 1) {
   Vector<double> tmp = function_coefficients[i + 1];
   mesh->transfer(i + 1, i, {&tmp});
   dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);

   tmp = function_coefficients[i - 1];
   mesh->transfer(i - 1, i, {&tmp});
   dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)), tmp);
   } else if (i == mesh->length() - 1) {
   dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)), function_coefficients[i]);

   Vector<double> tmp = function_coefficients[i - 1];
   mesh->transfer(i - 1, i, &tmp);
   dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);
   } else if (i == mesh->length() - 2) {
   Vector<double> tmp = function_coefficients[i + 1];
   mesh->transfer(i + 1, i, {&tmp});
   dest.add(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), tmp);

   tmp = function_coefficients[i - 1];
   mesh->transfer(i - 1, i, {&tmp});
   dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);
   } else {
   Vector<double> tmp = function_coefficients[i + 1];
   mesh->transfer(i + 1, i, {&tmp});
   dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);

   tmp = function_coefficients[i - 1];
   mesh->transfer(i - 1, i, {&tmp});
   dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);
   }
   }
   */

  /* better: forward- and backward sweep */
  // forward sweep
  for (size_t i = 0; i < mesh->length(); i++) {
    auto& dest = result.function_coefficients[i];

    if (i == 0) {
      dest.add(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), function_coefficients[i]);
    } else if (i == 1) {
      Vector<double> tmp = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&tmp});
      dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)), tmp);
    } else if (i == mesh->length() - 1) {
      Vector<double> tmp = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&tmp});
      dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);

      dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)), function_coefficients[i]);
    } else if (i == mesh->length() - 2) {
      Vector<double> tmp = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&tmp});
      dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);
    } else {
      Vector<double> tmp = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&tmp});
      dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);
    }
  }

  // backward sweep
  for (size_t j = 0; j < mesh->length(); j++) {
    size_t i   = mesh->length() - 1 - j;
    auto& dest = result.function_coefficients[i];

    if (i == 0) {
      Vector<double> tmp = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&tmp});
      dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);
    } else if (i == 1) {
      Vector<double> tmp = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&tmp});
      dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);
    } else if (i == mesh->length() - 1) {
      // nothing to be done
    } else if (i == mesh->length() - 2) {
      Vector<double> tmp = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&tmp});
      dest.add(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), tmp);
    } else {
      Vector<double> tmp = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&tmp});
      dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);
    }
  }

  return result;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::calculate_second_derivative_transpose() const {
  AssertThrow(mesh, ExcNotInitialized());

  // because of the special cases
  AssertThrow(mesh->length() > 3, ExcInternalError());

  DiscretizedFunction<dim> result(mesh, norm_);

  /* implementation for constant mesh */

  for (size_t i = 0; i < mesh->length(); i++) {
    auto dest = &result.function_coefficients[i];

    if (i < mesh->length() - 1)
      AssertThrow(function_coefficients[i + 1].size() == function_coefficients[i].size(), ExcNotImplemented());

    if (i == 0) {
      dest->equ(1.0 / square(mesh->get_time(i + 1) - mesh->get_time(i)), function_coefficients[i]);
      dest->add(1.0 * 4 / square(mesh->get_time(i + 2) - mesh->get_time(i)), function_coefficients[i + 1]);
    } else if (i == 1) {
      dest->equ(-2.0 * 4 / square(mesh->get_time(i + 1) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(-2.0 / square(mesh->get_time(i) - mesh->get_time(i - 1)), function_coefficients[i - 1]);
      dest->add(1.0 * 4 / square(mesh->get_time(i + 2) - mesh->get_time(i)), function_coefficients[i + 1]);
    } else if (i == 2) {
      dest->equ(-2.0 * 4 / square(mesh->get_time(i + 1) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(1.0 * 4 / square(mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
      dest->add(1.0 * 4 / square(mesh->get_time(i + 2) - mesh->get_time(i)), function_coefficients[i + 1]);
      dest->add(1.0 / square(mesh->get_time(i - 2) - mesh->get_time(i - 1)), function_coefficients[i - 2]);
    } else if (i == mesh->length() - 1) {
      dest->equ(1.0 / square(mesh->get_time(i) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(1.0 * 4 / square(mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
    } else if (i == mesh->length() - 2) {
      dest->equ(-2.0 * 4 / square(mesh->get_time(i + 1) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(-2.0 / square(mesh->get_time(i) - mesh->get_time(i + 1)), function_coefficients[i + 1]);
      dest->add(1.0 * 4 / square(mesh->get_time(i - 2) - mesh->get_time(i)), function_coefficients[i - 1]);
    } else if (i == mesh->length() - 3) {
      dest->equ(-2.0 * 4 / square(mesh->get_time(i + 1) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(1.0 * 4 / square(mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
      dest->add(1.0 * 4 / square(mesh->get_time(i + 2) - mesh->get_time(i)), function_coefficients[i + 1]);
      dest->add(1.0 / square(mesh->get_time(i + 2) - mesh->get_time(i + 1)), function_coefficients[i + 2]);
    } else {
      dest->equ(-2.0 * 4 / square(mesh->get_time(i + 1) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(1.0 * 4 / square(mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
      dest->add(1.0 * 4 / square(mesh->get_time(i + 2) - mesh->get_time(i)), function_coefficients[i + 1]);
    }
  }

  return result;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator=(double x) {
  AssertThrow(mesh, ExcNotInitialized());

  for (size_t i = 0; i < mesh->length(); i++) {
    function_coefficients[i] = x;

    if (store_derivative) derivative_coefficients[i] = 0.0;
  }

  return *this;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator+=(double offset) {
  // note that this is correct independent of store_derivative.

  for (size_t i = 0; i < mesh->length(); i++) {
    function_coefficients[i].add(offset);
  }

  return *this;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator+=(const DiscretizedFunction<dim>& V) {
  AssertThrow(norm_ && V.norm_, ExcNotInitialized());
  AssertThrow(*norm_ == *V.norm_, ExcMessage("DiscretizedFunction<dim>::operator+= : Norms not compatible"));
  this->add(1.0, V);

  return *this;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator-=(const DiscretizedFunction<dim>& V) {
  AssertThrow(norm_ && V.norm_, ExcNotInitialized());
  AssertThrow(*norm_ == *V.norm_, ExcMessage("DiscretizedFunction<dim>::operator-= : Norms not compatible"));
  this->add(-1.0, V);

  return *this;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator*=(const double factor) {
  AssertThrow(mesh, ExcNotInitialized());

  for (size_t i = 0; i < mesh->length(); i++) {
    function_coefficients[i] *= factor;

    if (store_derivative) derivative_coefficients[i] *= factor;
  }

  return *this;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::noise(const DiscretizedFunction<dim>& like) {
  Assert(!like.store_derivative, ExcInternalError());

  DiscretizedFunction<dim> res = noise(like.mesh);
  res.set_norm(like.get_norm());

  return res;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::noise(std::shared_ptr<SpaceTimeMesh<dim>> mesh) {
  Assert(mesh, ExcNotInitialized());

  DiscretizedFunction<dim> res(mesh);

  // auto time = std::chrono::high_resolution_clock::now();
  // std::default_random_engine generator(time.time_since_epoch().count() % 1000000);
  std::default_random_engine generator(2307);
  std::uniform_real_distribution<double> distribution(-1, 1);

  for (size_t i = 0; i < res.mesh->length(); i++)
    for (size_t j = 0; j < res.function_coefficients[i].size(); j++)
      res.function_coefficients[i][j] = distribution(generator);

  return res;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::noise(const DiscretizedFunction<dim>& like, double norm) {
  DiscretizedFunction<dim> result = noise(like);

  result *= norm / result.norm();

  return result;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator/=(const double factor) {
  return this->operator*=(1.0 / factor);
}

template <int dim>
void DiscretizedFunction<dim>::pointwise_multiplication(const DiscretizedFunction<dim>& V) {
  Assert(mesh, ExcNotInitialized());
  Assert(mesh == V.mesh, ExcInternalError());
  Assert(!store_derivative || (store_derivative == V.store_derivative), ExcInternalError());

  for (size_t i = 0; i < mesh->length(); i++) {
    Assert(function_coefficients[i].size() == V.function_coefficients[i].size(),
           ExcDimensionMismatch(function_coefficients[i].size(), V.function_coefficients[i].size()));

    if (store_derivative) {
      Assert(derivative_coefficients[i].size() == V.derivative_coefficients[i].size(),
             ExcDimensionMismatch(derivative_coefficients[i].size(), V.derivative_coefficients[i].size()));

      derivative_coefficients[i].scale(V.function_coefficients[i]);

      Vector<double> tmp = function_coefficients[i];
      tmp.scale(V.derivative_coefficients[i]);

      derivative_coefficients[i] += tmp;
    }

    function_coefficients[i].scale(V.function_coefficients[i]);
  }
}

template <int dim>
void DiscretizedFunction<dim>::add(const double a, const DiscretizedFunction<dim>& V) {
  AssertThrow(mesh && norm_ && V.norm_, ExcNotInitialized());
  AssertThrow(mesh == V.mesh, ExcInternalError());
  AssertThrow(!store_derivative || (store_derivative == V.store_derivative), ExcInternalError());
  AssertThrow(*norm_ == *V.norm_, ExcMessage("DiscretizedFunction<dim>::add : Norms not compatible"));

  for (size_t i = 0; i < mesh->length(); i++) {
    Assert(function_coefficients[i].size() == V.function_coefficients[i].size(),
           ExcDimensionMismatch(function_coefficients[i].size(), V.function_coefficients[i].size()));

    function_coefficients[i].add(a, V.function_coefficients[i]);

    if (store_derivative) {
      Assert(derivative_coefficients[i].size() == V.derivative_coefficients[i].size(),
             ExcDimensionMismatch(derivative_coefficients[i].size(), V.derivative_coefficients[i].size()));

      derivative_coefficients[i].add(a, V.derivative_coefficients[i]);
    }
  }
}

template <int dim>
void DiscretizedFunction<dim>::sadd(const double s, const double a, const DiscretizedFunction<dim>& V) {
  AssertThrow(mesh && norm_ && V.norm_, ExcNotInitialized());
  AssertThrow(mesh == V.mesh, ExcInternalError());
  AssertThrow(!store_derivative || (store_derivative == V.store_derivative), ExcInternalError());
  AssertThrow(*norm_ == *V.norm_, ExcMessage("DiscretizedFunction<dim>::sadd : Norms not compatible"));

  for (size_t i = 0; i < mesh->length(); i++) {
    Assert(function_coefficients[i].size() == V.function_coefficients[i].size(),
           ExcDimensionMismatch(function_coefficients[i].size(), V.function_coefficients[i].size()));

    function_coefficients[i].sadd(s, a, V.function_coefficients[i]);

    if (store_derivative) {
      Assert(derivative_coefficients[i].size() == V.derivative_coefficients[i].size(),
             ExcDimensionMismatch(derivative_coefficients[i].size(), V.derivative_coefficients[i].size()));

      derivative_coefficients[i].sadd(s, a, V.derivative_coefficients[i]);
    }
  }
}

template <int dim>
void DiscretizedFunction<dim>::throw_away_derivative() {
  store_derivative        = false;
  derivative_coefficients = std::vector<Vector<double>>();
}

template <int dim>
double DiscretizedFunction<dim>::norm() const {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  return norm_->norm(*this);
}

template <int dim>
double DiscretizedFunction<dim>::operator*(const DiscretizedFunction<dim>& V) const {
  Assert(mesh && norm_ && V.norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());
  AssertThrow(*V.norm_ == *norm_, ExcMessage("DiscretizedFunction<dim>::operator* : Norms not compatible"));

  return norm_->dot(*this, V);
}

template <int dim>
double DiscretizedFunction<dim>::dot(const DiscretizedFunction<dim>& V) const {
  return (*this) * V;
}

template <int dim>
void DiscretizedFunction<dim>::duality_mapping_lp(double p) {
  AssertThrow(p > 1, ExcMessage("duality_mapping_lp: p has to be larger than 1!"));

  for (size_t i = 0; i < mesh->length(); i++)
    for (size_t j = 0; j < function_coefficients[i].size(); j++)
      if (function_coefficients[i][j] != 0.0)
        function_coefficients[i][j] =
            std::pow(std::abs(function_coefficients[i][j]), p - 2) * function_coefficients[i][j];
}

template <int dim>
void DiscretizedFunction<dim>::duality_mapping(double p) {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->duality_mapping(*this, p);
}

template <int dim>
void DiscretizedFunction<dim>::duality_mapping_dual(double q) {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->duality_mapping_dual(*this, q);
}

template <int dim>
double DiscretizedFunction<dim>::norm_dual() const {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  return norm_->norm_dual(*this);
}

template <int dim>
double DiscretizedFunction<dim>::norm_p(double p) {
  AssertThrow(p >= 1, ExcMessage("norm_p: p has to be >= 1!"));
  double result = 0.0;

  for (size_t i = 0; i < mesh->length(); i++)
    for (size_t j = 0; j < function_coefficients[i].size(); j++)
      result += std::pow(std::abs(function_coefficients[i][j]), p);

  return std::pow(result, 1 / p);
}

template <int dim>
bool DiscretizedFunction<dim>::hilbert() const {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  return norm_->hilbert();
}

template <int dim>
void DiscretizedFunction<dim>::dot_transform() {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->dot_transform(*this);
}

template <int dim>
void DiscretizedFunction<dim>::dot_transform_inverse() {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->dot_transform_inverse(*this);
}

template <int dim>
void DiscretizedFunction<dim>::dot_solve_mass_and_transform() {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->dot_solve_mass_and_transform(*this);
}

template <int dim>
void DiscretizedFunction<dim>::dot_mult_mass_and_transform_inverse() {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->dot_mult_mass_and_transform_inverse(*this);
}

template <int dim>
void DiscretizedFunction<dim>::mult_mass() {
  Assert(!store_derivative, ExcInternalError());

  for (size_t i = 0; i < mesh->length(); i++) {
    Vector<double> tmp(function_coefficients[i].size());
    mesh->get_mass_matrix(i)->vmult(tmp, function_coefficients[i]);
    function_coefficients[i] = tmp;
  }
}

template <int dim>
void DiscretizedFunction<dim>::solve_mass() {
  Assert(!store_derivative, ExcInternalError());

  LogStream::Prefix p("solve_mass");
  Timer timer;
  timer.start();

  // PreconditionIdentity precondition;
  PreconditionSSOR<SparseMatrix<double>> precondition;
  precondition.initialize(*mesh->get_mass_matrix(0), PreconditionSSOR<SparseMatrix<double>>::AdditionalData(1.0));

  for (size_t i = 0; i < mesh->length(); i++) {
    LogStream::Prefix p("step-" + Utilities::int_to_string(i, 4));

    Vector<double> tmp(function_coefficients[i].size());

    SolverControl solver_control(2000, 1e-10 * function_coefficients[i].l2_norm());
    SolverCG<> cg(solver_control);

    cg.solve(*mesh->get_mass_matrix(i), tmp, function_coefficients[i], precondition);
    function_coefficients[i] = tmp;
  }

  deallog << "solved space-time-mass matrices in " << timer.wall_time() << "s" << std::endl;
}

template <int dim>
void DiscretizedFunction<dim>::write_pvd(std::string path, std::string filename, std::string name) const {
  write_pvd(path, filename, name, name + "_prime");
}

template <int dim>
void DiscretizedFunction<dim>::write_pvd(std::string path, std::string filename, std::string name,
                                         std::string name_deriv) const {
  Assert(mesh, ExcNotInitialized());

  LogStream::Prefix p("write_pvd");
  deallog << "Writing " << path << filename << ".pvd" << std::endl;

  Assert(mesh->length() < 10000, ExcNotImplemented());  // 4 digits are ok
  std::vector<std::pair<double, std::string>> times_and_names(mesh->length(), std::pair<double, std::string>(0.0, ""));

  for (size_t i = 0; i < mesh->length(); i++) {
    const std::string vtuname = filename + "-" + Utilities::int_to_string(i, 4) + ".vtu";
    times_and_names[i]        = std::pair<double, std::string>(mesh->get_time(i), vtuname);

    write_vtu(name, name_deriv, path + vtuname, i);
  }

  std::ofstream pvd_output(path + filename + ".pvd");
  AssertThrow(pvd_output, ExcMessage("write_pvd :: output handle invalid"));

  DataOutBase::write_pvd_record(pvd_output, times_and_names);
  // deallog << "Wrote " << filename << std::endl;
}

template <int dim>
void DiscretizedFunction<dim>::write_vtu(const std::string name, const std::string name_deriv,
                                         const std::string filename, size_t i) const {
  DataOut<dim> data_out;

  data_out.attach_dof_handler(*mesh->get_dof_handler(i));
  data_out.add_data_vector(function_coefficients[i], name);

  if (store_derivative) data_out.add_data_vector(derivative_coefficients[i], name_deriv);

  data_out.build_patches();

  deallog << "Writing " << filename << std::endl;

  std::ofstream output(filename.c_str());
  AssertThrow(output, ExcMessage("write_vtk :: output handle invalid"));

  data_out.write_vtu(output);
  // deallog << "Wrote " << filename << std::endl;
}

template <int dim>
const std::shared_ptr<Norm<DiscretizedFunction<dim>>> DiscretizedFunction<dim>::get_norm() const {
  return norm_;
}

template <int dim>
void DiscretizedFunction<dim>::set_norm(std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm) {
  this->norm_ = norm;
}

template <int dim>
double DiscretizedFunction<dim>::evaluate(const Point<dim>& p, const double time) const {
  const size_t time_idx = mesh->find_time(time);

  return VectorTools::point_value(*mesh->get_dof_handler(time_idx), function_coefficients[time_idx], p);
}

template <int dim>
double DiscretizedFunction<dim>::value(const Point<dim>& p, const unsigned int component) const {
  Assert(component == 0, ExcIndexRange(component, 0, 1));
  Assert(cur_time_idx >= 0 && cur_time_idx < mesh->length(), ExcIndexRange(cur_time_idx, 0, mesh->length()));

  return VectorTools::point_value(*mesh->get_dof_handler(cur_time_idx), function_coefficients[cur_time_idx], p);
}

template <int dim>
Tensor<1, dim, double> DiscretizedFunction<dim>::gradient(const Point<dim>& p, const unsigned int component) const {
  Assert(component == 0, ExcIndexRange(component, 0, 1));
  Assert(cur_time_idx >= 0 && cur_time_idx < mesh->length(), ExcIndexRange(cur_time_idx, 0, mesh->length()));

  return VectorTools::point_gradient(*mesh->get_dof_handler(cur_time_idx), function_coefficients[cur_time_idx], p);
}

template <int dim>
double DiscretizedFunction<dim>::get_time_index() const {
  return cur_time_idx;
}

template <int dim>
void DiscretizedFunction<dim>::set_time(const double new_time) {
  Function<dim>::set_time(new_time);
  cur_time_idx = mesh->find_time(new_time);
}

template <int dim>
std::shared_ptr<SpaceTimeMesh<dim>> DiscretizedFunction<dim>::get_mesh() const {
  return mesh;
}

template <int dim>
void DiscretizedFunction<dim>::min_max_value(double* min_out, double* max_out) const {
  *min_out = std::numeric_limits<double>::infinity();
  *max_out = -std::numeric_limits<double>::infinity();

  for (size_t i = 0; i < this->length(); i++)
    for (size_t j = 0; j < function_coefficients[i].size(); j++) {
      if (*min_out > function_coefficients[i][j]) *min_out = function_coefficients[i][j];

      if (*max_out < function_coefficients[i][j]) *max_out = function_coefficients[i][j];
    }
}

template <int dim>
double DiscretizedFunction<dim>::min_value() const {
  double tmp = std::numeric_limits<double>::infinity();

  for (size_t i = 0; i < this->length(); i++)
    for (size_t j = 0; j < function_coefficients[i].size(); j++)
      if (tmp > function_coefficients[i][j]) tmp = function_coefficients[i][j];

  return tmp;
}

template <int dim>
double DiscretizedFunction<dim>::max_value() const {
  double tmp = -std::numeric_limits<double>::infinity();

  for (size_t i = 0; i < this->length(); i++)
    for (size_t j = 0; j < function_coefficients[i].size(); j++)
      if (tmp < function_coefficients[i][j]) tmp = function_coefficients[i][j];

  return tmp;
}

template <int dim>
double DiscretizedFunction<dim>::relative_error(const DiscretizedFunction<dim>& other) const {
  DiscretizedFunction<dim> tmp(*this);
  tmp -= other;

  double denom = this->norm();
  return tmp.norm() / (denom == 0.0 ? 1.0 : denom);
}

#ifdef WAVEPI_MPI
template <int dim>
void DiscretizedFunction<dim>::mpi_irecv(size_t source, std::vector<MPI_Request>& reqs) {
  AssertThrow(reqs.size() == 0, ExcInternalError());

  reqs.reserve(function_coefficients.size());

  for (size_t i = 0; i < mesh->length(); i++) {
    reqs.emplace_back();
    MPI_Irecv(&function_coefficients[i][0], function_coefficients[i].size(), MPI_DOUBLE, source, 1, MPI_COMM_WORLD,
              &reqs[i]);
  }

  if (store_derivative) {
    reqs.reserve(function_coefficients.size() + derivative_coefficients.size());

    for (size_t i = 0; i < mesh->length(); i++) {
      reqs.emplace_back();
      MPI_Irecv(&derivative_coefficients[i][0], derivative_coefficients[i].size(), MPI_DOUBLE, source, 1,
                MPI_COMM_WORLD, &reqs[function_coefficients.size() + i]);
    }
  }
}

template <int dim>
void DiscretizedFunction<dim>::mpi_send(size_t destination) {
  for (size_t i = 0; i < mesh->length(); i++)
    MPI_Send(&function_coefficients[i][0], function_coefficients[i].size(), MPI_DOUBLE, destination, 1, MPI_COMM_WORLD);

  if (store_derivative) {
    for (size_t i = 0; i < mesh->length(); i++)
      MPI_Send(&derivative_coefficients[i][0], derivative_coefficients[i].size(), MPI_DOUBLE, destination, 1,
               MPI_COMM_WORLD);
  }
}

template <int dim>
void DiscretizedFunction<dim>::mpi_bcast(size_t root) {
  for (size_t i = 0; i < mesh->length(); i++)
    MPI_Bcast(&function_coefficients[i][0], function_coefficients[i].size(), MPI_DOUBLE, root, MPI_COMM_WORLD);

  if (store_derivative) {
    for (size_t i = 0; i < mesh->length(); i++)
      MPI_Bcast(&derivative_coefficients[i][0], derivative_coefficients[i].size(), MPI_DOUBLE, root, MPI_COMM_WORLD);
  }
}

template <int dim>
void DiscretizedFunction<dim>::mpi_isend(size_t destination, std::vector<MPI_Request>& reqs) {
  AssertThrow(reqs.size() == 0, ExcInternalError());

  reqs.reserve(function_coefficients.size());

  for (size_t i = 0; i < mesh->length(); i++) {
    reqs.emplace_back();
    MPI_Isend(&function_coefficients[i][0], function_coefficients[i].size(), MPI_DOUBLE, destination, 1, MPI_COMM_WORLD,
              &reqs[i]);
  }

  if (store_derivative) {
    reqs.reserve(function_coefficients.size() + derivative_coefficients.size());

    for (size_t i = 0; i < mesh->length(); i++) {
      reqs.emplace_back();
      MPI_Isend(&derivative_coefficients[i][0], derivative_coefficients[i].size(), MPI_DOUBLE, destination, 1,
                MPI_COMM_WORLD, &reqs[function_coefficients.size() + i]);
    }
  }
}

template <int dim>
void DiscretizedFunction<dim>::mpi_all_reduce(DiscretizedFunction<dim> source, MPI_Op op) {
  for (size_t i = 0; i < mesh->length(); i++)
    MPI_Allreduce(&source.function_coefficients[i][0], &function_coefficients[i][0], function_coefficients[i].size(),
                  MPI_DOUBLE, op, MPI_COMM_WORLD);

  if (store_derivative) {
    for (size_t i = 0; i < mesh->length(); i++)
      MPI_Allreduce(&source.derivative_coefficients[i][0], &derivative_coefficients[i][0],
                    derivative_coefficients[i].size(), MPI_DOUBLE, op, MPI_COMM_WORLD);
  }
}
#endif

template class DiscretizedFunction<1>;
template class DiscretizedFunction<2>;
template class DiscretizedFunction<3>;

}  // namespace base
}  // namespace wavepi


#pragma once

#include "ompu/music/music_fwd.hpp"

#include <boost/variant/variant_fwd.hpp>

#include <iosfwd>
#include <stdexcept>

namespace ompu { namespace music { namespace concrete {

using octave_type = int;

struct concrete_error : std::runtime_error
{
    using std::runtime_error::runtime_error;
};

using any_mods = saya::zed::rewrap_t<
    std::tuple,
    boost::variant,
    all_mods
>;

using any_tones = saya::zed::rewrap_t<
    std::tuple,
    boost::variant,
    all_tones
>;

#if 0
// WOW.
// this won't work due to Boost.MPL limitations

using any_keys = saya::zed::rewrap_t<
    std::tuple,
    boost::variant,
    all_keys
>;
#endif

enum struct CanonicalNoteHeight : unsigned
{
    C = 0, D = 2, E = 4, F = 5, G = 7, A = 9, B = 11,
};

using note_height_type = std::underlying_type_t<CanonicalNoteHeight>;

template<class Tone, class Mod>
inline CanonicalNoteHeight make_note_height(basic_ident<Tone, Mod>)
{
    switch (Tone::height + cvt::detail::opposite_mod_t<Mod>::offset) {
    case 0:
        return CanonicalNoteHeight::C;
    case 2:
        return CanonicalNoteHeight::D;
    case 4:
        return CanonicalNoteHeight::E;
    case 5:
        return CanonicalNoteHeight::F;
    case 7:
        return CanonicalNoteHeight::G;
    case 9:
        return CanonicalNoteHeight::A;
    case 11:
        return CanonicalNoteHeight::B;

    default:
        throw std::logic_error("unhandled note height");
    }
}

std::ostream& operator<<(std::ostream& os, CanonicalNoteHeight nh);

enum struct ModType : int
{
    none = 0, natural = 1234,
    sharp = +1, dbl_sharp = +2,
    flat = -1, dbl_flat = -2,
};

inline std::underlying_type_t<ModType>
mod_value(ModType mt) noexcept
{
    if (mt == ModType::natural) return 0;
    return static_cast<std::underlying_type_t<ModType>>(mt);
}

std::ostream& operator<<(std::ostream& os, ModType mt);

template<class Mod>
inline ModType make_mod(Mod) noexcept
{
    if (std::is_same_v<Mod, mods::none>) {
        return ModType::none;
    } else if (std::is_same_v<Mod, mods::sharp>) {
        return ModType::sharp;
    } else if (std::is_same_v<Mod, mods::flat>) {
        return ModType::flat;
    } else if (std::is_same_v<Mod, mods::dbl_sharp>) {
        return ModType::dbl_sharp;
    } else if (std::is_same_v<Mod, mods::dbl_flat>) {
        return ModType::dbl_flat;
    } else if (std::is_same_v<Mod, mods::natural>) {
        return ModType::natural;
    }
}

}}} // ompu


// External libraries
#include <boost/property_tree/ptree.hpp>

// Local libraries
#include <fmath/physics.h>
#include <fparameters/Dimension.h>
#include <fparameters/parameters.h>
#include <fparameters/SpaceIterator.h>

// Local headers
#include "State.h"
#include "globalVariables.h"
#include "adafFunctions.h"
#include "modelParameters.h"
#include "messages.h"

using namespace std;

State::State(boost::property_tree::ptree& cfg):
    photon{ "photon" },
	ntPhoton{ "ntPhoton" },
    ntProton{ "ntProton" },
    ntElectron{ "ntElectron" },
    ntNeutron{ "ntNeutron" },
	ntChargedPion{ "ntChargedPion" },
	ntMuon{ "ntMuon" },
	neutrino{ "neutrino" },
	ntPair{ "ntPair" },
	tau_gg(ntPhoton.ps, false),
    magf(ntPhoton.ps, false),
    denf_i(ntPhoton.ps, false),
    denf_e(ntPhoton.ps, false),
    tempElectrons(ntPhoton.ps, false),
    tempIons(ntPhoton.ps, false),
    thetaH(ntPhoton.ps, false),
    height(ntPhoton.ps, false)
{
    show_message(msgStart, Module_state);

	particles.push_back(&photon);
	particles.push_back(&ntPhoton);
    particles.push_back(&ntElectron);
    particles.push_back(&ntProton);
    particles.push_back(&ntNeutron);
	particles.push_back(&ntChargedPion);
	particles.push_back(&ntMuon);
	particles.push_back(&neutrino);
	particles.push_back(&ntPair);
    for (auto p : particles) {
        initializeParticle(*p, cfg);
    }
    magf.initialize();
    magf.fill([&](const SpaceIterator& i){
        double r = i.val(DIM_R);
        return magneticField(r);
    });
    denf_i.initialize();
    denf_i.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        double dens = massDensityADAF(r)/(atomicMassUnit*iMeanMolecularWeight);
		return dens;
    });
    denf_e.initialize();
    denf_e.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        return massDensityADAF(r)/(atomicMassUnit*eMeanMolecularWeight);
    });
    tempElectrons.initialize();
    tempElectrons.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        return electronTemp(r);
    });
    tempIons.initialize();
    tempIons.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        return ionTemp(r);
    });
    thetaH.initialize();
    thetaH.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        return acos(costhetaH(r));
    });
    height.initialize();
    height.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        return height_fun(r);
    });
	tau_gg.initialize();

    show_message(msgEnd, Module_state);
}

Dimension* State::createDimension(Particle& p, string dimid,
		function<void(Vector&, double, double)> initializer, function<double(double)> to_linear,  function<double(double)> from_linear, boost::property_tree::ptree& cfg)
{
	int samples = p.getpar<int>(cfg, "dim."+dimid+".samples");
	double min = p.getpar<double>(cfg, "dim."+dimid+".min");
	double max = p.getpar<double>(cfg, "dim."+dimid+".max");
	return new Dimension(samples, bind(initializer, placeholders::_1, min, max), to_linear, from_linear);
}

auto l10 = [](double x) { return (x > 0.0) ? log10(x) : -300.0;};
auto e10 = [](double x) { return exp10(x); };

void State::initializeParticle(Particle& p, boost::property_tree::ptree& cfg)
{
	using std::bind;
	p.configure(cfg.get_child("particle.default"));
	p.configure(cfg.get_child("particle."+p.id));

	// add dimension for energies
	p.ps.add(
		createDimension(
			p,
			"energy",
			initEnergyPoints,
			l10, e10,
			cfg
		)
	);

	// add dimension for r
	double innerRadius = exp(logr.front()) * schwRadius;
	double edgeRadius = exp(logr.back()) * schwRadius;
	p.ps.add(new Dimension(nR, bind(initGridLogarithmically, placeholders::_1, innerRadius, edgeRadius), l10, e10));

	// add dimension for rcd
	p.ps.add(new Dimension(nRcd,bind(initGridLogarithmically,placeholders::_1,
							rTr, rOutCD),l10,e10));

	p.initialize();
}


// vi: set et ts=4 sw=2 sts=2:

#ifndef HMAT_HMATRIX_DENSE_DATA_IMPL_HPP
#define HMAT_HMATRIX_DENSE_DATA_IMPL_HPP

#include "common.hpp"
#include <armadillo>

#include "hmatrix_dense_data.hpp"

namespace hmat {

template <typename ValueType>
void HMatrixDenseData<ValueType>::apply(const arma::Mat<ValueType> &X,
                                        arma::Mat<ValueType> &Y,
                                        TransposeMode trans, ValueType alpha,
                                        ValueType beta) const {

  arma::subview<ValueType> xsub = X.submat(arma::span::all, arma::span::all);
  arma::subview<ValueType> ysub = Y.submat(arma::span::all, arma::span::all);

  this->apply(xsub, ysub, trans, alpha, beta);
}

template <typename ValueType>
void HMatrixDenseData<ValueType>::apply(const arma::subview<ValueType> &X,
                                        arma::subview<ValueType> &Y,
                                        TransposeMode trans, ValueType alpha,
                                        ValueType beta) const {
  if (beta == ValueType(0))
    Y.zeros();
  if (alpha == ValueType(0)) {
    Y = beta * Y;
    return;
  }

  if (trans == TransposeMode::NOTRANS)
    Y = alpha * m_A * X + beta * Y;
  else if (trans == TransposeMode::TRANS)
    Y = alpha * m_A.st() * X + beta * Y;
  else if (trans == TransposeMode::CONJ)
    Y = alpha * arma::conj(m_A) * X + beta * Y;
  else
    Y = alpha * m_A.t() * X + beta * Y;
}

template <typename ValueType>
const arma::Mat<ValueType> &HMatrixDenseData<ValueType>::A() const {
  return m_A;
}

template <typename ValueType>
arma::Mat<ValueType> &HMatrixDenseData<ValueType>::A() {
  return m_A;
}

template <typename ValueType> int HMatrixDenseData<ValueType>::rows() const {
  return m_A.n_rows;
}

template <typename ValueType> int HMatrixDenseData<ValueType>::cols() const {
  return m_A.n_cols;
}

template <typename ValueType> int HMatrixDenseData<ValueType>::rank() const {
  return m_A.n_cols;
}

template <typename ValueType>
typename ScalarTraits<ValueType>::RealType
HMatrixDenseData<ValueType>::frobeniusNorm() const {
  return norm(m_A, "fro");
}

template <typename ValueType>
double HMatrixDenseData<ValueType>::memSizeKb() const {
  return sizeof(ValueType) * (this->rows()) * (this->cols()) / (1.0 * 1024);
}
}
#endif


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

#include "Geomlib_ReadOFF.h"
#include <iostream>
#include <vector>

#include <Eigen/Core>

#include <igl/readOFF.h>
#include <igl/list_to_matrix.h>

#include "ConversionUtilities.h"
#include "TriMesh.h"

using namespace Urho3D;

bool Geomlib::ReadOFF(
	const Urho3D::String& off_filename,
	Urho3D::Variant& tri_mesh,
	bool yup
)
{
	Eigen::MatrixXf V;
	Eigen::MatrixXi F;
	bool read_success = igl::readOFF(off_filename.CString(), V, F);
	if (!read_success) {
		return false;
	}

	if (yup) {
		Eigen::MatrixXf W = V;
		W.col(1) = V.col(2);
		W.col(2) = -1 * V.col(1);
		V = W;
	}

	tri_mesh = TriMesh_Make(V, F);
	if (!TriMesh_Verify(tri_mesh)) {
		return false;
	}
	return true;
}

bool Geomlib::ReadOFF(
	Urho3D::File* source,
	Urho3D::Variant& tri_mesh,
	bool yup
)
{
	using namespace std;

	std::vector<std::vector<float> > V;
	std::vector<std::vector<float> > N;
	std::vector<std::vector<int> > F;
	std::vector<std::vector<float> > C;

	// First line is always OFF
	char header[1000];
	const std::string OFF("OFF");
	const std::string NOFF("NOFF");
	const std::string COFF("COFF");

	if (source->IsEof()) return false;
	String headerString = source->ReadLine();
	strncpy(header, headerString.CString(), 999);

	if (!(
		string(header).compare(0, OFF.length(), OFF) == 0 ||
		string(header).compare(0, COFF.length(), COFF) == 0 ||
		string(header).compare(0, NOFF.length(), NOFF) == 0
		)) {
		printf("Error: Geomlib::ReadOFF() first line should be OFF or NOFF or COFF, not %s...", header);
		return false;
	}
	bool has_normals = string(header).compare(0, NOFF.length(), NOFF) == 0;
	bool has_vertexColors = string(header).compare(0, COFF.length(), COFF) == 0;
	// Second line is #vertices #faces #edges
	int number_of_vertices;
	int number_of_faces;
	int number_of_edges;
	char tic_tac_toe;
	char line[1000];
	bool still_comments = true;
	while (still_comments)
	{
		String lineString = source->ReadLine();
		strncpy(line, lineString.CString(), 999);
		//fgets(line, 1000, off_file);
		still_comments = (line[0] == '#' || line[0] == '\n');
	}
	sscanf(line, "%d %d %d", &number_of_vertices, &number_of_faces, &number_of_edges);
	V.resize(number_of_vertices);
	if (has_normals)
		N.resize(number_of_vertices);
	if (has_vertexColors)
		C.resize(number_of_vertices);
	F.resize(number_of_faces);
	//printf("%s %d %d %d\n",(has_normals ? "NOFF" : "OFF"),number_of_vertices,number_of_faces,number_of_edges);
	// Read vertices
	for (int i = 0; i<number_of_vertices;)
	{
		char line[1000];
		String lineString = source->ReadLine();
		strncpy(line, lineString.CString(), 999);
		double x, y, z, nx, ny, nz;
		if (sscanf(line, "%lg %lg %lg %lg %lg %lg", &x, &y, &z, &nx, &ny, &nz) >= 3)
		{
			std::vector<float > vertex;
			vertex.resize(3);
			vertex[0] = x;
			vertex[1] = y;
			vertex[2] = z;
			V[i] = vertex;

			if (has_normals)
			{
				std::vector<float > normal;
				normal.resize(3);
				normal[0] = nx;
				normal[1] = ny;
				normal[2] = nz;
				N[i] = normal;
			}

			if (has_vertexColors)
			{
				C[i].resize(3);
				C[i][0] = nx / 255.0;
				C[i][1] = ny / 255.0;
				C[i][2] = nz / 255.0;
			}
			i++;
		}
		else if (
			sscanf(line, "%[#]", &tic_tac_toe) >= 1)
		{
			// this line is a comment
		}
		else
		{
			printf("Error: bad line (%d)\n", i);
			if (source->IsEof())
			{
				return false;
			}
		}
	}
	// Read faces
	for (int i = 0; i<number_of_faces;)
	{
		char line[1000];
		String lineString = source->ReadLine();
		strncpy(line, lineString.CString(), 999);

		std::vector<int > face;
		int valence;
		if (sscanf(line, "%d", &valence) == 1)
		{
			face.resize(valence);

			if (valence == 3) {
				int val, i0, i1, i2;
				if (sscanf(line, "%d %d %d %d", &val, &i0, &i1, &i2) > 0) {
					face[0] = i0;
					face[1] = i1;
					face[2] = i2;
				}
			}
			else if (valence == 4) {
				int val, i0, i1, i2, i3;
				if (sscanf(line, "%d %d %d %d %d", &val, &i0, &i1, &i2, &i3) > 0) {
					face[0] = i0;
					face[1] = i1;
					face[2] = i2;
					face[3] = i3;
				}
			}

			F[i] = face;
			i++;
		}
		else if (
			sscanf(line, "%[#]", &tic_tac_toe) >= 1)
		{
			// this line is a comment
		}
		else
		{
			printf("Error: bad line\n");
			return false;
		}
	}

	const char* format = "Failed to cast %s to matrix: min (%d) != max (%d)\n";

	Eigen::MatrixXf xV; // xV = matrix V
	bool V_rect = igl::list_to_matrix(V, xV);
	if (!V_rect) {
		printf(format, "xV", igl::min_size(V), igl::max_size(V));
		return false;
	}

	Eigen::MatrixXi xF; // xF = matrix F
	bool F_rect = igl::list_to_matrix(F, xF);
	if (!F_rect) {
		printf(format, "xF", igl::min_size(F), igl::max_size(F));
		return false;
	}

	// xV, xF record the mesh at this stage

	if (yup) {
		Eigen::MatrixXf xW = xV;
		xW.col(1) = xV.col(2);
		xW.col(2) = -1 * xV.col(1);
		xV = xW;
	}

	tri_mesh = TriMesh_Make(xV, xF);
	if (!TriMesh_Verify(tri_mesh)) {
		return false;
	}

	return true;
}


/**************************************************************************
Copyright (c) 2012, Julio C. Estrada
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

+ Redistributions of source code must retain the above copyright notice,
  this list of conditions and the following disclaimer.

+ Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**************************************************************************/

#include <imcore/scanner.h>
#include <utils/utils.h>
#include <imcore/unwrap.h>
#include <string>
#include <iostream>
#include <fstream>
#include <cstdlib>
#include <Eigen/Dense>
#include <CImg.h>

using namespace std;

inline
void imshow(cimg_library::CImgDisplay& disp, const Eigen::ArrayXXf& im)
{
  const cimg_library::CImg<float> img(im.data(), im.cols(),
                                      im.rows(), 1, 1, true);

  disp.display(img);
}

inline
Eigen::ArrayXXf readFltFile(const char* fname)
{
  ifstream file;
  file.open(fname, ios::in);
  float M=0, N=0;
  float val;
  Eigen::ArrayXXf dat((int)M,(int)N);
  //dat.resize(M,N);

  if(file.is_open()){

    file>>M;
    file>>N;
    dat.resize((int)M,(int)N);

    for(int i=0; i<M; i++)
      for(int j=0; j<N;j++){
        file>>val;
        dat(i,j)=val;
      }
  }

  cout<<"("<<M << ", "<< N<<")"<<endl;
  return dat;
}

inline
void writeFltFile(const Eigen::ArrayXXf& mat, const char* fname)
{
  ofstream file;
  file.open(fname, ios::out);
  float M=mat.rows(), N=mat.cols();
  float val;
  file<<M<<endl;
  file<<N<<endl;

  for(int i=0; i<M; i++)
    for(int j=0; j<N;j++){
      val=mat(i,j);
      file<<val<<endl;
    }
}

int main(int argc, char* argv[])
{
  using namespace cimg_library;
  Eigen::ArrayXXf wphase;
  Eigen::ArrayXXf uphase;
  Eigen::ArrayXXi visited;
  Eigen::ArrayXXi mask;
  Eigen::ArrayXXf path, dx, dy;
  Eigen::Array2i pixel;

  std::string mfile, phasefile, outfile;

  cimg_usage("Command line argumments");
  const float tau = cimg_option("-t", 0.2,
                                "Bandwidth of the unwrapping system");
  const float sigma = cimg_option("-s", 13,
                                  "Smooth power to generate the scanning path");
  const int N = cimg_option("-N", 9, "Window size of the phase unwrapper");
  const int x =cimg_option("-x", 338, "x-position of starting pixel");
  const int y =cimg_option("-y", 400, "y-position of starting pixel");
  mfile = cimg_option("-m", "no_input_file",
                      "File name of the mask to select region of interest");
  phasefile = cimg_option("-i" , "no_input_file",
                          "Wrapped phase to be processed");
  outfile = cimg_option("-o", "output.flt",
                        "Output file where result is saved");

  /*
  if(phasefile == "no_input_file"){
    cout<< "You must indicate an input file using option -i"
        << endl;
    return 1;
  }
  */
  if(mfile == "no_input_file"){
    mfile="";
  }

  //Eigen::ArrayXXf image = cv::imread(argv[1], 0);
  Eigen::ArrayXXf image = readFltFile(phasefile.c_str());
  if(image.cols()==0 and image.rows()==0){
    cerr<<"Error: file name "<<phasefile<<" can not be opened." << endl;
    return 1;
  }
  if(mfile != ""){
    CImg<int> cimg_mask(mfile.c_str());
    Eigen::Map<Eigen::ArrayXXi> data(cimg_mask.data(), cimg_mask.height(),
                                     cimg_mask.width());
    if(cimg_mask.is_empty()){
      cerr<<"Error: file name "<<phasefile<<" can not be opened." << endl;
      return 1;
    }
    if(mask.rows()!=image.rows() || mask.cols()!=image.cols()){
      cerr<<"Error: Mask dimensions must match image dimensions." << endl;
      return 1;
    }
    mask = data;
  }
  else
    mask = Eigen::ArrayXXi::Constant(image.rows(), image.cols(), 1);

  pixel(0) = x;
  pixel(1)= y;
  wphase.resize(image.rows(), image.cols());
  float a = image.minCoeff(), b= image.maxCoeff();
  float range = b-a;
  wphase = 2*M_PI*(image-a)/range - M_PI;

  a = mask.minCoeff();
  b = mask.maxCoeff();
  range = b-a;
  if(range != 0)
    mask = (mask-a)/range;

  visited= Eigen::ArrayXXi::Zero(image.rows(), image.cols());
  uphase = Eigen::ArrayXXf::Zero(image.rows(), image.cols());

  Unwrap unwrap(wphase, tau, sigma, N);
  unwrap.setPixel(pixel);
  unwrap.setMask(mask);

  int iter=0;
  unwrap.runInteractive();

  CImgDisplay disp_uphase(wphase.cols(), wphase.rows(), "Uphase");

  do{
    if(iter++ % 3000 ==0){
      uphase = unwrap.getOutput();
      imshow(disp_uphase, uphase);
      disp_uphase.wait(32);
    }
  }while(unwrap.runInteractive());

  /*
  unwrap.setPixel(pixel);
  unwrap.runInteractive();
  unwrap.setTao(0.05);
  iter=0;

  do{
    if(iter++ % 9000 ==0){
      imshow("phase", uphase);
      cv::waitKey(32);
    }
  }while(unwrap.runInteractive());
  std::cout<<"Number of pixels: "<< iter<<std::endl;
  */

  writeFltFile(uphase, outfile.c_str());

  CImgDisplay disp_wphase(wphase.cols(), wphase.rows(), "WPhase");
  CImgDisplay disp_path(wphase.cols(), wphase.rows(), "Path");
  path = unwrap.genPath(sigma);

  imshow(disp_uphase, uphase);
  imshow(disp_wphase, wphase);
  imshow(disp_path, path);

  while(!disp_path.is_closed() && !disp_uphase.is_closed()
        && !disp_wphase.is_closed()){
    disp_uphase.wait_all();
  }

  return 0;
}


/*!
  \file gpp_knowledge_gradient_inner_optimization_test.cpp
  \rst
  Routines to test the functions in gpp_knowledge_gradient_optimization.cpp.

  The tests verify KnowledgeGradientEvaluator, and KG optimization from gpp_knowledge_gradient_optimization.cpp.

  1. Ping testing (verifying analytic gradient computation against finite difference approximations)

     a. Following gpp_covariance_test.cpp, we define class PingKnowledgeGradient for
        evaluating those functions + their spatial gradients.

     b. Ping for derivative accuracy (PingGPComponentTest, PingEITest); these unit test the analytic derivatives.

  2. Monte-Carlo KG vs analytic KG validation: the monte-carlo versions are run to "high" accuracy and checked against
     analytic formulae when applicable
  3. Gradient Descent: using polynomials and other simple fucntions with analytically known optima
     to verify that the algorithm(s) underlying KG optimization are performing correctly.
  4. Single-threaded vs multi-threaded KG optimization validation: single and multi-threaded runs are checked to have the same
     output.
  5. End-to-end test of the KG optimization process for the analytic and monte-carlo cases.  These tests use constructed
     data for inputs but otherwise exercise the same code paths used for KG optimization in production.
\endrst*/

// #define OL_VERBOSE_PRINT
#include "gpp_knowledge_gradient_inner_optimization_test.hpp"

#include <cmath>

#include <algorithm>
#include <limits>
#include <vector>

#include <boost/random/uniform_real.hpp>  // NOLINT(build/include_order)

#include "gpp_common.hpp"
#include "gpp_covariance.hpp"
#include "gpp_geometry.hpp"
#include "gpp_linear_algebra.hpp"
#include "gpp_logging.hpp"
#include "gpp_math.hpp"
#include "gpp_random.hpp"
#include "gpp_test_utils.hpp"
#include "gpp_knowledge_gradient_inner_optimization.hpp"
#include "gpp_optimizer_parameters.hpp"

namespace optimal_learning {

namespace {  // contains classes/routines for ping testing

/*!\rst
  Supports evaluating the knowledge gradient, KnowledgeGradientEvaluator::ComputeKnowledgeGradient() and
  its gradient, KnowledgeGradientEvaluator::ComputeGradKnowledgeGradient()

  The gradient is taken wrt ``points_to_sample[dim]``, so this is the ``input_matrix``, ``X_{d,i}``.
  The other inputs to KG are not differentiated against, so they are taken as input and stored by the constructor.

  The output of KG is a scalar.
\endrst*/
class PingFuturePosteriorMean final : public PingableMatrixInputVectorOutputInterface {
 public:
  constexpr static char const * const kName = "Posterior Mean";

  PingFuturePosteriorMean(double const * restrict lengths, double const * restrict points_sampled, double const * restrict points_sampled_value,
                          int const * restrict gradients, double const * restrict points_to_sample,
                          double alpha, int dim, int num_to_sample, int num_sampled, int num_gradients) OL_NONNULL_POINTERS
      : dim_(dim),
        num_to_sample_(num_to_sample),
        num_sampled_(num_sampled),
        num_gradients_(num_gradients),
        gradients_already_computed_(false),
        gradients_(gradients, gradients + num_gradients),
        noise_variance_(1+num_gradients, 0.1),
        points_sampled_(points_sampled, points_sampled + dim_*num_sampled_),
        points_to_sample_(points_to_sample, points_to_sample + dim_*num_to_sample_),
        points_sampled_value_(points_sampled_value, points_sampled_value + num_sampled_*(1+num_gradients_)),
        grad_PS_(dim_*num_to_sample_),
        sqexp_covariance_(dim_, alpha, lengths),
        gaussian_process_(sqexp_covariance_, points_sampled_.data(), points_sampled_value_.data(), noise_variance_.data(),
                          gradients_.data(), num_gradients_, dim_, num_sampled_),
        normals_(normals(num_to_sample, num_gradients)),
        chol_(chol()),
        train_sample_(train()),
        ps_evaluator_(gaussian_process_, normals_.data(), points_to_sample_.data(), num_to_sample_, gradients_.data(), num_gradients_,
                      chol_.data(), train_sample_.data()) {
  }

  std::vector<double> normals(int num_to_sample, int num_gradients) {
     std::vector<double> randomDiscrete(num_to_sample*(1+num_gradients));
     UniformRandomGenerator uniform_generator(318);
     boost::uniform_real<double> uniform_double(-5.0, 5.0);
     for (int i = 0; i < num_to_sample*(1+num_gradients); ++i) {
       randomDiscrete[i] = uniform_double(uniform_generator.engine);
     }
     return randomDiscrete;
  }

  std::vector<double> chol() {
     std::vector<double> chol_(Square(num_to_sample_*(1+num_gradients_)));
     PointsToSampleState points_to_sample_state(gaussian_process_, points_to_sample_.data(),
                                                num_to_sample_, gradients_.data(), num_gradients_, 0);
     gaussian_process_.ComputeVarianceOfPoints(&points_to_sample_state, gradients_.data(),
                                                num_gradients_, chol_.data());
     int leading_minor_index = ComputeCholeskyFactorL(num_to_sample_*(1+num_gradients_), chol_.data());
     ZeroUpperTriangle(num_to_sample_*(1+num_gradients_), chol_.data());
     return chol_;
  }

  std::vector<double> train() {
     std::vector<double> randomDiscrete(num_to_sample_*Square(1+num_gradients_)*num_sampled_);
     gaussian_process_.ComputeTrain(points_to_sample_.data(), num_to_sample_, gradients_.data(), num_gradients_, randomDiscrete.data());
     return randomDiscrete;
  }

  virtual void GetInputSizes(int * num_rows, int * num_cols) const noexcept override OL_NONNULL_POINTERS {
    *num_rows = dim_;
    *num_cols = 1;
  }

  virtual int GetGradientsSize() const noexcept override OL_WARN_UNUSED_RESULT {
    return dim_*GetOutputSize()*1;
  }

  virtual int GetOutputSize() const noexcept override OL_WARN_UNUSED_RESULT {
    return 1;
  }

  virtual void EvaluateAndStoreAnalyticGradient(double const * restrict points_to_sample, double * restrict gradients) noexcept override OL_NONNULL_POINTERS_LIST(2) {
    if (gradients_already_computed_ == true) {
      OL_WARNING_PRINTF("WARNING: grad_KG data already set.  Overwriting...\n");
    }
    gradients_already_computed_ = true;

    NormalRNG normal_rng(3141);
    bool configure_for_gradients = true;

    FuturePosteriorMeanEvaluator::StateType ps_state(ps_evaluator_, 0, points_to_sample, configure_for_gradients);

    ps_evaluator_.ComputeGradPosteriorMean(&ps_state, grad_PS_.data());

    if (gradients != nullptr) {
      std::copy(grad_PS_.begin(), grad_PS_.end(), gradients);
    }
  }

  virtual double GetAnalyticGradient(int row_index, int column_index, int OL_UNUSED(output_index)) const override OL_WARN_UNUSED_RESULT {
    if (gradients_already_computed_ == false) {
      OL_THROW_EXCEPTION(OptimalLearningException, "PingPosteriorMean::GetAnalyticGradient() called BEFORE EvaluateAndStoreAnalyticGradient. NO DATA!");
    }

    return grad_PS_[column_index*dim_ + row_index];
  }

  virtual void EvaluateFunction(double const * restrict points_to_sample, double * restrict function_values) const noexcept override OL_NONNULL_POINTERS {
    bool configure_for_gradients = false;

    FuturePosteriorMeanEvaluator::StateType ps_state(ps_evaluator_, 0, points_to_sample, configure_for_gradients);
    *function_values = ps_evaluator_.ComputePosteriorMean(&ps_state);
  }

 private:
  //! spatial dimension (e.g., entries per point of ``points_sampled``)
  int dim_;
  //! number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-EI)
  int num_to_sample_;
  //! number of points in ``points_sampled``
  int num_sampled_;
  //! number of derivatives' observations.
  int num_gradients_;

  //! whether gradients been computed and stored--whether this class is ready for use
  bool gradients_already_computed_;
  // indices of the derivatives' observations.
  std::vector<int> gradients_;

  //! ``\sigma_n^2``, the noise variance
  std::vector<double> noise_variance_;
  //! coordinates of already-sampled points, ``X``
  std::vector<double> points_sampled_;
  std::vector<double> points_to_sample_;
  //! function values at points_sampled, ``y``
  std::vector<double> points_sampled_value_;
  //! the gradient of KG at union_of_points, wrt union_of_points[0:num_to_sample]
  std::vector<double> grad_PS_;

  //! covariance class (for computing covariance and its gradients)
  SquareExponential sqexp_covariance_;
  //! gaussian process used for computations
  GaussianProcess gaussian_process_;

  std::vector<double> normals_;
  std::vector<double> chol_;
  std::vector<double> train_sample_;

  //! expected improvement evaluator object that specifies the parameters & GP for KG evaluation
  FuturePosteriorMeanEvaluator ps_evaluator_;

  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(PingFuturePosteriorMean);
};

/*!\rst
  Pings the gradients (spatial) of the KG 50 times with randomly generated test cases
  Works with MC formulae

  \param
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-KG)
    :num_being_sampled: number of points being sampled in concurrent experiments (i.e., the "p" in q,p-KG)
    :epsilon: coarse, fine ``h`` sizes to use in finite difference computation
    :tolerance_fine: desired amount of deviation from the exact rate
    :tolerance_coarse: maximum allowable abmount of deviation from the exact rate
    :input_output_ratio: for ``||analytic_gradient||/||input|| < input_output_ratio``, ping testing is not performed, see PingDerivative()
  \return
    number of ping/test failures
\endrst*/
template <typename PSEvaluator>
OL_WARN_UNUSED_RESULT int PingPSTest(int num_to_sample, double epsilon[2], double tolerance_fine, double tolerance_coarse, double input_output_ratio) {
  int total_errors = 0;
  int errors_this_iteration;
  const int dim = 3;

  int num_sampled = 7;

  int * gradients = new int[3]{0, 1, 2};
  int num_gradients = 3;

  std::vector<double> lengths(dim);
  double alpha = 2.80723;

  MockExpectedImprovementEnvironment KG_environment;

  UniformRandomGenerator uniform_generator(2718);
  boost::uniform_real<double> uniform_double(0.5, 2.5);

  for (int i = 0; i < 50; ++i) {
    KG_environment.Initialize(dim, num_to_sample, 3, num_sampled, num_gradients);
    for (int j = 0; j < dim; ++j) {
      lengths[j] = uniform_double(uniform_generator.engine);
    }

    PSEvaluator PS_evaluator(lengths.data(), KG_environment.points_sampled(),
                             KG_environment.points_sampled_value(), gradients, KG_environment.points_being_sampled(),
                             alpha, KG_environment.dim, KG_environment.num_being_sampled, KG_environment.num_sampled, num_gradients);

    //KGEvaluator KG_evaluator(lengths.data(), KG_environment.points_sampled(), KG_environment.points_sampled_value(), alpha, KG_environment.dim, KG_environment.num_to_sample, KG_environment.num_sampled);
    PS_evaluator.EvaluateAndStoreAnalyticGradient(KG_environment.points_to_sample(), nullptr);

    errors_this_iteration = PingDerivative(PS_evaluator, KG_environment.points_to_sample(), epsilon, tolerance_fine, tolerance_coarse, input_output_ratio);

    if (errors_this_iteration != 0) {
      OL_PARTIAL_FAILURE_PRINTF("on iteration %d\n", i);
    }
    total_errors += errors_this_iteration;
  }

  if (total_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("%s (%d,%d-PS) gradient pings failed with %d errors\n", PSEvaluator::kName, num_to_sample, 0, total_errors);
  } else {
    OL_PARTIAL_SUCCESS_PRINTF("%s (%d,%d-PS) gradient pings passed\n", PSEvaluator::kName, num_to_sample, 0);
  }
  delete [] gradients;
  return total_errors;
};

}  // end unnamed namespace

/*!\rst
  Wrapper to ping the gradients (spatial) of the inverse of the cholesky factorization with noise.

  \return
    number of ping/test failures
\endrst*/

int PingKGInnerGeneralTest() {
  double epsilon_KG[2] = {1.0e-3, 1.0e-4};

  int total_errors = PingPSTest<PingFuturePosteriorMean>(1, epsilon_KG, 9.0e-2, 3.0e-1, 1.0e-18);

  return total_errors;
}

int RunKGInnerTests() {
  int total_errors = 0;
  int current_errors = 0;

  {
    current_errors = PingKGInnerGeneralTest();
    if (current_errors != 0) {
      OL_PARTIAL_FAILURE_PRINTF("pinging inner KG failed with %d errors\n", current_errors);
    }
    total_errors += current_errors;
  }

  if (total_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("Inner KG functions failed with %d errors\n\n", total_errors);
  } else {
    OL_PARTIAL_SUCCESS_PRINTF("Inner KG functions passed\n");
  }

  return total_errors;
}
}  // end namespace optimal_learning


#ifndef HITTABLE_HH
#define HITTABLE_HH

#include <Eigen/Dense>

#include "ray.hh"

namespace hittable
{

struct hitRecord
{
    bool didHit{false};
    double t{0};
    Eigen::Vector3d p;
    Eigen::Vector3d norm;
    bool frontFace{false};

    inline void setFaceNormal(const ray::Ray& ray,
                              const Eigen::Vector3d& outwardNormal)
    {
        frontFace = ray.direction().dot(outwardNormal) < 0;
        norm = frontFace ? outwardNormal : -outwardNormal;
    }
};

class Hittable
{
  public:
    virtual ~Hittable();

    [[nodiscard]] virtual auto
    hit(const ray::Ray& r, double t_min = 0,
        double t_max = std::numeric_limits<double>::infinity()) const
        -> hitRecord = 0;
};

} // namespace hittable

#endif // HITTABLE_HH


#ifndef __UTILS_HPP__
#define __UTILS_HPP__

#include <iostream>
#include <math.h>
#include <mpi.h>
#include <algorithm>
#include <type_traits>
#include "../common.hpp"
#include <armadillo>
#include <iomanip>

extern "C" {
  void tic(const char* s);
  void toc(const char* s);
  void show_time(const char* s);
  void show_all();
}

namespace oa {
  namespace utils {

    void set_disp_format(int flag);
    int& get_disp_format();

    template<class T>
    void print_data_t(T* buf, const Shape& shape, int is, int ie, int js, int je, int ks, int ke) {
      const int M = shape[0];
      const int N = shape[1];
      const int P = shape[2];
      const int buf_size = M * N * P;
      bool printlocal = false;
      if(is!=-1&&ie!=-1&&js!=-1&&je!=-1&&ks!=-1&&ke!=-1) printlocal = true;

      std::string frt_str;
      if (std::is_same<T, int>::value) {
        frt_str = "%11d";
      } else if (std::is_same<T, float>::value
        || std::is_same<T, double>::value) {
        if(get_disp_format() == 1)
          frt_str = "%20.15f";
        else
          frt_str = "%12.4f";
      } else if (std::is_same<T, bool>::value) {
        frt_str = "%4d";
      }

      static auto abs_compare = [](T a, T b) {
        return std::abs(a) < std::abs(b);
      };

      T val = 1;

      if (std::is_same<T, float>::value ||
        std::is_same<T, double>::value) {
        T max = *std::max_element(buf, buf+buf_size, abs_compare);

        if(std::abs(max) > 0.1){

          int factor = (int)log10(std::abs(max));

          if (factor > 2) {
            std::cout<< " * 1E"<<factor<<std::endl;
            val  = val / pow(10, factor);
          }
        } else if (std::abs(max) < 0.001 && std::abs(max) > 0) {

          int factor = (int)log10(std::abs(max));

          if (factor <= -3) {
            std::cout<< " * 1E"<<factor<<std::endl;
            val  = val / pow(10, factor);
          }
        }

        //std::cout<<"min = "<< max << std::endl;
        //std::cout<<"factor = "<<factor << std::endl;
      }

      //for (int i = 0; i < 24; i++) std::cout<<buf[i]<<" "<<std::endl;

      for(int k = 0; k < P; ++k) {
        if(printlocal && (k < ks || k > ke)) continue;
        std::cout<<"[k = " << k << "]" << std::endl;
        std::cout<<"      "<<std::setw(1+(int)log10(M))<<"";
        for(int j = 0; j < N; ++j) {
          if(printlocal && (j < js || j > je)) continue;
          std::cout<<"j = "<<std::setiosflags(std::ios::left)<<std::setw(16)<<j;
        }
        std::cout<<std::endl;
        for(int i = 0; i < M; ++i) {
          if(printlocal && (i < is || i > ie)) continue;
          std::cout<<"i = "<<std::setw(1+(int)log10(M))<<i;
          for(int j = 0; j < N; ++j) {
            if(printlocal && (j < js || j > je)) continue;
            printf(frt_str.c_str(), buf[i + j * M + k * M * N] * val);
          }
          std::cout<<std::endl;
        }
      }
    }

    template <typename T>
    DataType to_type() {
      if (std::is_same<T, int>::value) return DATA_INT;
      if (std::is_same<T, float>::value) return DATA_FLOAT;
      if (std::is_same<T, double>::value) return DATA_DOUBLE;
      return DATA_UNKNOWN;
    }

    template<class T>
    arma::Cube<T> make_cube(const Shape& shape, void* buf = NULL) {
      if (buf != NULL) {
        return arma::Cube<T>((T*)buf, shape[0], shape[1], shape[2],
          false, false);
      }
      return arma::Cube<T>(shape[0], shape[1], shape[2]);
    };

    template<class T>
    arma::Col<T> make_vec(int size, T* buf = NULL) {
      if (buf != NULL) {
        return arma::Col<T>((T*)buf, size, false, false);
      }
      return arma::Col<T>(size);
    };

    template<class T>
    struct dtype {
      const static DataType type = -1;
      const static int size = -1;
      static MPI_Datatype mpi_type() {
        if (std::is_same<T, int>::value) {
          return MPI_INT;
        } else if (std::is_same<T, float>::value) {
          return MPI_FLOAT;
        } else if (std::is_same<T, double>::value) {
          return MPI_DOUBLE;
        } else {
          return MPI_DATATYPE_NULL;
        }
      }
    };

    template<>
    struct dtype<bool>{
      const static DataType type = DATA_BOOL;
      const static int size = sizeof(bool);
    };

    template<>
    struct dtype<int>{
      const static DataType type = DATA_INT;
      const static int size = sizeof(int);
    };

    template<>
    struct dtype<float>{
      const static DataType type = DATA_FLOAT;
      const static int size = sizeof(float);
    };

    template<>
    struct dtype<double>{
      const static DataType type = DATA_DOUBLE;
      const static int size = sizeof(double);
    };

    DataType cast_data_type(DataType t1, DataType t2);

    //! display array for a buffer.
    void print_data(void* buf, const Shape& shape, DATA_TYPE dt, int is = -1, int ie = -1, int js = -1, int je = -1, int ks = -1, int ke = -1);

    int data_size(int data_type);

    MPI_Datatype mpi_datatype(int t);

    bool is_equal_shape(const Shape& u, const Shape& v);

    //convert a given datatype to a string
    std::string get_type_string(DataType t);

    int get_shape_dimension(Shape S);

    bool check_legal_shape_calc(Shape u, Shape v);

  }
}

#endif


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"
#include "graph_exceptions.hh"

#include <boost/lambda/bind.hpp>

#include "graph_sfdp.hh"
#include "random.hh"
#include "hash_map_wrap.hh"

using namespace std;
using namespace boost;
using namespace graph_tool;

void sfdp_layout(GraphInterface& g, boost::any pos, boost::any vweight,
                 boost::any eweight, boost::any pin, python::object spring_parms,
                 double theta, double init_step, double step_schedule,
                 size_t max_level, double epsilon, size_t max_iter,
                 bool adaptive, bool verbose, rng_t& rng)
{
    typedef UnityPropertyMap<int,GraphInterface::vertex_t> vweight_map_t;
    typedef UnityPropertyMap<int,GraphInterface::edge_t> eweight_map_t;
    typedef mpl::push_back<vertex_scalar_properties, vweight_map_t>::type
        vertex_props_t;
    typedef mpl::push_back<edge_scalar_properties, eweight_map_t>::type
        edge_props_t;

    typedef vprop_map_t<int32_t>::type group_map_t;

    double C = python::extract<double>(spring_parms[0]);
    double K = python::extract<double>(spring_parms[1]);
    double p = python::extract<double>(spring_parms[2]);
    double gamma = python::extract<double>(spring_parms[3]);
    double mu = python::extract<double>(spring_parms[4]);
    double mu_p = python::extract<double>(spring_parms[5]);
    group_map_t groups =
        any_cast<group_map_t>(python::extract<any>(spring_parms[6]));

    if(vweight.empty())
        vweight = vweight_map_t();
    if(eweight.empty())
        eweight = eweight_map_t();

    typedef vprop_map_t<uint8_t>::type pin_map_t;
    pin_map_t pin_map = any_cast<pin_map_t>(pin);

    run_action<graph_tool::detail::never_directed>()
        (g,
         std::bind(get_sfdp_layout(C, K, p, theta, gamma, mu, mu_p, init_step,
                                   step_schedule, max_level, epsilon,
                                   max_iter, adaptive),
                   std::placeholders::_1, std::placeholders::_2,
                   std::placeholders::_3, std::placeholders::_4,
                   pin_map.get_unchecked(num_vertices(g.get_graph())),
                   groups.get_unchecked(num_vertices(g.get_graph())), verbose,
                   std::ref(rng)),
         vertex_floating_vector_properties(), vertex_props_t(), edge_props_t())
        (pos, vweight, eweight);
}

struct do_propagate_pos
{
    template <class Graph, class CoarseGraph, class VertexMap, class PosMap,
              class RNG>
    void operator()(Graph& g, CoarseGraph& cg, VertexMap vmap,
                    boost::any acvmap, PosMap pos, boost::any acpos,
                    double delta, RNG& rng) const
    {
        typename PosMap::checked_t cpos =
            any_cast<typename PosMap::checked_t>(acpos);
        typename VertexMap::checked_t cvmap =
            any_cast<typename VertexMap::checked_t>(acvmap);
        typedef typename property_traits<VertexMap>::value_type c_t;
        typedef typename property_traits<PosMap>::value_type pos_t;
        typedef typename pos_t::value_type val_t;

        uniform_real_distribution<val_t> noise(-delta, delta);
        gt_hash_map<c_t, pos_t> cmap;

        for (auto v : vertices_range(cg))
            cmap[cvmap[v]] = cpos[v];

        for (auto v : vertices_range(g))
        {
            pos[v] = cmap[vmap[v]];

            if (delta > 0)
            {
                for (size_t j = 0; j < pos[v].size(); ++j)
                    pos[v][j] += noise(rng);
            }
        }
    }
};

void propagate_pos(GraphInterface& gi, GraphInterface& cgi, boost::any vmap,
                   boost::any cvmap, boost::any pos, boost::any cpos,
                   double delta, rng_t& rng)
{
    typedef mpl::vector<property_map_type::apply
                            <int32_t,
                             GraphInterface::vertex_index_map_t>::type>::type
        vmaps_t;

    gt_dispatch<>()
        (std::bind(do_propagate_pos(),
                       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
                       cvmap, std::placeholders::_4, cpos, delta, std::ref(rng)),
         all_graph_views(), all_graph_views(),
         vmaps_t(), vertex_floating_vector_properties())
        (gi.get_graph_view(), cgi.get_graph_view(), vmap, pos);
}

struct do_propagate_pos_mivs
{
    template <class Graph, class MIVSMap, class PosMap,
              class RNG>
    void operator()(Graph& g, MIVSMap mivs, PosMap pos, double delta, RNG& rng) const
    {
        typedef typename property_traits<PosMap>::value_type pos_t;
        typedef typename pos_t::value_type val_t;

        uniform_real_distribution<val_t> noise(-delta, delta);

        for (auto v : vertices_range(g))
        {
            if (mivs[v])
                continue;
            size_t count = 0;
            for (auto a : adjacent_vertices_range(v, g))
            {
                if (!mivs[a])
                    continue;
                pos[v].resize(pos[a].size(), 0);
                for (size_t j = 0; j < pos[a].size(); ++j)
                    pos[v][j] += pos[a][j];
                ++count;
            }

            if (count == 0)
                throw ValueException("invalid MIVS! Vertex has no neighbors "
                                     "belonging to the set!");

            if (count == 1)
            {
                if (delta > 0)
                {
                    for (size_t j = 0; j < pos[v].size(); ++j)
                        pos[v][j] += noise(rng);
                }
            }
            else
            {
                for (size_t j = 0; j < pos[v].size(); ++j)
                    pos[v][j] /= count;
            }
        }
    }
};

void propagate_pos_mivs(GraphInterface& gi, boost::any mivs, boost::any pos,
                        double delta, rng_t& rng)
{
    run_action<>()
        (gi, std::bind(do_propagate_pos_mivs(),
                       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
                       delta, std::ref(rng)),
         vertex_scalar_properties(), vertex_floating_vector_properties())
        (mivs, pos);
}

struct do_avg_dist
{
    template <class Graph, class PosMap>
    void operator()(Graph& g, PosMap pos, double& ad) const
    {
        size_t count = 0;
        double d = 0;
        #pragma omp parallel if (num_vertices(g) > OPENMP_MIN_THRESH) \
            reduction(+: d, count)
        parallel_vertex_loop_no_spawn
                (g,
                 [&](auto v)
                 {
                     for (auto a : adjacent_vertices_range(v, g))
                     {
                         d += dist(pos[v], pos[a]);
                         count++;
                     }
                 });
        if (count > 0)
            d /= count;
        ad = d;
    }
};

double avg_dist(GraphInterface& gi, boost::any pos)
{
    double d;
    run_action<>()
        (gi, std::bind(do_avg_dist(), std::placeholders::_1,
                       std::placeholders::_2, std::ref(d)),
         vertex_scalar_vector_properties()) (pos);
    return d;
}

struct do_sanitize_pos
{
    template <class Graph, class PosMap>
    void operator()(Graph& g, PosMap pos) const
    {
        parallel_vertex_loop
                (g,
                 [&](auto v)
                 {
                     pos[v].resize(2);
                 });
    }
};

void sanitize_pos(GraphInterface& gi, boost::any pos)
{
    run_action<>()
        (gi, std::bind(do_sanitize_pos(), std::placeholders::_1, std::placeholders::_2),
         vertex_scalar_vector_properties()) (pos);
}

#include <boost/python.hpp>

void export_sfdp()
{
    python::def("sfdp_layout", &sfdp_layout);
    python::def("propagate_pos", &propagate_pos);
    python::def("propagate_pos_mivs", &propagate_pos_mivs);
    python::def("avg_dist", &avg_dist);
    python::def("sanitize_pos", &sanitize_pos);
}


// Copyright (C) 2015 National ICT Australia (NICTA)
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
// -------------------------------------------------------------------
//
// Written by Conrad Sanderson - http://conradsanderson.id.au

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("fn_conj_1")
  {
  vec re =   linspace<vec>(1,5,6);
  vec im = 2*linspace<vec>(1,5,6);

  cx_vec a = cx_vec(re,im);
  cx_vec b = conj(a);

  REQUIRE( accu(abs(real(b) - ( re))) == Approx(0.0) );
  REQUIRE( accu(abs(imag(b) - (-im))) == Approx(0.0) );
  }

TEST_CASE("fn_conj2")
  {
  cx_mat A = randu<cx_mat>(5,6);

  cx_mat B = conj(A);

  REQUIRE( all(vectorise(real(B) ==  real(A))) == true );
  REQUIRE( all(vectorise(imag(B) == -imag(A))) == true );
  }


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_NELDER_MEAD_OPT_TRIANGULATION_HPP
#define PIC_COMPUTER_VISION_NELDER_MEAD_OPT_TRIANGULATION_HPP

#include "../util/matrix_3_x_3.hpp"
#include "../util/nelder_mead_opt_base.hpp"

#ifndef PIC_DISABLE_EIGEN
#ifndef PIC_EIGEN_NOT_BUNDLED
   #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif
#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

class NelderMeadOptTriangulation: public NelderMeadOptBase<double>
{
public:

    std::vector< Eigen::Matrix34d > M;
    std::vector< Eigen::Vector2f > p;

    /**
     * @brief NelderMeadOptTriangulation
     * @param M0
     * @param M1
     */
    NelderMeadOptTriangulation(Eigen::Matrix34d &M0, Eigen::Matrix34d &M1) : NelderMeadOptBase()
    {
        this->M.push_back(M0);
        this->M.push_back(M1);
    }

    /**
     * @brief NelderMeadOptTriangulation
     * @param M0
     * @param M1
     */
    NelderMeadOptTriangulation(std::vector< Eigen::Matrix34d> &M) : NelderMeadOptBase()
    {
        this->M.assign(M.begin(), M.end());
    }

    /**
     * @brief update
     * @param p0
     * @param p1
     */
    void update(Eigen::Vector2f &p0, Eigen::Vector2f &p1)
    {
        this->p.clear();
        this->p.push_back(p0);
        this->p.push_back(p1);
    }

    /**
     * @brief update
     * @param p0
     * @param p1
     */
    void update(std::vector< Eigen::Vector2f> &p)
    {
        this->p.clear();
        this->p.assign(p.begin(), p.end());
    }

    /**
     * @brief function
     * @param x
     * @param n
     * @return
     */
    double function(double *x, unsigned int n)
    {
        Eigen::Vector4d point(x[0], x[1], x[2], 1.0);

        double err = 0.0;
        for(unsigned int i = 0; i < M.size(); i++) {
            Eigen::Vector3d proj = M[i] * point;

            proj[0] /= proj[2];
            proj[1] /= proj[2];

            double dx = p[i][0] - proj[0];
            double dy = p[i][1] - proj[1];

            err += (dx * dx) + (dy * dy);
        }

        return err;
    }
};

#endif

}

#endif // PIC_COMPUTER_VISION_NELDER_MEAD_OPT_TRIANGULATION_HPP