#include <k4a/k4a.hpp>

#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <vector>
#include <array>

#include <Eigen/Dense>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/bilateral.h>
#include <pcl/filters/fast_bilateral.h>

#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <Open3D/Geometry/PointCloud.h>
#include <Open3d/Registration/ColoredICP.h>
#include <Open3D/Open3D.h>

#include "k4a_grabber.h"

using namespace std;
using namespace boost;
using namespace pcl;
using namespace Eigen;
using namespace glm;

typedef pcl::PointXYZRGB PointType;

template<typename DataType>
Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> load_csv(const std::string & path)
{
	std::ifstream indata;
	indata.open(path);
	std::string line;
	std::vector<DataType> values;
	unsigned int rows = 0;
	while (std::getline(indata, line))
	{
		std::stringstream lineStream(line);
		std::string cell;
		while (std::getline(lineStream, cell, ','))
		{
			values.push_back(std::stod(cell));
		}
		++rows;
	}
	return Eigen::Map<const Eigen::Matrix<typename Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>::Scalar, Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>::RowsAtCompileTime, Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>::ColsAtCompileTime, Eigen::RowMajor>>(values.data(), rows, values.size() / rows);
}

void convertToOpenCDPointCloud(pcl::PointCloud<PointType>::ConstPtr cloud, open3d::geometry::PointCloud &pointCloud)
{
	double x, y, z, r, g, b;
	pointCloud.Clear();
	for (const auto &point : cloud->points)
	{
		x = point.x;
		y = point.y;
		z = point.z;
		r = point.r / 256.0;
		g = point.g / 256.0;
		b = point.b / 256.0;

		pointCloud.points_.push_back(Eigen::Vector3d(x, y, z));
		pointCloud.colors_.push_back(Eigen::Vector3d(r, g, b));
	}
}

Eigen::Matrix4d colorCloudRegistrationOptimization(pcl::PointCloud<PointType>::ConstPtr pcl_source, pcl::PointCloud<PointType>::ConstPtr pcl_target, const Eigen::Matrix4d &init_transformation)
{
	open3d::geometry::PointCloud open3d_source_original, open3d_target_original;
	open3d::geometry::PointCloud open3d_source_current, open3d_target_current;

	convertToOpenCDPointCloud(pcl_source, open3d_source_original);
	convertToOpenCDPointCloud(pcl_target, open3d_target_original);

	open3d::registration::RegistrationResult result;
	Eigen::Matrix4d current_transformation = init_transformation;

	open3d_source_current = open3d_source_original;
	open3d_target_current = open3d_target_original;

	open3d::geometry::PointCloud open3d_source_down;
	open3d::geometry::PointCloud open3d_target_down;

	double radius = 0.01;

	open3d_source_down = *open3d_source_current.VoxelDownSample(radius);
	open3d_target_down = *open3d_target_current.VoxelDownSample(radius);

	open3d_source_down.EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(radius * 2, 30));
	open3d_target_down.EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(radius * 2, 30));

	result = open3d::registration::RegistrationColoredICP(open3d_source_down, open3d_target_down,
		radius, current_transformation,
		open3d::registration::ICPConvergenceCriteria(1e-16, 1e-16, 500));

	return result.transformation_;
}

int main(int argc, char **argv)
{
	string frame_sub_master_file = "frame_sub_master.csv";
	cout << "Reading frame_sub_master.csv" << endl;
	Transform<double, 3, Affine> transformation_sub_master;
	transformation_sub_master.matrix() = load_csv<double>(frame_sub_master_file);

	const uint32_t deviceCount = k4a::device::get_installed_count();
	if (deviceCount == 0)
	{
		cout << "no azure kinect devices detected!" << endl;
	}

	// PCL Visualizer
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));

	// Point Cloud
	pcl::PointCloud<PointType>::Ptr cloud_master;
	pcl::PointCloud<PointType>::Ptr cloud_sub;

	// Retrieved Point Cloud Callback Function
	boost::mutex mutex_master;
	boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&)> function_master =
		[&cloud_master, &mutex_master](const pcl::PointCloud<PointType>::ConstPtr& ptr)
	{
		boost::mutex::scoped_lock lock(mutex_master);

		/* Point Cloud Processing */

		cloud_master = ptr->makeShared();

	};

	boost::mutex mutex_sub;
	boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&)> function_sub =
		[&cloud_sub, &mutex_sub](const pcl::PointCloud<PointType>::ConstPtr& ptr)
	{
		boost::mutex::scoped_lock lock(mutex_sub);

		/* Point Cloud Processing */

		cloud_sub = ptr->makeShared();

	};

	// KinectAzureDKGrabber
	boost::shared_ptr<pcl::Grabber> grabber_master =
		boost::make_shared<pcl::KinectAzureDKGrabber>(0, K4A_DEPTH_MODE_NFOV_UNBINNED, K4A_IMAGE_FORMAT_COLOR_BGRA32, K4A_COLOR_RESOLUTION_720P);
	boost::shared_ptr<pcl::Grabber> grabber_sub =
		boost::make_shared<pcl::KinectAzureDKGrabber>(1, K4A_DEPTH_MODE_NFOV_UNBINNED, K4A_IMAGE_FORMAT_COLOR_BGRA32, K4A_COLOR_RESOLUTION_720P);

	boost::shared_ptr<pcl::KinectAzureDKGrabber> grabber_ = boost::dynamic_pointer_cast<pcl::KinectAzureDKGrabber>(grabber_master);

	// Register Callback Function
	boost::signals2::connection connection_sub = grabber_sub->registerCallback(function_sub);
	boost::signals2::connection connection_master = grabber_master->registerCallback(function_master);

	// Start Grabber
	grabber_sub->start();
	grabber_master->start();

	k4a::calibration calibration = grabber_->getCalibration();
	k4a_calibration_intrinsic_parameters_t *intrinsics = &calibration.color_camera_calibration.intrinsics.parameters;
	Eigen::Matrix3f intrinsics_eigen;
	intrinsics_eigen <<
		intrinsics->param.fx, 0.0f, intrinsics->param.cx,
		0.0f, intrinsics->param.fy, intrinsics->param.cy,
		0.0f, 0.0f, 1.0f;
	Eigen::Matrix4f extrinsics_eigen = Eigen::Matrix4f::Identity();
	viewer->setCameraParameters(intrinsics_eigen, extrinsics_eigen);

	pcl::PointCloud<PointType>::Ptr cloud(new PointCloud<PointType>);

	float aspect = 0.475f;

	transformation_sub_master.matrix()(0, 3) *= aspect;
	transformation_sub_master.matrix()(1, 3) *= aspect;
	transformation_sub_master.matrix()(2, 3) *= aspect;

	bool refineResult = false;

	Eigen::Matrix4d init_transformation;
	init_transformation = transformation_sub_master.matrix();

	Eigen::Matrix4d refinedTransformation;

	while (!viewer->wasStopped())
	{
		// Update Viewer
		viewer->spinOnce();
		boost::mutex::scoped_try_lock lock_master(mutex_master);
		boost::mutex::scoped_try_lock lock_sub(mutex_sub);

		if (lock_master.owns_lock() && cloud_master &&
			lock_sub.owns_lock() && cloud_sub)
		{
			if (!refineResult)
			{
				std::cout << "Start Calibration Optimization." << std::endl;
				refinedTransformation = colorCloudRegistrationOptimization(cloud_master, cloud_sub, init_transformation);
				refineResult = true;
				std::cout << "Calibration optimization process over." << std::endl;
			}

			transformPointCloud(*cloud_master, *cloud_master, refinedTransformation, true);

			*cloud = *cloud_sub + *cloud_master;

			// Update Point Cloud
			if (!viewer->updatePointCloud(cloud, "cloud"))
			{
				viewer->addPointCloud(cloud, "cloud");
			}
		}
	}
	// Stop Grabber
	grabber_sub->stop();
	grabber_master->stop();

	// Disconnect Callback Function
	if (connection_master.connected() || connection_sub.connected())
	{
		connection_sub.disconnect();
		connection_master.disconnect();
	}

	return 0;
}


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

 VectorField.hpp
 GeoShoot

 Author: Alexandre Martin, Laurent Risser
 Copyright © 2016 scalexm, lrisser. All rights reserved.

 Disclaimer: This software has been developed for research purposes only, and hence should
 not be used as a diagnostic tool. In no event shall the authors or distributors
 be liable to any direct, indirect, special, incidental, or consequential
 damages arising of the use of this software, its documentation, or any
 derivatives thereof, even if the authors have been advised of the possibility
 of such damage.

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

#ifndef VECTOR_FIELD_HPP
#define VECTOR_FIELD_HPP

#define BOOST_COMPUTE_DEBUG_KERNEL_COMPILATION
#include <boost/compute.hpp>
#include <vector>
#include <array>
#include <type_traits>

template<size_t Row, size_t Col>
using Matrix = std::array<std::array<float, Col>, Row>;

namespace compute = boost::compute;

struct CPUPolicy {
    using Container = std::vector<float>;

    template<class... Args>
    static Container MakeContainer(const compute::context &, Args && ... args) {
        return Container(std::forward<Args>(args)...);
    }
};

struct GPUPolicy {
    using Container = compute::vector<float>;

    template<class... Args>
    static Container MakeContainer(const compute::context & ctx, Args && ... args) {
        return Container(std::forward<Args>(args)..., ctx);
    }
};

template<size_t Dim, class DevicePolicy>
class GenericVectorField {
private:
    template<size_t OtherDim, class OtherDevicePolicy>
    friend class GenericVectorField;

    int NX_ = 0, NY_ = 0, NZ_ = 0;
    int NXtY_ = 0, NXtYtZ_ = 0;
    Matrix<4, 4> Image2World_, World2Image_;

    size_t Channel_ = 0;

    std::vector<typename DevicePolicy::Container> VecField_;

    template<size_t Direction>
    size_t Index(int x, int y, int z) const {
        return Direction * NXtYtZ_ + z * NXtY_ + y * NX_ + x;
    }

public:
    using Iterator = typename DevicePolicy::Container::iterator;
    using ConstIterator = typename DevicePolicy::Container::const_iterator;

    GenericVectorField() = default;

    GenericVectorField(int NX, int NY, int NZ, int NT = 1,
                       const compute::context & ctx = compute::system::default_context())
                       : NX_ { NX }, NY_ { NY }, NZ_ { NZ },
                         NXtY_ { NX * NY }, NXtYtZ_ { NX * NY * NZ },
                         VecField_(NT) {
        for (auto t = 0; t < NT; ++t)
            VecField_[t] = DevicePolicy::MakeContainer(ctx, Dim * NX * NY * NZ);

        memset(&Image2World_[0], 0, 16 * sizeof(float));
        Image2World_[0][0] = Image2World_[1][1] = Image2World_[2][2] = Image2World_[3][3] = 1;
        World2Image_ = Image2World_;
    }

    const Matrix<4, 4> & Image2World() const {
        return Image2World_;
    }

    const Matrix<4, 4> & World2Image() const {
        return World2Image_;
    }

    void ChangeChannel(size_t c) {
        Channel_ = c;
    }

    // emplace a vector at point (x, y, z)
    void P(std::array<float, Dim> values, int x, int y, int z) {
        auto index = Index<0>(x, y, z);
        for (auto dir = 0; dir < Dim; ++dir)
            VecField_[Channel_][dir * NXtYtZ_ + index] = values[dir];
    }

    // add a vector at point (x, y, z)
    void Add(std::array<float, Dim> values, int x, int y, int z) {
        auto index = Index<0>(x, y, z);
        for (auto dir = 0; dir < Dim; ++dir)
            VecField_[Channel_][dir * NXtYtZ_ + index] += values[dir];
    }

    // get value at point (x, y, z) and at direction Direction
    template<size_t Direction>
    float G(int x, int y, int z) const {
        static_assert(Direction < Dim, "Direction should be less than Dim");
        return VecField_[Channel_][Index<Direction>(x, y, z)];
    }

    // set all voxels to value given by cst
    void Fill(float cst) {
        static_assert(std::is_same<DevicePolicy, CPUPolicy>::value, "only on CPU");
        std::fill(Begin(), End(), cst);
    }

    void Fill(float cst, compute::command_queue & queue) {
        compute::fill(Begin(), End(), cst, queue);
    }

    float MaxAbsVal() const {
        static_assert(std::is_same<DevicePolicy, CPUPolicy>::value, "only on CPU");
        auto max = 0.f;
        for (auto && x : VecField_) {
            auto xa = fabs(x);
            if (xa > max)
                max = xa;
        }
        return max;
    }

    float MaxAbsVal(compute::command_queue & queue) const {
        auto it = compute::minmax_element(Begin(), End(), compute::less<float>(), queue);
        return std::max(fabs(it.first.read(queue)), fabs(it.second.read(queue)));
    }

    int NX() const {
        return NX_;
    }

    int NY() const {
        return NY_;
    }

    int NZ() const {
        return NZ_;
    }

    int NT() const {
        return VecField_.size();
    }

    compute::int4_ Dims() const {
        return { NX_, NY_, NZ_, 0 };
    }

    Iterator Begin() {
        return VecField_[Channel_].begin();
    }

    ConstIterator Begin() const {
        return VecField_[Channel_].begin();
    }

    Iterator End() {
        return VecField_[Channel_].end();
    }

    ConstIterator End() const {
        return VecField_[Channel_].end();
    }

    typename DevicePolicy::Container & Buffer() {
        return VecField_[Channel_];
    }

    const typename DevicePolicy::Container & Buffer() const {
        return VecField_[Channel_];
    }

    /* not perfect regarding memory efficiency, should be rewritten if needed */
    static GenericVectorField Read(const std::array<const char *, Dim> & paths) {
        static_assert(std::is_same<DevicePolicy, CPUPolicy>::value, "only on CPU");
        GenericVectorField<Dim, DevicePolicy> field;

        for (auto dim = 0; dim < Dim; ++dim) {
            auto dir = GenericVectorField<1, CPUPolicy>::Read({ paths[dim] });

            if (dim != 0) {
                auto eq_x = dir.NX() == field.NX();
                auto eq_y = dir.NY() == field.NY();
                auto eq_z = dir.NZ() == field.NZ();
                auto eq_t = dir.NT() == field.NT();
                if (!eq_x || !eq_y || !eq_z || !eq_t)
                    throw std::invalid_argument { "directions do not have the same dimensions" };
            }

            if (dim == 0) {
                field = GenericVectorField<Dim, DevicePolicy> {
                    dir.NX(),
                    dir.NY(),
                    dir.NZ(),
                    dir.NT()
                };
                field.Image2World_ = std::move(dir.Image2World_);
                field.World2Image_ = std::move(dir.World2Image_);
            }

            for (auto t = 0; t < dir.NT(); ++t) {
                dir.ChangeChannel(t);
                std::copy(dir.Begin(), dir.End(), &field.VecField_[t][dim * field.NXtYtZ_]);
            }
        }

        return field;
    }

    void Write(const std::array<const char *, Dim> & paths) const {
        static_assert(std::is_same<DevicePolicy, CPUPolicy>::value, "only on CPU");
        for (auto dim = 0; dim < Dim; ++dim) {
            GenericVectorField<1, CPUPolicy> dir { NX_, NY_, NZ_, NT() };

            for (auto t = 0; t < NT(); ++t) {
                dir.ChangeChannel(t);
                std::copy(
                    &VecField_[t][dim * NXtYtZ_],
                    &VecField_[t][dim * NXtYtZ_] + NXtYtZ_,
                    dir.Begin()
                );
            }
            dir.Image2World_ = Image2World_;
            dir.Write({ paths[dim] });
        }
    }
};

template<size_t Dim>
using GPUVectorField = GenericVectorField<Dim, GPUPolicy>;

template<size_t Dim>
using VectorField = GenericVectorField<Dim, CPUPolicy>;

using GPUScalarField = GPUVectorField<1>;
using ScalarField = VectorField<1>;

template<>
ScalarField ScalarField::Read(const std::array<const char *, 1> & path);

template<>
void ScalarField::Write(const std::array<const char *, 1> & path) const;

#endif


#include <aslam/Frame.hpp>
#include <aslam/backend/EuclideanExpression.hpp>
#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/HomogeneousPoint.hpp>
#include <aslam/backend/OptimizationProblem.hpp>
#include <aslam/backend/Optimizer.hpp>
#include <aslam/backend/ReprojectionError.hpp>
#include <aslam/backend/RotationQuaternion.hpp>
#include <aslam/backend/TransformationBasic.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <sm/eigen/gtest.hpp>
#include <sm/kinematics/Transformation.hpp>
#include <sm/kinematics/homogeneous_coordinates.hpp>
#include <sm/kinematics/rotations.hpp>
#include <sm/random.hpp>
//#include <aslam/FrameTypedefs.hpp>
#include <aslam/backend/CameraDesignVariable.hpp>
#include <aslam/cameras.hpp>

class HpErr : public aslam::backend::ErrorTermFs<4> {
  public:
    aslam::backend::HomogeneousPoint _p;
    typedef aslam::backend::ErrorTermFs<4> parent_t;
    HpErr(const Eigen::Vector4d& p) : _p(p) {
        _p.setActive(true);
        _p.setBlockIndex(0);
        parent_t::setDesignVariables(&_p);
        Eigen::MatrixXd invR = parent_t::invR();
        invR.setIdentity();
        parent_t::setInvR(invR);
    }
    virtual ~HpErr() {}

    /// \brief evaluate the error term
    virtual double evaluateErrorImplementation() {
        parent_t::setError(_p.toHomogeneous());

        return parent_t::error().dot(parent_t::invR() * parent_t::error());
    }

    /// \brief evaluate the jacobian

    virtual void evaluateJacobiansImplementation(aslam::backend::JacobianContainer& J) { _p.evaluateJacobians(J); }
};

TEST(ReprojectionErrorTestSuite, testSimpleError) {
    try {
        using namespace aslam::backend;
        HpErr e(Eigen::Vector4d::Random());
        JacobianContainer estJ(4);
        e.evaluateJacobiansFiniteDifference(estJ);

        JacobianContainer J(4);
        e.evaluateJacobians(J);

        SCOPED_TRACE("");
        sm::eigen::assertNear(J.asDenseMatrix(), estJ.asDenseMatrix(), 1e-6, SM_SOURCE_FILE_POS,
                              "Checking the jacobian vs. finite differences");

    } catch (const std::exception& e) {
        FAIL() << e.what();
    }
}

///*
TEST(ReprojectionErrorTestSuite, testReprojectionError) {
    try {
        using namespace aslam;
        using namespace aslam::backend;
        using namespace aslam::cameras;
        typedef Frame<OmniCameraGeometry> frame_t;
        typedef frame_t::camera_geometry_t camera_geometry_t;
        typedef frame_t::keypoint_t keypoint_t;
        typedef ReprojectionError<camera_geometry_t> error_t;

        boost::shared_ptr<camera_geometry_t> geometry(new camera_geometry_t(camera_geometry_t::getTestGeometry()));
        // create the camera design variable:
        CameraDesignVariable<camera_geometry_t> cameraDv(geometry);

        frame_t frame;
        frame.setGeometry(geometry);

        Eigen::Matrix2d invR;
        invR.setIdentity();

        /// N keypoints per frame
        const int N = 1;

        for (int n = 0; n < N; n++) {
            keypoint_t k;
            Eigen::Vector4d p = sm::kinematics::toHomogeneous(geometry->createRandomVisiblePoint());
            boost::shared_ptr<aslam::backend::HomogeneousPoint> pt(new aslam::backend::HomogeneousPoint(p));
            pt->setActive(true);
            pt->setBlockIndex(0);
            HomogeneousExpression hep(pt);

            Eigen::Vector2d kp;
            geometry->homogeneousToKeypoint(hep.toHomogeneous(), kp);
            k.setMeasurement(kp);
            k.setInverseMeasurementCovariance(invR);
            frame.addKeypoint(k);

            error_t e(&frame, n, hep, cameraDv);

            // ASSERT_NEAR(e.evaluateError(), 0.0, 1e-14);

            // Get the jacobians by finite difference.
            JacobianContainer estJ(2);
            e.evaluateJacobiansFiniteDifference(estJ);

            JacobianContainer J(2);
            e.evaluateJacobians(J);

            SCOPED_TRACE("");
            sm::eigen::assertNear(J.asDenseMatrix(), estJ.asDenseMatrix(), 1e-6, SM_SOURCE_FILE_POS,
                                  "Checking the jacobian vs. finite differences");
        }
    } catch (const std::exception& e) {
        FAIL() << e.what();
    }
}

using namespace aslam;
using namespace aslam::backend;

TEST(ReprojectionErrorTestSuite, testEstimator) {
    try {
        using namespace aslam;
        using namespace aslam::backend;
        using namespace aslam::cameras;
        typedef Frame<OmniCameraGeometry> frame_t;
        typedef frame_t::camera_geometry_t camera_geometry_t;
        typedef frame_t::keypoint_t keypoint_t;
        typedef ReprojectionError<camera_geometry_t> error_t;

        boost::shared_ptr<camera_geometry_t> geometry(new camera_geometry_t(camera_geometry_t::getTestGeometry()));

        // create the camera design variable:
        CameraDesignVariable<camera_geometry_t> cameraDv(geometry);

        // N keypoints per frame
        const int N = 200;
        // K frames
        const int K = 5;
        // noise on keypoints
        const double sigma = 1.0;
        // noise on initial guess
        const double perturbQuat = 0.01;
        const double perturbTrans = 0.01;

        Eigen::Matrix2d invR;
        invR.setIdentity();

        // Step 1. Generate the trajectory
        boost::ptr_vector<sm::kinematics::Transformation> T_w_ck;
        boost::ptr_vector<aslam::backend::EuclideanPoint> t_w_ck_w;
        boost::ptr_vector<aslam::backend::RotationQuaternion> q_w_ck;
        boost::ptr_vector<frame_t> frame_k;
        boost::ptr_vector<aslam::backend::HomogeneousPoint> landmarks;

        boost::shared_ptr<OptimizationProblem> problem_ptr(new OptimizationProblem);
        OptimizationProblem& problem = *problem_ptr;

        // add the camera stuff to the problem
        problem.addDesignVariable(cameraDv.projectionDesignVariable());
        problem.addDesignVariable(cameraDv.distortionDesignVariable());
        problem.addDesignVariable(cameraDv.shutterDesignVariable());

        // start at identity
        sm::kinematics::Transformation T_w_c;
        sm::kinematics::Transformation T_ck_ckp1;
        // Start at identity.
        for (int k = 0; k < K; ++k) {
            T_w_ck.push_back(new sm::kinematics::Transformation(T_w_c));
            t_w_ck_w.push_back(new aslam::backend::EuclideanPoint(T_w_c.t()));
            q_w_ck.push_back(new aslam::backend::RotationQuaternion(T_w_c.q()));

            T_ck_ckp1.setRandom(0.5, sm::kinematics::deg2rad(3.0));
            T_w_c = T_w_c * T_ck_ckp1;

            frame_k.push_back(new frame_t);
            frame_k[k].setGeometry(geometry);

            // Setting these poses to active means they will be estimated.
            q_w_ck[k].setActive(true);
            t_w_ck_w[k].setActive(true);

            // Add the design variables to the problem In this case, we pass in raw pointers
            // and tell the optimization problem that it doesn't own these variables.
            // In this case, it is the users responsibility to ensure that these pointers don't
            // go out of scope while the problem exists.
            problem.addDesignVariable(&q_w_ck[k], false);
            problem.addDesignVariable(&t_w_ck_w[k], false);
        }

        // Step 2. Create some visible landmarks.

        for (int k = 0; k < K; ++k) {
            for (int n = 0; n < N; ++n) {
                // create a point.
                Eigen::Vector4d p_ck = sm::kinematics::toHomogeneous(
                    frame_k[k].geometry().createRandomVisiblePoint(sm::random::randLU(1.0, 10.0)));

                // the aslam::backend::HomogeneousPoint class is a design variable.
                landmarks.push_back(new aslam::backend::HomogeneousPoint(T_w_ck[k] * p_ck));
                problem.addDesignVariable(&landmarks.back(), false);
            }
        }

        std::set<int> activeLandmarks;

        // Step 3. Project all of the landmarks into the images to create some keypoint measurements.
        for (size_t l = 0; l < landmarks.size(); ++l) {
            int seenCount = 0;
            for (int k = 0; k < K; k++) {
                sm::kinematics::Transformation T_ck_w = T_w_ck[k].inverse();
                // Transform the landmark into the current camera frame.
                Eigen::Vector4d p_ck = T_ck_w * landmarks[l].toHomogeneous();
                // Project it into the image using this frame's camera geometry.
                Eigen::Vector2d y;
                frame_k[k].geometry().homogeneousToKeypoint(p_ck, y);

                // Check if the projection is valid and the point is in front of the camera.
                if (frame_k[k].geometry().isValid(y) && sm::kinematics::fromHomogeneous(p_ck)[2] > 0.0) {
                    ++seenCount;
                }
            }

            if (seenCount > 1) {
                // If we see this landmark in 2+ frames
                // add it to the list of landmarks in the estimation problem.
                activeLandmarks.insert(l);
                // Keeping 10 landmarks fixed will make sure this is an observable system. This is a hack.
                if (activeLandmarks.size() > 10) {
                    // Past 10 landmarks we will estimate the position
                    landmarks[l].setActive(true);
                    // setting this to marginalized will enable the Schur complement trick for these variables.
                    landmarks[l].setMarginalized(true);
                }

                // Now we add all of the keypoint measurements
                LandmarkId lid(l);
                for (int k = 0; k < K; k++) {
                    // Create a transformation expression from the design variables.
                    TransformationExpression T_w_ck(q_w_ck[k].toExpression(), t_w_ck_w[k].toExpression());

                    // Invert this expression to get a new expression that takes points from the world frame to the
                    // camera frame.
                    TransformationExpression T_ck_w = T_w_ck.inverse();

                    // Multiplying the above expression with a landmark HomogeneousPointExpression makes a new
                    // HomogeneousPointExpression representing the landmark expressed in the camera frame.
                    HomogeneousExpression p_ck = T_ck_w * landmarks[l].toExpression();

                    // Project this into the image.
                    Eigen::Vector2d y;
                    frame_k[k].geometry().homogeneousToKeypoint(p_ck.toHomogeneous(), y);

                    // Again, check if this is a valid projection and in front of the camera.
                    if (frame_k[k].geometry().isValid(y) &&
                        sm::kinematics::fromHomogeneous(p_ck.toHomogeneous())[2] > 0.0) {
                        // Now we can add an error term...First add the measurement to the frame.
                        keypoint_t kp;
                        kp.setMeasurement(y +
                                          Eigen::Vector2d(sm::random::randn() * sigma, sm::random::randn() * sigma));
                        kp.setInverseMeasurementCovariance(invR);
                        kp.setLandmarkId(lid);
                        frame_k[k].addKeypoint(kp);

                        // Then add an error term into the optimization problem.
                        boost::shared_ptr<error_t> error(
                            new error_t(&frame_k[k], frame_k[k].numKeypoints() - 1, p_ck, cameraDv));
                        problem.addErrorTerm(error);
                    }
                }
            }  // end if this landmark was seen by more than one pose.
        }      // end for each landmark

        // Perturb the initial guesses a little bit;
        for (size_t i = 0; i < q_w_ck.size(); i++) {
            Eigen::Vector3d up = Eigen::Vector3d::Random() * perturbQuat;
            q_w_ck[i].update(&up[0], 3);
        }
        for (size_t i = 0; i < t_w_ck_w.size(); i++) {
            Eigen::Vector3d up = Eigen::Vector3d::Random() * perturbTrans;
            t_w_ck_w[i].update(&up[0], 3);
        }

        // Okay, now we have a bunch of design variables in "q_w_ck", "t_w_ck_w", and "landmarks".
        // And a bunch of reprojection errors in "errors".
        // These are packed in to the variable "problem"
        OptimizerOptions options;
        options.verbose = true;
        options.linearSolver = "cholmod";
        options.levenbergMarquardtLambdaInit = 10;
        options.doSchurComplement = true;
        options.doLevenbergMarquardt = true;
        Optimizer optimizer(options);
        optimizer.setProblem(problem_ptr);
        optimizer.options().verbose = true;
        optimizer.optimize();

    } catch (const std::exception& e) {
        FAIL() << e.what();
    }
}


#include <MeshFEM/MeshIO.hh>
#include <MeshFEM/MSHFieldWriter.hh>
#include <MeshFEM/LinearElasticity.hh>
#include <MeshFEM/Materials.hh>
#include <MeshFEM/PeriodicHomogenization.hh>
#include <MeshFEM/OrthotropicHomogenization.hh>
#include <MeshFEM/GlobalBenchmark.hh>
#include <MeshFEM/TensorProjection.hh>
#include <vector>
#include <queue>
#include <iostream>
#include <iomanip>
#include <memory>
#include <cmath>

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

namespace po = boost::program_options;
using namespace std;
using namespace PeriodicHomogenization;

[[ noreturn ]] void usage(int exitVal, const po::options_description &visible_opts) {
    cout << "Usage: PeriodicHomogenization_cli [options] mesh" << endl;
    cout << visible_opts << endl;
    exit(exitVal);
}

po::variables_map parseCmdLine(int argc, const char *argv[])
{
    po::options_description hidden_opts("Hidden Arguments");
    hidden_opts.add_options()
        ("mesh",       po::value<string>(),                     "input mesh")
        ;
    po::positional_options_description p;
    p.add("mesh",                1);

    po::options_description visible_opts;
    visible_opts.add_options()("help", "Produce this help message")
        ("material,m", po::value<string>(),                "base material")
        ("degree,d",   po::value<int>()->default_value(2), "degree of finite elements")
        ("m2mstress,M",po::value<string>(),                "Dump macroscopic to microscopic stress tensors to specified file")
        ("fieldOutput,o",po::value<string>(),              "Dump fluctuation stress and strain fields to specified msh file")
        ("centerFluctuationDisplacements,c",               "Shift each fluctuation displacement so that it averages to zero")
        ("fullDegreeFieldOutput,D",                        "Output full-degree nodal fields (don't do piecewise linear subsample)")
        ("distanceToIsotropy",                             "Output the distance to the closest isotropic tensor")
        ("distanceToMaterial", po::value<string>(),        "Output the distance to a particular material")
        ("ignorePeriodicMismatch",                         "Ignore mismatched nodes on the periodic faces (useful for voxel grids)")
        ("manualPeriodicVertices", po::value<string>(),    "Manually specify identified periodic vertices using a hacky file format (see PeriodicCondition constructor)")
        ("orthotropicCell,O",                              "Analyze the orthotropic symmetry base cell only")
        ;

    po::options_description cli_opts;
    cli_opts.add(visible_opts).add(hidden_opts);

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).
                  options(cli_opts).positional(p).run(), vm);
        po::notify(vm);
    }
    catch (std::exception &e) {
        cout << "Error: " << e.what() << endl << endl;
        usage(1, visible_opts);
    }

    bool fail = false;
    if (vm.count("mesh") == 0) {
        cout << "Error: must specify input mesh" << endl;
        fail = true;
    }

    int d = vm["degree"].as<int>();
    if (d < 1 || d > 2) {
        cout << "Error: FEM Degree must be 1 or 2" << endl;
        fail = true;
    }

    if (fail || vm.count("help"))
        usage(fail, visible_opts);

    return vm;
}

template<size_t _N>
using HMG = LinearElasticity::HomogenousMaterialGetter<Materials::Constant>::template Getter<_N>;

template<size_t _N, size_t _FEMDegree>
void execute(const po::variables_map &args,
             const vector<MeshIO::IOVertex> &inVertices,
             const vector<MeshIO::IOElement> &inElements) {
    auto &mat = HMG<_N>::material;
    if (args.count("material")) mat.setFromFile(args["material"].as<string>());

    typedef LinearElasticity::Mesh<_N, _FEMDegree, HMG> Mesh;
    typedef LinearElasticity::Simulator<Mesh> Simulator;
    Simulator sim(inElements, inVertices);
    typedef typename Simulator::ETensor ETensor;
    typedef typename Simulator::VField  VField;

    BENCHMARK_START_TIMER_SECTION("Cell Problems");
    std::vector<VField> w_ij;
    std::unique_ptr<PeriodicCondition<_N>> pc;
    if (args.count("manualPeriodicVertices"))
        pc = Future::make_unique<PeriodicCondition<_N>>(sim.mesh(), args["manualPeriodicVertices"].as<string>());
    if (args.count("orthotropicCell") == 0) {
        solveCellProblems(w_ij, sim, 1e-7, args.count("ignorePeriodicMismatch"), std::move(pc));
    }
    else {
        auto systems = PeriodicHomogenization::Orthotropic::solveCellProblems(w_ij, sim, 1e-7);
    }

    BENCHMARK_STOP_TIMER_SECTION("Cell Problems");

    BENCHMARK_START_TIMER_SECTION("Compute Tensor");
    // ETensor Eh = homogenizedElasticityTensor(w_ij, sim);
    ETensor Eh;
    if (args.count("orthotropicCell") == 0)   Eh = homogenizedElasticityTensorDisplacementForm(w_ij, sim);
    else Eh = PeriodicHomogenization::Orthotropic::homogenizedElasticityTensorDisplacementForm(w_ij, sim);
    BENCHMARK_STOP_TIMER_SECTION("Compute Tensor");

    cout << setprecision(16);
    cout << "Homogenized elasticity tensor:" << endl;
    cout << Eh << endl << endl;

    auto eigs = Eh.computeEigenstrains();
    cout << "Minimum Eh eigenvalue " << eigs.lambdas[0] << " for eigenstrain: "
         << eigs.strains.col(0).transpose() << endl;

    cout << "Intermediate Eh eigenvalue " << eigs.lambdas[1] << " for eigenstrain: "
         << eigs.strains.col(1).transpose() << endl;

    cout << "Max Eh eigenvalue " << eigs.lambdas[2] << " for eigenstrain: "
         << eigs.strains.col(2).transpose() << endl;

    ETensor S = Eh.inverse();
    cout << "Homogenized compliance tensor:" << endl;
    cout << S << endl;
    vector<Real> moduli(flatLen(_N));

    // Shear moduli are multiplied by 4 in flattened compliance tensor...
    for (size_t i = 0; i < flatLen(_N); ++i)
        moduli[i] = ((i < _N) ? 1.0 : 0.25) / S.D(i, i);

    vector<Real> poisson;
    if (_N == 2) poisson = { -S.D(0, 1) / S.D(1, 1),   // v_yx
                             -S.D(1, 0) / S.D(0, 0) }; // v_xy
    else         poisson = { -S.D(0, 1) / S.D(1, 1),   // v_yx
                             -S.D(0, 2) / S.D(2, 2),   // v_zx
                             -S.D(1, 2) / S.D(2, 2),   // v_zy
                             -S.D(1, 0) / S.D(0, 0),   // v_xy
                             -S.D(2, 0) / S.D(0, 0),   // v_xz
                             -S.D(2, 1) / S.D(1, 1) }; // v_zy

    if (_N == 2)  {
        cout << "Approximate Young moduli:\t"  << moduli[0] << "\t" << moduli[1] << endl;
        cout << "Approximate shear modulus:\t" << moduli[2] << endl;

        cout << "v_yx, v_xy:\t" << poisson[0] << "\t" << poisson[1] << endl;
    }
    else {
        cout << "Approximate Young moduli:\t" << moduli[0] << "\t" << moduli[1] << "\t"
             << moduli[2] << endl;
        cout << "Approximate shear moduli:\t" << moduli[3] << "\t" << moduli[4] << "\t"
             << moduli[5] << endl;

        cout << "v_yx, v_zx, v_zy:\t" << poisson[0] << "\t" << poisson[1] << "\t" << poisson[2] << endl;
        cout << "v_xy, v_xz, v_yz:\t" << poisson[3] << "\t" << poisson[4] << "\t" << poisson[5] << endl;
    }

    cout << "Anisotropy:\t" << Eh.anisotropy() << endl;

    if (args.count("m2mstress")) {
        string mpath = args["m2mstress"].as<string>();
        ofstream mfile(mpath);
        ofstream gfile("gtensors.txt");
        mfile << setprecision(16);
        gfile << setprecision(16);
        if (!mfile.is_open()) throw runtime_error("Failed to open output file " + mpath);
        auto G = macroStrainToMicroStrainTensors(w_ij, sim);
        for (size_t ei = 0; ei < sim.mesh().numElements(); ++ei) {
            G.at(ei).writeUnflattened(gfile); gfile << endl;
            auto F = mat.getTensor().doubleContract(G.at(ei).doubleContract(S));
            F.writeUnflattened(mfile);
            mfile << endl;
        }
    }

    if (args.count("fieldOutput")) {
        bool linearSubsampleFields = args.count("fullDegreeFieldOutput") == 0;
        MSHFieldWriter writer(args["fieldOutput"].as<string>(), sim.mesh(),
                              linearSubsampleFields);
        if (args.count("centerFluctuationDisplacements")) {
            for (size_t i = 0; i < w_ij.size(); ++i) {
                auto &w = w_ij[i];
                VectorND<_N> total(VectorND<_N>::Zero());
                for (size_t ii = 0; ii < w.domainSize(); ++ii) total += w(ii);
                total *= 1.0 / w.domainSize();
                for (size_t ii = 0; ii < w.domainSize(); ++ii) w(ii) -= total;
            }
        }
        for (size_t i = 0; i < w_ij.size(); ++i) {
            writer.addField("load_ij " + to_string(i), sim.dofToNodeField(sim.constantStrainLoad(-Simulator::SMatrix::CanonicalBasis(i))), DomainType::PER_NODE);
            writer.addField("w_ij " + to_string(i), w_ij[i], DomainType::PER_NODE);
            if ((Simulator::Strain::Deg == 0) || linearSubsampleFields) {
                // Output constant (average) strain when we're outputting piecewise
                // linear solutions.
                writer.addField("strain w_ij " + to_string(i),
                        sim.averageStrainField(w_ij[i]), DomainType::PER_ELEMENT);
            }
            else {
                // Output full-degree per-element strain. (Wasteful since
                // strain fields are of degree - 1, but Gmsh/MSHFieldWriter
                // only supports full-degree ElementNodeData).
                auto strainField = sim.strainField(w_ij[i]);
                typedef SymmetricMatrixInterpolant<typename Simulator::SMatrix,
                                               _N, _FEMDegree> UpsampledStrain;
                vector<UpsampledStrain> upsampledStrainField;
                upsampledStrainField.reserve(strainField.size());
                for (const auto s: strainField)
                    upsampledStrainField.emplace_back(s);
                writer.addField("strain w_ij " + to_string(i),
                                upsampledStrainField, DomainType::PER_ELEMENT);
            }
        }
    }

    if (args.count("distanceToIsotropy")) {
        auto isoFit = closestIsotropicTensor(Eh);
        cout << endl;
        cout << "(Sq Rel Frob) Distance to Isotropy:\t" << (isoFit - Eh).frobeniusNormSq() / isoFit.frobeniusNormSq() << endl;
        cout << "Closest isotropic tensor:" << endl << isoFit << endl;
        cout << endl;
    }

    if (args.count("distanceToMaterial")) {
        Materials::Constant<_N> targetMat(args.at("distanceToMaterial").as<string>());
        auto tgtE = targetMat.getTensor();
        cout << "(Sq Rel Frob) Distance to Specified Tensor:\t" << (Eh - tgtE).frobeniusNormSq() / tgtE.frobeniusNormSq() << endl;
    }

    BENCHMARK_REPORT();
}

////////////////////////////////////////////////////////////////////////////////
/*! Program entry point
//  @param[in]  argc    Number of arguments
//  @param[in]  argv    Argument strings
//  @return     status  (0 on success)
*///////////////////////////////////////////////////////////////////////////////
int main(int argc, const char *argv[])
{
    po::variables_map args = parseCmdLine(argc, argv);

    vector<MeshIO::IOVertex>  inVertices;
    vector<MeshIO::IOElement> inElements;
    string meshPath = args["mesh"].as<string>();
    auto type = load(meshPath, inVertices, inElements, MeshIO::FMT_GUESS,
                     MeshIO::MESH_GUESS);

    // Infer dimension from mesh type.
    size_t dim;
    if      (type == MeshIO::MESH_TET) dim = 3;
    else if (type == MeshIO::MESH_TRI) dim = 2;
    else    throw std::runtime_error("Mesh must be triangle or tet.");

    // Look up and run appropriate homogenizer instantiation.
    int deg = args["degree"].as<int>();
    auto exec = (dim == 3) ? ((deg == 2) ? execute<3, 2> : execute<3, 1>)
                           : ((deg == 2) ? execute<2, 2> : execute<2, 1>);

    exec(args, inVertices, inElements);

    return 0;
}


#include<iostream>
#include"basis.hpp"
#include"operators.hpp"
#include"diag.h"
#include"tpoperators.hpp"
#include"files.hpp"
#include"timeev.hpp"
#include"ETH.hpp"
#include <boost/program_options.hpp>
int main(int argc, char *argv[])
{
  using namespace Eigen;
using namespace std;
using namespace Many_Body;
  using Fermi_HubbardBasis= TensorProduct<ElectronBasis, ElectronBasis>;
   using HolsteinBasis= TensorProduct<ElectronBasis, PhononBasis>;
  using Mat= Operators::Mat;
  using boost::program_options::value;
   size_t M{};
  size_t L{};
  double t0{};
  double omega{};
  double gamma{};
  double dt{};
  double tot{};
  bool PB{};
  std::string dirname="";
  std::string filename={};
  try
  {
    boost::program_options::options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help screen")
      ("L", value(&L)->default_value(4), "L")
      ("M,m", value(&M)->default_value(2), "M")
      ("t", value(&t0)->default_value(1.), "t0")
      ("gam", value(&gamma)->default_value(1.), "gamma")
      ("omg", value(&omega)->default_value(1.), "omega")
      ("dir", value(&dirname)->default_value(""), "dirname")
    ("dt", value(&dt)->default_value(0.1), "dt")
      ("tot", value(&tot)->default_value(1.), "tot")
    ("pb", value(&PB)->default_value(true), "PB");

    boost::program_options::variables_map vm;
    boost::program_options::store(parse_command_line(argc, argv, desc), vm);
    boost::program_options::notify(vm);

    if (vm.count("help"))
      {std::cout << desc << '\n'; return 0;}
    else{
      if (vm.count("L"))
      {      std::cout << "L: " << vm["L"].as<size_t>() << '\n';
	filename+="L"+std::to_string(vm["L"].as<size_t>());
      }
     if (vm.count("M"))
      {
	std::cout << "M: " << vm["M"].as<size_t>() << '\n';
	filename+="M"+std::to_string(vm["M"].as<size_t>());
      }
      if (vm.count("t"))
      {
	std::cout << "t0: " << vm["t"].as<double>() << '\n';
	filename+="t0"+std::to_string(vm["t"].as<double>()).substr(0, 3);
      }
       if (vm.count("omg"))
      {
	std::cout << "omega: " << vm["omg"].as<double>() << '\n';
	filename+="omega"+std::to_string(vm["omg"].as<double>()).substr(0, 3);
      }
       if (vm.count("gam"))
      {
	std::cout << "gamma: " << vm["gam"].as<double>() << '\n';
	filename+="gam"+std::to_string(vm["gam"].as<double>()).substr(0, 4);
      }
       if (vm.count("dt"))
      {
	std::cout << "dt: " << vm["dt"].as<double>() << '\n';
	filename+="dt"+std::to_string(vm["dt"].as<double>()).substr(0, 4);
      }
       if (vm.count("tot"))
      {
	std::cout << "total time: " << vm["tot"].as<double>() << '\n';
	filename+="tot"+std::to_string(vm["tot"].as<double>()).substr(0, 3);
      }
       if (vm.count("pb"))
      {
	std::cout << "PB: " << vm["pb"].as<bool>() << '\n';
	filename+="PB"+std::to_string(vm["pb"].as<bool>());
      }
    }
  }
  catch (const boost::program_options::error &ex)
  {
    std::cerr << ex.what() << '\n';
    return 0;
  }

  //std::vector<int> ee(L, 0);
  // ee[L-1]=1;

  ElectronState estate1(L, 1);
  ElectronBasis e( L, 1);

  //  std::cout<< e<<std::endl;
std::vector<size_t> es(L, 0);
      es[0]=1;
      ElectronState aa(es);
  PhononBasis ph(L, M);
  //       std::cout<< ph<<std::endl;
      HolsteinBasis TP(e, ph);
      std::vector<size_t> state(L, 0);
      //std::fill(state.begin(), state.end(), 1);
  BosonState b2(state, M);

     auto it3=TP.lbasis.find(aa.GetId());
     auto it4=TP.rbasis.find(b2.GetId());
     size_t StateNr= Position(*it4)*TP.lbasis.dim +Position(*it3);
     //       std::cout<< TP << std::endl;
     //    std::cout<< b2.GetId()<<std::endl;
     // 	std::cout<< b2.GetId()<<std::endl;
      std::cout<<"state nr "<< StateNr<<std::endl;
         Eigen::VectorXcd inistate(TP.dim);
	 double para=1;

 	 Mat O0=Operators::NumberOperatore_1(TP, e, para, 0,  PB);
	 Mat O1=Operators::NumberOperatore_1(TP, e, para, 1,  PB);
	 Mat Nph0=Operators::NumberOperatorph_1(TP, ph, para, 0,  PB);
	 Mat Nph1=Operators::NumberOperatorph_1(TP, ph, para, 1,  PB);
	 Mat X0=std::sqrt(1./2)*(Operators::BosonDOperator_1(TP, ph, para,0,  PB)+Operators::BosonCOperator_1(TP, ph, para,0,  PB));
	 Mat X1=std::sqrt(1./2)*(Operators::BosonDOperator_1(TP, ph, para,1,  PB)+Operators::BosonCOperator_1(TP, ph, para,1,  PB));
	 Mat P0=std::sqrt(1./2)*(Operators::BosonCOperator_1(TP, ph, para,0,  PB)-Operators::BosonDOperator_1(TP, ph, para,0,  PB));
	 Mat P1=std::sqrt(1./2)*(Operators::BosonCOperator_1(TP, ph, para,1,  PB)-Operators::BosonDOperator_1(TP, ph, para,1,  PB));
	 Mat Q1=X0-X1;
	 Mat QP1=P0-P1;
	 Mat Q2=Q1*(X0-X1);
	 Mat QP2=QP1*(P0-P1);
	 Mat Q3=Q2*(X0-X1);
	 Mat QP3=QP2*(P0-P1);
	 Mat Q4=Q3*(X0-X1);
	 Mat QP4=QP3*(P0-P1);
	 //
   inistate.setZero();

   inistate[StateNr]=1;
   	          Eigen::VectorXcd i0=inistate;
		  std::cout<<"dim " <<e.dim << std::endl;

           std::cout<< TP.dim << std::endl;
      Mat EK=Operators::EKinOperatorL(TP, e, t0, PB);
      Mat Ebdag=Operators::NBosonCOperator(TP, ph, gamma, PB);
      Mat Eb=Operators::NBosonDOperator(TP, ph, gamma, PB);
      Mat Eph=Operators::NumberOperator(TP, ph, omega,  PB);

      //Mat E=Operators::NumberOperatore(TP, e, 1, false);
      //    std::cout<< HH << std::xbendl;
      Eigen::VectorXd eigenVals(TP.dim);
      Mat H=EK+Eph  +Ebdag + Eb;
      // making one Hamiltonian with infinite chem pot on one site
       double mu=100000;
       Eigen::VectorXd eigenVals_mit_pot(TP.dim);
       Mat H_mit_pot=H+Operators::NumberOperatore_1(TP, e, mu, 1,  PB);
      //

    Eigen::MatrixXd HH=Eigen::MatrixXd(H);
        Eigen::MatrixXd HH_mit_pot=Eigen::MatrixXd(H_mit_pot);
     Eigen::MatrixXd H_cop=Eigen::MatrixXd(H);
     Eigen::MatrixXd H_mit_pot_cop=Eigen::MatrixXd(H_mit_pot);
    //        std::cout<< HH<<std::endl;
        Eigen::MatrixXd N=Eigen::MatrixXd(Eph);

     Many_Body::diagMat(HH, eigenVals);
     Many_Body::diagMat(HH_mit_pot, eigenVals_mit_pot);
         Eigen::VectorXd energy(TP.dim);
	 std::vector<double> ek_vec;
    	   std::vector<double> n0_vec;
	   std::vector<double> n1_vec;
	   std::vector<double> nph0_vec;
	   std::vector<double> nph1_vec;
	   std::vector<double> x0_vec;
	   std::vector<double> x1_vec;
	   std::vector<double> p0_vec;
	   std::vector<double> p1_vec;
	   std::vector<double> q1_vec;
	   std::vector<double> qp1_vec;
	   std::vector<double> q2_vec;
	   std::vector<double> qp2_vec;
	   std::vector<double> q3_vec;
	   std::vector<double> qp3_vec;
	   std::vector<double> q4_vec;
	   std::vector<double> qp4_vec;
	   std::vector<double> ensvec_vec;
	   //	   Eigen::MatrixXd PHD2=HH.adjoint()*N.selfadjointView<Lower>()*HH;
	   std::cout<< "eigenVals(0) "<<eigenVals(0)<<std::endl;
	   //	    std::cout<< "eigenValseigenVals_mit_pot(0) "<<eigenVals_mit_pot(0)<<std::endl;
     Eigen::MatrixXcd evExp=TimeEv::EigenvalExponent(eigenVals, dt);
   Eigen::MatrixXcd cEVec=HH.cast<std::complex<double>>();
    Eigen::MatrixXcd cEVec_mit_pot=HH_mit_pot.cast<std::complex<double>>();
   Eigen::VectorXcd newIn=cEVec_mit_pot.col(0);
   //

      //
   int i=0;

   // O=H;
   std::complex<double> E_in=(newIn.adjoint()*(H_cop*newIn))(0);
   std::complex<double> X_in=(newIn.adjoint()*(X0*newIn))(0);
      std::complex<double> n_in=(newIn.adjoint()*(O0*newIn))(0);
           std::complex<double> N_in=(newIn.adjoint()*(Nph0*newIn))(0);
   std::cout<< " init energy "<<E_in<<std::endl;
   std::cout<< " init n site 0 "<<n_in<<std::endl;
   std::cout<< " init N site 0 "<<N_in<<std::endl;
      std::cout<< " init X site 0 "<<X_in<<std::endl;
        while(i*dt<tot)
       {

  	   	 std::complex<double> ek=(newIn.adjoint()*(EK*newIn))(0);
   	  	 std::complex<double> n0=(newIn.adjoint()*(O0*newIn))(0);
 		 std::complex<double> nph0=(newIn.adjoint()*(Nph0*newIn))(0);
 		 std::complex<double> nph1=(newIn.adjoint()*(Nph1*newIn))(0);
 		 std::complex<double> e0=(newIn.adjoint()*(H_cop*newIn))(0);
 		 std::complex<double> n1=(newIn.adjoint()*(O1*newIn))(0);
 		 std::complex<double> x0=(newIn.adjoint()*(X0*newIn))(0);
 		 std::complex<double> x1=(newIn.adjoint()*(X1*newIn))(0);
 		 std::complex<double> p0=std::complex<double>(0,1)*(newIn.adjoint()*(P0*newIn))(0);
 		 std::complex<double> p1=std::complex<double>(0,1)*(newIn.adjoint()*(P1*newIn))(0);
 		 std::complex<double> q1=(newIn.adjoint()*(Q1*newIn))(0);
 		 std::complex<double> qp1=std::complex<double>(0,1)*(newIn.adjoint()*(QP1*newIn))(0);
 		 std::complex<double> q2=(newIn.adjoint()*(Q2*newIn))(0);
 		 std::complex<double> qp2=std::complex<double>(0,1)*std::complex<double>(0,1)*(newIn.adjoint()*(QP2*newIn))(0);
 		 std::complex<double> q3=(newIn.adjoint()*(Q3*newIn))(0);
 		 std::complex<double> qp3=std::complex<double>(0,1)*std::complex<double>(0,1)*std::complex<double>(0,1)*(newIn.adjoint()*(QP3*newIn))(0);
 		 std::complex<double> q4=(newIn.adjoint()*(Q4*newIn))(0);
 		 std::complex<double> qp4=std::complex<double>(0,1)*std::complex<double>(0,1)*std::complex<double>(0,1)*std::complex<double>(0,1)*(newIn.adjoint()*(QP4*newIn))(0);
		 ek_vec.push_back(real(ek));
 		 n0_vec.push_back(real(n0));
 		 n1_vec.push_back(real(n1));
 		 nph0_vec.push_back(real(nph0));
 		 nph1_vec.push_back(real(nph1));
 		 x0_vec.push_back(real(x0));
 		 x1_vec.push_back(real(x1));
 		 p0_vec.push_back(real(p0));
 		 p1_vec.push_back(real(p1));
 		 q1_vec.push_back(real(q1));
 		 qp1_vec.push_back(real(qp1));
 		 q2_vec.push_back(real(q2));
 		 qp2_vec.push_back(real(qp2));
 		 q3_vec.push_back(real(q3));
 		 qp3_vec.push_back(real(qp3));
 		 q4_vec.push_back(real(q4));
 		 qp4_vec.push_back(real(qp4));
 		 //	 std::complex<double> c=(inistate.adjoint()*(newIn))(0);

   			// 	outputVals(i)=real(c);
   			// 	outputVals2(i)=real(c2);
        		// outputTime(i)=i*dt;
 		 std::cout<< std::setprecision(8)<< " ene "<< E_in-e0<< " partcle "<<n0 +n1<<"  " << x0 <<x1 << "  " << p0 << "  "  << p1 <<"  "<< " dt "<< i*dt<< std::endl;
 	 //	  	 BOOST_CHECK(std::abs(real(c2)-real(c))<Many_Body::err);
 	 TimeEv::timeev_exact(newIn, cEVec, evExp);
   	 i++;
 	     	             }

 //std::cout<< MatrixXd(OBS2) << std::endl;
 // Eigen::MatrixXd OBS22=HH.adjoint()*(OBS2.selfadjointView<Lower>())*HH;
 // Eigen::VectorXd diagobs2=OBS22.diagonal();
 	std::cout<< n0_vec.size()<< "  i "<< i << std::endl;
 // std::cout<<diagobs2(0)<<std::endl;
   filename+=".bin";
   bin_write(dirname+"/ek"+filename, ek_vec);
         bin_write(dirname+"/n0"+filename, n0_vec);
 	   bin_write(dirname+"/n1"+filename, n1_vec);
 	   bin_write(dirname+"/nph0"+filename, nph0_vec);
 	   bin_write(dirname+"/nph1"+filename, nph1_vec);
 	     bin_write(dirname+"/x0"+filename, x0_vec);
 	     bin_write(dirname+"/x1"+filename, x1_vec);
 	       bin_write(dirname+"/p0"+filename, p0_vec);
 	         bin_write(dirname+"/p1"+filename, p1_vec);
 		 bin_write(dirname+"/q1"+filename, q1_vec);
 		 bin_write(dirname+"/qp1"+filename, qp1_vec);
 		 bin_write(dirname+"/q2"+filename, q2_vec);
 		 bin_write(dirname+"/qp2"+filename, qp2_vec);
 		 bin_write(dirname+"/q3"+filename, q3_vec);
 		 bin_write(dirname+"/qp3"+filename, qp3_vec);
 		 bin_write(dirname+"/q4"+filename, q4_vec);
 		 bin_write(dirname+"/qp4"+filename, qp4_vec);
  return 0;
}


//!
//!@ detals     Test the main.cpp
//!
//!@ author     Miran Kim
//!@ date       Dec.1, 2017

#include <cmath>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <sys/time.h>
#include <chrono>

#include <NTL/RR.h>
#include <NTL/xdouble.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include "NTL/RR.h"
#include "NTL/vec_RR.h"
#include "NTL/mat_RR.h"
#include <NTL/BasicThreadPool.h>

#include <NTL/mat_ZZ.h>
#include <NTL/mat_poly_ZZ.h>
#include <NTL/ZZXFactoring.h>

#include "../HEAAN/Context.h"
#include "../HEAAN/Scheme.h"
#include "../HEAAN/SecretKey.h"
#include "../HEAAN/TimeUtils.h"

#include "matrix.h"
#include "HEmatrix.h"
#include "TestHEmatrix.h"

using namespace NTL;
using namespace std;

int main() {

    SetNumThreads(4);

    long dim, dim1, dim2;        //! dimension of (square) matrix
    long nbatching;   //! number of multiple matrices in a single ciphertext
    long niter;       //! number of iterations

    while (true)
    {
        cout << "+------------------------------------------+" << endl;
        cout << "| Basic Examples                           |" << endl;
        cout << "+------------------------------------------+" << endl;
        cout << "| 1. testHEAAN                             |" << endl;
        cout << "| 2. testEnc                               |" << endl;
        cout << "| 3. Matrix addition                       |" << endl;
        cout << "| 4. Matrix transposition                  |" << endl;
        cout << "| 5. Matrix multiplication                 |" << endl;
        cout << "| 6. Rectangular matrix multiplication     |" << endl;
        cout << "+------------------------------------------+" << endl;
        cout << "+------------------------------------------+" << endl;
        cout << "| Pre-processing Examples                  |" << endl;
        cout << "+------------------------------------------+" << endl;
        cout << "| 7. Matrix multiplication                 |" << endl;
        cout << "| 8. Rectangular matrix multiplication     |" << endl;
        cout << "+------------------------------------------+" << endl;
        cout << "+------------------------------------------+" << endl;
        cout << "| Parallel Computation Examples            |" << endl;
        cout << "+------------------------------------------+" << endl;
        cout << "| 9. Parallel Matrix addition              |" << endl;
        cout << "| 10. Parallel Matrix transposition        |" << endl;
        cout << "| 11. Parallel Matrix multiplication       |" << endl;
        cout << "+------------------------------------------+" << endl;

        int selection = 0;
        bool invalid = true;
        do
        {
            cout << endl << "> Run example (1 ~ 11) or exit (0): ";
            if (!(cin >> selection))
            {
                invalid = false;
            }
            else if (selection < 0 || selection > 11)
            {
                invalid = false;
            }
            else
            {
                invalid = true;
            }
            if (!invalid)
            {
                cout << "  Invalid option: type 0 ~ 11" << endl;
                cin.clear();
            }
        } while (!invalid);

        switch (selection)
        {
            case 1:
                cout << endl << "> Enter log(N) and loq(q): ";
                long logn, logq;
                cin >> logn;
                cin >> logq;
                TestHEmatrix::testHEAAN(logn, logq);
                break;

            case 2:
                cout << endl << "> Enter a dim of matrix (as power-of-two): ";
                cin >> dim;
                TestHEmatrix::testEnc(dim);
                break;

            case 3:
                cout << endl << "> Enter a dim of matrix (as power-of-two): ";
                cin >> dim;
                TestHEmatrix::testAdd(dim);
                break;

            case 4:
                cout << endl << "> Enter a dim of matrix (as power-of-two): ";
                cin >> dim;
                TestHEmatrix::testTrans(dim);
                break;

            case 5:
                cout << endl << "> Enter a dim of matrix (as power-of-two): ";
                cin >> dim;
                TestHEmatrix::testMult(dim);
                break;

            case 6:
                cout << endl << "> Enter dim-1 & dim-2 (dim-1 >= dim-2, e.g., 32 8): ";
                cin >> dim1;
                cin >> dim2;
                cout << "(" << dim2 << "," << dim1 << ") * (" << dim1 << "," << dim1 << ")" << endl;
                TestHEmatrix::testRMult(dim1, dim2); //! (d2 * d1) * (d1 * d1)
                break;

            case 7:
                cout << endl << "> Enter a dim of matrix (as power-of-two): ";
                cin >> dim;
                TestHEmatrix::testMult_preprocessing(dim);
                break;

            case 8:
                cout << endl << "> Enter dim-1 & dim-2 (dim-1 >= dim-2, e.g., 32 8): ";
                cin >> dim1 >> dim2;
                cout << "(" << dim2 << "," << dim1 << ") * (" << dim1 << "," << dim1 << ")" << endl;
                TestHEmatrix::testRMult_preprocessing(dim1, dim2); //! (d1 * d2) * (d2 * d2)
                break;

            case 9:
                cout << endl << "> Enter dim & #(parallel matrices) & #(iterations): ";
                cin >> dim >> nbatching >> niter;
                TestHEmatrix::testSIMDAdd(dim, nbatching, niter);
                break;

            case 10:
                cout << endl << "> Enter dim & #(parallel matrices) & #(iterations): ";
                cin >> dim >> nbatching >> niter;
                TestHEmatrix::testSIMDTrans(dim, nbatching, niter);
                break;

            case 11:
                cout << endl << "> Enter dim & #(parallel matrices) & #(iterations): ";
                cin >> dim >> nbatching >> niter;
                TestHEmatrix::testSIMDMult(dim, nbatching, niter);  //! square matrix
                break;

            case 0:
                return 0;
        }
    }

    return 0;
}


#ifndef STAN_MATH_TORSTEN_PKMODEL_PRED_PREDSS_ONECPT_HPP
#define STAN_MATH_TORSTEN_PKMODEL_PRED_PREDSS_ONECPT_HPP

#include <stan/math/torsten/PKModel/Pred/PolyExp.hpp>
#include <stan/math/torsten/PKModel/functors/check_mti.hpp>
#include <stan/math/torsten/PKModel/ModelParameters.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <boost/math/tools/promotion.hpp>
#include <iostream>
#include <vector>
#include <limits>

namespace torsten {

struct PredSS_oneCpt {
  PredSS_oneCpt() { }

  /**
   * One compartment model with first-order absorption.
   * Calculate amount in each compartment at the end of a steady-state dosing interval
   * or during a steady-state constant input (if ii=0)
   *
   * If the initial time equals the time of the event, than the code does
   * not run the ode integrator, and sets the predicted amount equal to the
   * initial condition. This can happen when we are dealing with events that
   * occur simultaneously. The change to the predicted amount caused by bolus
   * dosing events is handled later in the main Pred function.
   *
   * @tparam T_time type of scalar for time
   * @tparam T_amt type of scalar for amount
   * @tparam T_rate type of scalar for rate
   * @tparam T_ii type of scalar for interdose interval
   * @tparam T_parameters type of scalar for model parameters
   * @tparam T_addParm type of scalar for additional model parameters
   * @param[in] parameter model parameters at current event
   * @param[in] rate
   * @param[in] ii interdose interval
   * @param[in] cmt compartment in which the event occurs
   * @return an eigen vector that contains predicted amount in each compartment
   *   at the current event.
   */
  template<typename T_time, typename T_amt, typename T_rate, typename T_ii,
           typename T_parameters, typename T_biovar, typename T_tlag>
  Eigen::Matrix<typename boost::math::tools::promote_args<T_amt, T_rate,
    T_ii, T_parameters>::type, 1, Eigen::Dynamic>
  operator()(const ModelParameters<T_time, T_parameters, T_biovar,
             T_tlag>& parameter,
             const T_amt& amt,
             const T_rate& rate,
             const T_ii& ii,
             const int& cmt) const {
    typedef typename boost::math::tools::promote_args<T_amt, T_rate,
      T_ii, T_parameters>::type scalar;

    double inf = std::numeric_limits<double>::max();  // "infinity"

    T_parameters CL = parameter.get_RealParameters(false)[0],
      V2 = parameter.get_RealParameters(false)[1],
      ka = parameter.get_RealParameters(false)[2],
      k10 = CL/V2;

    std::vector<scalar> alpha(2, 0);
    alpha[0] = k10;
    alpha[1] = ka;

    Eigen::Matrix<scalar, 1, Eigen::Dynamic> pred
      = Eigen::Matrix<scalar, 1, Eigen::Dynamic>::Zero(2);
    std::vector<scalar> a(2, 0);
    if (rate == 0) {  // bolus dose
      if (cmt == 1) {
        a[0] = 0;
        a[1] = 1;
        pred(0) = PolyExp(ii, amt, 0, 0, ii, true, a, alpha, 2);
        a[0] = ka / (ka - alpha[0]);
        a[1] = -a[0];
        pred(1) = PolyExp(ii, amt, 0, 0, ii, true, a, alpha, 2);
      } else {  // cmt=2
        a[0] = 1;
        pred(1) = PolyExp(ii, amt, 0, 0, ii, true, a, alpha, 1);
      }
    } else if (ii > 0) {  // multiple truncated infusions
      double delta = unpromote(amt / rate);
      static const char* function("Steady State Event");
      check_mti(amt, delta, ii, function);

      if (cmt == 1) {
        a[0] = 0;
        a[1] = 1;
        pred(0) = PolyExp(ii, 0, rate, amt / rate, ii, true, a, alpha, 2);
        a[0] = ka / (ka - alpha[0]);
        a[1] = -a[0];
        pred(1) = PolyExp(ii, 0, rate, amt / rate, ii, true, a, alpha, 2);
      } else {  // cmt = 2
        a[0] = 1;
        pred(1) = PolyExp(ii, 0, rate, amt / rate, ii, true, a, alpha, 1);
      }
    } else {  // constant infusion
      if (cmt == 1) {
        a[0] = 0;
        a[1] = 1;
        pred(0) = PolyExp(0, 0, rate, inf, 0, true, a, alpha, 2);
        a[0] = ka / (ka - alpha[0]);
        a[1] = -a[0];
        pred(1) = PolyExp(0, 0, rate, inf, 0, true, a, alpha, 2);
      } else {  // cmt = 2
        a[0] = 1;
        pred(1) = PolyExp(0, 0, rate, inf, 0, true, a, alpha, 1);
      }
    }
    return pred;
  }
};

}
#endif


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/matrix/tensor_rank_2.h>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>

namespace scitbx { namespace matrix { namespace boost_python {

namespace {

  void wrap_tensor_rank_2()
  {
    using namespace boost::python;

    def("tensor_rank_2_gradient_transform",
      (sym_mat3<double>(*)(
        mat3<double> const& a,
        sym_mat3<double> const& g))
      tensor_rank_2::gradient_transform, (arg("a"), arg("g")));

    def("tensor_rank_2_gradient_transform_matrix",
      (af::versa<double, af::c_grid<2> >(*)(
        mat3<double> const& a))
      tensor_rank_2::gradient_transform_matrix, (arg("a")));
  }

}}} // namespace matrix::boost_python::<anonymous>

namespace math { namespace boost_python {

  void wrap_tensor_rank_2()
  {
    matrix::boost_python::wrap_tensor_rank_2();
  }

}}} // namespace scitbx::math::boost_python


/*
 * ConvolutionMeasure.cpp
 *
 *  Created on: 31.08.2017
 *      Author: thies
 */

#include <base/Norm.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/timer.h>
#include <measurements/ConvolutionMeasure.h>
#include <measurements/SensorValues.h>
#include <stddef.h>
#include <cmath>
#include <iostream>
#include <string>
#include <vector>

namespace wavepi {
namespace measurements {

using namespace dealii;
using namespace wavepi::base;

template <int dim>
ConvolutionMeasure<dim>::ConvolutionMeasure(std::shared_ptr<SpaceTimeMesh<dim>> mesh,
                                            std::shared_ptr<SensorDistribution<dim>> points,
                                            std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm,
                                            std::shared_ptr<LightFunction<dim>> delta_shape, double delta_scale_space,
                                            double delta_scale_time)
    : mesh(mesh),
      sensor_distribution(points),
      norm(norm),
      delta_shape(delta_shape),
      delta_scale_space(delta_scale_space),
      delta_scale_time(delta_scale_time) {
  AssertThrow(mesh && delta_shape && norm, ExcNotInitialized());
}

template <int dim>
ConvolutionMeasure<dim>::ConvolutionMeasure(std::shared_ptr<SpaceTimeMesh<dim>> mesh,
                                            std::shared_ptr<SensorDistribution<dim>> points,
                                            std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm)
    : mesh(mesh),
      sensor_distribution(points),
      norm(norm),
      delta_shape(),
      delta_scale_space(0.0),
      delta_scale_time(0.0) {
  AssertThrow(mesh && norm, ExcNotInitialized());
}

template <int dim>
std::vector<std::vector<std::pair<size_t, double>>> ConvolutionMeasure<dim>::compute_jobs() const {
  std::vector<std::vector<std::pair<size_t, double>>> jobs(mesh->length());

  size_t sensor_offset = 0;
  for (size_t mti = 0; mti < sensor_distribution->get_times().size(); mti++) {
    double mtime = sensor_distribution->get_times()[mti];

    for (size_t ti = 0; ti < mesh->get_times().size(); ti++) {
      double t = mesh->get_time(ti);

      if (t < mtime - delta_scale_time) continue;
      if (t > mtime + delta_scale_time) break;

      double factor = 0.0;

      if (ti > 0) factor += (t - mesh->get_time(ti - 1)) / 2;

      if (ti + 1 < mesh->get_times().size()) factor += (mesh->get_time(ti + 1) - t) / 2;

      // norm correction
      factor *= delta_scale_time * pow(delta_scale_space, dim);

      for (size_t msi = 0; msi < sensor_distribution->get_points_per_time(mti).size(); msi++)
        jobs[ti].emplace_back(msi + sensor_offset, factor);
    }

    sensor_offset += sensor_distribution->get_points_per_time(mti).size();
  }

  return jobs;
}

template <int dim>
SensorValues<dim> ConvolutionMeasure<dim>::evaluate(const DiscretizedFunction<dim>& field) {
  AssertThrow(delta_shape && delta_scale_space > 0 && delta_scale_time > 0, ExcNotInitialized());
  AssertThrow(sensor_distribution && sensor_distribution->size(), ExcNotInitialized());
  AssertThrow(mesh == field.get_mesh(), ExcMessage("ConvolutionMeasure called with different meshes"));
  AssertThrow(*norm == *field.get_norm(), ExcMessage("ConvolutionMeasure called with different norms"));

  LightFunctionWrapper wrapper(delta_shape, delta_scale_space, delta_scale_time);
  SensorValues<dim> res(sensor_distribution);

  auto jobs = compute_jobs();

  for (size_t ji = 0; ji < jobs.size(); ji++) {
    auto dof = field.get_mesh()->get_dof_handler(ji);
    Vector<double> interp_shape(dof->n_dofs());

    wrapper.set_time(mesh->get_time(ji));

    for (size_t k = 0; k < jobs[ji].size(); k++) {
      wrapper.set_offset((*sensor_distribution)[jobs[ji][k].first]);

      interp_shape = 0.0;
      VectorTools::interpolate(*dof, wrapper, interp_shape);
      mesh->get_constraint_matrix(ji)->distribute(interp_shape);

      res[jobs[ji][k].first] +=
          jobs[ji][k].second * mesh->get_mass_matrix(ji)->matrix_scalar_product(interp_shape, field[ji]);
    }
  }

  return res;
}

template <int dim>
SensorValues<dim> ConvolutionMeasure<dim>::zero() {
  return SensorValues<dim>(sensor_distribution);
}

template <int dim>
DiscretizedFunction<dim> ConvolutionMeasure<dim>::adjoint(const SensorValues<dim>& measurements) {
  AssertThrow(delta_shape && delta_scale_space > 0 && delta_scale_time > 0, ExcNotInitialized());
  AssertThrow(mesh && sensor_distribution && sensor_distribution->size(), ExcNotInitialized());

  DiscretizedFunction<dim> res(mesh);
  auto jobs = compute_jobs();

  LightFunctionWrapper wrapper(delta_shape, delta_scale_space, delta_scale_time);

  for (size_t ji = 0; ji < jobs.size(); ji++) {
    auto dof_handler = mesh->get_dof_handler(ji);
    wrapper.set_time(mesh->get_time(ji));
    Vector<double> tmp(dof_handler->n_dofs());

    for (size_t i = 0; i < jobs[ji].size(); i++) {
      size_t sensor_idx = jobs[ji][i].first;

      wrapper.set_offset((*sensor_distribution)[sensor_idx]);

      tmp = 0.0;

      // interpolate makes sense if the forward measurement operator also uses the interpolation.
      VectorTools::interpolate(*dof_handler, wrapper, tmp);
      mesh->get_constraint_matrix(ji)->distribute(tmp);

      res[ji].add(jobs[ji][i].second * measurements[sensor_idx], tmp);
    }
  }

  res.set_norm(norm);

  // res has coefficients in there, not dot products.
  res.dot_mult_mass_and_transform_inverse();

  return res;
}

template <int dim>
void ConvolutionMeasure<dim>::declare_parameters(ParameterHandler& prm) {
  prm.enter_subsection("ConvolutionMeasure");
  {
    prm.declare_entry("radius space", "0.2", Patterns::Double(0), "scaling of shape function in spatial variables");
    prm.declare_entry("radius time", "0.2", Patterns::Double(0), "scaling of shape function in time variable");
    prm.declare_entry("shape", "hat", Patterns::Selection("hat|constant"),
                      "shape of the delta approximating function. ");
  }
  prm.leave_subsection();
}

template <int dim>
void ConvolutionMeasure<dim>::get_parameters(ParameterHandler& prm) {
  prm.enter_subsection("ConvolutionMeasure");
  {
    auto shape_desc = prm.get("shape");

    if (shape_desc == "hat")
      delta_shape = std::make_shared<HatShape>();
    else if (shape_desc == "constant")
      delta_shape = std::make_shared<ConstShape>();
    else
      AssertThrow(false, ExcMessage("Unknown delta shape: " + shape_desc));

    delta_scale_space = prm.get_double("radius space");
    delta_scale_time  = prm.get_double("radius time");

    AssertThrow(delta_scale_space * delta_scale_time > 0.0,
                ExcMessage("sensor radii in time and space have to be positive!"));
  }
  prm.leave_subsection();
}

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

}  // namespace measurements
} /* namespace wavepi */


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_ElasticElectronMomentsEvaluator.hpp
//! \author Luke Kersting
//! \brief  Elastic electron cross section moments evaluator declaration
//!
//---------------------------------------------------------------------------//

#ifndef DATA_GEN_ELASTIC_ELECTRON_MOMENTS_EVALUATOR_HPP
#define DATA_GEN_ELASTIC_ELECTRON_MOMENTS_EVALUATOR_HPP

// Boost Includes
#include <boost/multiprecision/cpp_dec_float.hpp>

// FRENSIE Includes
#include "Data_ElectronPhotonRelaxationDataContainer.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_SloanRadauQuadrature.hpp"
#include "MonteCarlo_CoupledElasticElectronScatteringDistribution.hpp"
#include "MonteCarlo_CoupledElasticElectroatomicReaction.hpp"
#include "MonteCarlo_TwoDInterpolationType.hpp"
#include "MonteCarlo_TwoDGridType.hpp"

namespace DataGen{

//! The elastic electron cross section moments class
class ElasticElectronMomentsEvaluator
{

public:

  //! Typedef for the elastic distribution
  typedef Utility::GaussKronrodIntegrator<Utility::long_float>
  Integrator;

  // Typedef for elastic electron traits
  typedef MonteCarlo::ElasticElectronTraits ElasticTraits;

  //! Constructor
  ElasticElectronMomentsEvaluator(
    const Data::ElectronPhotonRelaxationDataContainer& data_container,
    const MonteCarlo::TwoDInterpolationType two_d_interp,
    const MonteCarlo::TwoDGridType two_d_grid,
    const double cutoff_angle_cosine,
    const double tabular_evaluation_tol );

  //! Constructor (without data container)
  ElasticElectronMomentsEvaluator(
    const std::map<double,std::vector<double> >& cutoff_elastic_angles,
    const std::shared_ptr<const std::vector<double> >& incoming_energy_grid,
    const std::shared_ptr<const Utility::HashBasedGridSearcher<double> >&
     grid_searcher,
    const std::shared_ptr<const std::vector<double> >& cutoff_cross_section,
    const std::shared_ptr<const std::vector<double> >& total_elastic_cross_section,
    const unsigned screened_rutherford_threshold_energy_index,
    const std::shared_ptr<const MonteCarlo::CoupledElasticElectronScatteringDistribution>
        coupled_distribution,
    const std::shared_ptr<const ElasticTraits>& elastic_traits,
    const double cutoff_angle_cosine );

  //! Destructor
  ~ElasticElectronMomentsEvaluator()
  { /* ... */ }

  //! Evaluate the Legendre Polynomial expansion of the screened rutherford pdf
  double evaluateLegendreExpandedRutherford(
            const double scattering_angle_cosine,
            const double incoming_energy,
            const int polynomial_order = 0 ) const;

  //! Evaluate the Legendre Polynomial expansion of the screened rutherford pdf
  double evaluateLegendreExpandedRutherford(
            const double scattering_angle_cosine,
            const double incoming_energy,
            const double eta,
            const int polynomial_order = 0 ) const;

  //! Evaluate the Legendre Polynomial expansion of the differential hard elastic pdf
  double evaluateLegendreExpandedPDF(
            const double scattering_angle_cosine,
            const double incoming_energy,
            const int polynomial_order = 0) const;

  //! Evaluate the nth cross section moment of the elastic cutoff distribution at the energy
  void evaluateCutoffMoment(
            Utility::long_float& cutoff_moment,
            const std::vector<double>& angular_grid,
            const Integrator& integrator,
            const double energy,
            const int n ) const;

  //! Evaluate the nth cross section moment of the screened Rutherford peak distribution at the energy
  void evaluateScreenedRutherfordMoment(
            Utility::long_float& rutherford_moment,
            const double& energy,
            const int& n ) const;

  //! Evaluate the first n moments of the elastic scattering distribution at a given energy
  void evaluateElasticMoment(
            std::vector<Utility::long_float>& legendre_moments,
            const double energy,
            const int n,
            const double precision ) const;

protected:

  // Evaluate the nth PDF moment of the cutoff distribution at the energy
  void evaluateCutoffPDFMoment(
            Utility::long_float& cutoff_moment,
            const std::vector<double>& angular_grid,
            const Integrator& integrator,
            const double energy,
            const int n ) const;

  // Evaluate the nth PDF moment of the screened Rutherford peak distribution at the energy
  void evaluateScreenedRutherfordPDFMoment(
            Utility::long_float& rutherford_moment,
            const double& energy,
            const int& n ) const;

  /* Evaluate the nth PDF moment of the screened Rutherford peak distribution
   * at the eta using the recursion relationship
   */
  void evaluateScreenedRutherfordPDFMomentByRecursion(
            Utility::long_float& rutherford_moment,
            const Utility::long_float& eta,
            const int& n ) const;

  /* Evaluate the nth PDF moment of the screened Rutherford peak distribution
   * at the energy using numerical integration
   */
  void evaluateScreenedRutherfordPDFMomentByNumericalIntegration(
            Utility::long_float& rutherford_moment,
            const double energy,
            const int n,
            const double tolerance = 1e-13,
            const unsigned number_of_iterations = 1000 ) const;

  /* Evaluate the nth PDF moment of the screened Rutherford peak distribution
   * at the energy using numerical integration
   */
  void evaluateScreenedRutherfordPDFMomentByNumericalIntegration(
            Utility::long_float& rutherford_moment,
            const Utility::long_float& eta,
            const double energy,
            const int n,
            const double tolerance = 1e-13,
            const unsigned number_of_iterations = 1000 ) const;

private:

  // Return the angular integration points
  void getAngularIntegrationPoints(
        std::vector<double>& angular_integration_points,
        const double energy ) const;

  // Grid searcher for the energy grid
  std::shared_ptr<const Utility::HashBasedGridSearcher<double> > d_grid_searcher;

  // The screened rutherford elastic threshold_energy_index
  unsigned d_screened_rutherford_threshold_energy_index;

  // The coupled distribution
  std::shared_ptr<const MonteCarlo::CoupledElasticElectronScatteringDistribution>
    d_coupled_distribution;

  // The cutoff elastic reaction
  std::shared_ptr<const MonteCarlo::ElectroatomicReaction>
    d_cutoff_reaction;

  // The total elastic reaction
  std::shared_ptr<const MonteCarlo::ElectroatomicReaction>
    d_total_reaction;

  // The map of the cutoff angles
  std::map<double,std::vector<double> > d_cutoff_elastic_angles;

  // The angle cosine cutoff between hard and soft scattering
  double d_cutoff_angle_cosine;

  // Elastic electron traits
  std::shared_ptr<const ElasticTraits> d_elastic_traits;
};

} // end DataGen namespace

#endif // end DATA_GEN_ELASTIC_ELECTRON_MOMENTS_EVALUATOR_HPP

//---------------------------------------------------------------------------//
// end DataGen_ElasticElectronMomentsEvaluator.hpp
//---------------------------------------------------------------------------//