 #include <time.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/search/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/gicp.h>

#include <fstream>
#include <chrono>
#include <string>
#include <Eigen/Dense>

typedef  pcl::PointXYZ   PointType;

class LidarsCalibrater{
    private:
        ros::NodeHandle nh;
        ros::Subscriber sub_avia;
        ros::Subscriber sub_hori;
        ros::Subscriber sub_velo;
        ros::Subscriber sub_os0;
        ros::Subscriber sub_os1;

        ros::Publisher pub_avia;
        ros::Publisher pub_hori;
        ros::Publisher pub_velo;
        ros::Publisher pub_os0;
        ros::Publisher pub_os1;

        tf::TransformBroadcaster tf_br;

        // avia tf
        int avia_itegrate_frames = 5;
        pcl::PointCloud<PointType> avia_igcloud;
        Eigen::Matrix4f avia_tf_matrix;
        bool avia_tf_initd = false;

        // Hori TF
        int hori_itegrate_frames = 5;
        pcl::PointCloud<PointType> hori_igcloud;
        Eigen::Matrix4f hori_tf_matrix;
        bool hori_tf_initd = false;

        // OS0 TF
        Eigen::Matrix4f os0_tf_matrix;
        bool os0_tf_initd = false;
        pcl::PointCloud<PointType> os0_cloud;
        bool os0_received = false;

        // OS1 TF
        Eigen::Matrix4f os1_tf_matrix;
        bool os1_tf_initd = false;

        // Velo TF
        Eigen::Matrix4f velo_tf_matrix;
        bool velo_tf_initd = false;

    public:
        LidarsCalibrater(){
            sub_avia = nh.subscribe<sensor_msgs::PointCloud2>("/livox_avia", 1000, &LidarsCalibrater::avia_cloud_handler, this);
            sub_hori = nh.subscribe<sensor_msgs::PointCloud2>("/livox_horizon", 1000, &LidarsCalibrater::hori_cloud_handler, this);
            sub_velo = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1000, &LidarsCalibrater::velo_cloud_handler, this);
            sub_os0 = nh.subscribe<sensor_msgs::PointCloud2>("/os_cloud_node/points", 1000, &LidarsCalibrater::os0_cloud_handler, this);
            sub_os1 = nh.subscribe<sensor_msgs::PointCloud2>("/os_cloud_nodee/points", 1000, &LidarsCalibrater::os1_cloud_handler, this);

            pub_avia    = nh.advertise<sensor_msgs::PointCloud2>("/a_avia", 1);
            pub_hori    = nh.advertise<sensor_msgs::PointCloud2>("/a_horizon", 1);
            pub_velo    = nh.advertise<sensor_msgs::PointCloud2>("/a_velo", 1);
            pub_os0     = nh.advertise<sensor_msgs::PointCloud2>("/a_os0", 1);
            pub_os1     = nh.advertise<sensor_msgs::PointCloud2>("/a_os1", 1);

            // avia_itegrate_frames = 10;
            // hori_itegrate_frames = 10;
        };

        ~LidarsCalibrater(){};

        void matrix_to_transform(Eigen::Matrix4f & matrix, tf::Transform & trans){
            tf::Vector3 origin;
            origin.setValue(static_cast<double>(matrix(0,3)),static_cast<double>(matrix(1,3)),static_cast<double>(matrix(2,3)));

            tf::Matrix3x3 tf3d;
            tf3d.setValue(static_cast<double>(matrix(0,0)), static_cast<double>(matrix(0,1)), static_cast<double>(matrix(0,2)),
            static_cast<double>(matrix(1,0)), static_cast<double>(matrix(1,1)), static_cast<double>(matrix(1,2)),
            static_cast<double>(matrix(2,0)), static_cast<double>(matrix(2,1)), static_cast<double>(matrix(2,2)));

            tf::Quaternion tfqt;
            tf3d.getRotation(tfqt);

            trans.setOrigin(origin);
            trans.setRotation(tfqt);
        }

        void removeFarPointCloud(const pcl::PointCloud<PointType> &cloud_in, pcl::PointCloud<PointType> &cloud_out, float thres)
        {
            if (&cloud_in != &cloud_out) {
                cloud_out.header = cloud_in.header;
                cloud_out.points.resize(cloud_in.points.size());
            }

            size_t j = 0;

            for (size_t i = 0; i < cloud_in.points.size(); ++i) {
                if (cloud_in.points[i].x * cloud_in.points[i].x +
                        cloud_in.points[i].y * cloud_in.points[i].y +
                        cloud_in.points[i].z * cloud_in.points[i].z > thres * thres)
                    continue;
                cloud_out.points[j] = cloud_in.points[i];
                j++;
            }
            if (j != cloud_in.points.size()) {
                cloud_out.points.resize(j);
            }

            cloud_out.height = 1;
            cloud_out.width = static_cast<uint32_t>(j);
            cloud_out.is_dense = true;
        }

        void avia_cloud_handler(const sensor_msgs::PointCloud2ConstPtr& pointCloudIn)
        {
            if(!os0_received) return;
            pcl::PointCloud<PointType>  full_cloud_in;
            pcl::fromROSMsg(*pointCloudIn, full_cloud_in);
            pcl_conversions::toPCL(pointCloudIn->header, full_cloud_in.header);

            if(avia_itegrate_frames > 0)
            {
                avia_igcloud += full_cloud_in;
                avia_itegrate_frames--;
                // ROS_INFO_STREAM("Avia cloud integrating: " << avia_itegrate_frames);
                return;
            }else
            {
                if(!avia_tf_initd){
                    Eigen::AngleAxisf init_rot_x( 0.0 , Eigen::Vector3f::UnitX());
                    Eigen::AngleAxisf init_rot_y( 0.0 , Eigen::Vector3f::UnitY());
                    Eigen::AngleAxisf init_rot_z( 0.0 , Eigen::Vector3f::UnitZ());

                    Eigen::Translation3f init_trans(0.0,0.0,0.0);
                    Eigen::Matrix4f init_tf = (init_trans * init_rot_z * init_rot_y * init_rot_x).matrix();

                     ROS_INFO("\n\n\n  Calibrate Avia ...");
                    calibrate_PCLICP(avia_igcloud.makeShared(), os0_cloud.makeShared(), avia_tf_matrix, false);
                    Eigen::Matrix3f rot_matrix = avia_tf_matrix.block(0,0,3,3);
                    Eigen::Vector3f trans_vector = avia_tf_matrix.block(0,3,3,1);

                    // tf::Transform t_transform;
                    // matrix_to_transform(avia_tf_matrix,t_transform);
                    // tf_br.sendTransform(tf::StampedTransform(t_transform, ros::Time::now(), "avia_frame", "base_link"));

                    std::cout << "Avia -> base_link " << trans_vector.transpose()
                        << " " << rot_matrix.eulerAngles(2,1,0).transpose() << " /" << "avia_frame"
                        << " /" << "base_link" << " 10" << std::endl;

                    // publish result
                    pcl::PointCloud<PointType>  out_cloud;
                    pcl::transformPointCloud (avia_igcloud , out_cloud, avia_tf_matrix);

                    sensor_msgs::PointCloud2 avia_msg;
                    pcl::toROSMsg(out_cloud, avia_msg);
                    avia_msg.header.stamp = ros::Time::now();
                    avia_msg.header.frame_id = "base_link";
                    pub_avia.publish(avia_msg);

                    avia_tf_initd = true;
                }else
                {
                    // tf::Transform t_transform;
                    // matrix_to_transform(avia_tf_matrix,t_transform);
                    // tf_br.sendTransform(tf::StampedTransform(t_transform, ros::Time::now(), "avia_frame", "base_link"));

                    pcl::PointCloud<PointType>  out_cloud;
                    pcl::transformPointCloud (full_cloud_in , out_cloud, avia_tf_matrix);

                    sensor_msgs::PointCloud2 avia_msg;
                    pcl::toROSMsg(out_cloud, avia_msg);
                    avia_msg.header.stamp = ros::Time::now();
                    avia_msg.header.frame_id = "base_link";
                    pub_avia.publish(avia_msg);
                }
            }
        }

        void hori_cloud_handler(const sensor_msgs::PointCloud2ConstPtr& pointCloudIn)
        {
            if(!os0_received) return;
            pcl::PointCloud<PointType>  full_cloud_in;
            pcl::fromROSMsg(*pointCloudIn, full_cloud_in);
            pcl_conversions::toPCL(pointCloudIn->header, full_cloud_in.header);

            if(hori_itegrate_frames > 0)
            {
                hori_igcloud += full_cloud_in;
                hori_itegrate_frames--;
                // ROS_INFO_STREAM("hori cloud integrating: " << hori_itegrate_frames);
                return;
            }else
            {
                if(!hori_tf_initd){
                    Eigen::AngleAxisf init_rot_x( 0.0 , Eigen::Vector3f::UnitX());
                    Eigen::AngleAxisf init_rot_y( 0.0 , Eigen::Vector3f::UnitY());
                    Eigen::AngleAxisf init_rot_z( 0.0 , Eigen::Vector3f::UnitZ());

                    Eigen::Translation3f init_trans(0.0,0.0,0.0);
                    Eigen::Matrix4f init_tf = (init_trans * init_rot_z * init_rot_y * init_rot_x).matrix();

                    // pcl::transformPointCloud (full_cloud , cloud_out, transformation_matrix);
                     ROS_INFO("\n\n\n  Calibrate Horizon ...");
                    calibrate_PCLICP(hori_igcloud.makeShared(), os0_cloud.makeShared(), hori_tf_matrix, false);
                    Eigen::Matrix3f rot_matrix = hori_tf_matrix.block(0,0,3,3);
                    Eigen::Vector3f trans_vector = hori_tf_matrix.block(0,3,3,1);

                    // tf::Transform t_transform;
                    // matrix_to_transform(hori_tf_matrix,t_transform);
                    // tf_br.sendTransform(tf::StampedTransform(t_transform, ros::Time::now(), "hori_frame", "base_link"));

                    std::cout << "hori -> base_link " << trans_vector.transpose()
                        << " " << rot_matrix.eulerAngles(2,1,0).transpose() << " /" << "hori_frame"
                        << " /" << "base_link" << " 10" << std::endl;

                    // publish result
                    pcl::PointCloud<PointType>  out_cloud;
                    pcl::transformPointCloud (hori_igcloud , out_cloud, hori_tf_matrix);

                    sensor_msgs::PointCloud2 hori_msg;
                    pcl::toROSMsg(out_cloud, hori_msg);
                    hori_msg.header.stamp = ros::Time::now();
                    hori_msg.header.frame_id = "base_link";
                    pub_hori.publish(hori_msg);

                    hori_tf_initd = true;
                }else
                {
                    // tf::Transform t_transform;
                    // matrix_to_transform(hori_tf_matrix,t_transform);
                    // tf_br.sendTransform(tf::StampedTransform(t_transform, ros::Time::now(), "hori_frame", "base_link"));

                    pcl::PointCloud<PointType>  out_cloud;
                    pcl::transformPointCloud (full_cloud_in , out_cloud, hori_tf_matrix);

                    sensor_msgs::PointCloud2 hori_msg;
                    pcl::toROSMsg(out_cloud, hori_msg);
                    hori_msg.header.stamp = ros::Time::now();
                    hori_msg.header.frame_id = "base_link";
                    pub_hori.publish(hori_msg);
                }
            }
        }

        void velo_cloud_handler(const sensor_msgs::PointCloud2ConstPtr& pointCloudIn)
        {
            if(!os0_received) return;

            pcl::PointCloud<PointType>  full_cloud_in;
            pcl::fromROSMsg(*pointCloudIn, full_cloud_in);
            pcl_conversions::toPCL(pointCloudIn->header, full_cloud_in.header);

            Eigen::AngleAxisf init_rot_x( 0.0 , Eigen::Vector3f::UnitX());
            Eigen::AngleAxisf init_rot_y( 0.0 , Eigen::Vector3f::UnitY());
            Eigen::AngleAxisf init_rot_z( 0.0 , Eigen::Vector3f::UnitZ());

            Eigen::Translation3f init_trans(0.0,0.0,0.0);
            Eigen::Matrix4f init_tf = (init_trans * init_rot_z * init_rot_y * init_rot_x).matrix();

            if(!velo_tf_initd){
                pcl::transformPointCloud (full_cloud_in , full_cloud_in, init_tf);

                 ROS_INFO("\n\n\n  Calibrate Velo ...");
                calibrate_PCLICP(full_cloud_in.makeShared(), os0_cloud.makeShared(), velo_tf_matrix);
                // velo_tf_matrix = velo_tf_matrix * init_tf;
                Eigen::Matrix3f rot_matrix = velo_tf_matrix.block(0,0,3,3);
                Eigen::Vector3f trans_vector = velo_tf_matrix.block(0,3,3,1);

                // tf::Transform t_transform;
                // matrix_to_transform(velo_tf_matrix,t_transform);
                // tf_br.sendTransform(tf::StampedTransform(t_transform, ros::Time::now(), "velo_sensor", "base_link"));

                std::cout << "velo -> base_link " << trans_vector.transpose()
                    << " " << rot_matrix.eulerAngles(2,1,0).transpose() << " /" << "velo_sensor"
                    << " /" << "base_link" << " 10" << std::endl;

                // publish result
                pcl::PointCloud<PointType>  out_cloud;
                pcl::transformPointCloud (full_cloud_in , out_cloud, velo_tf_matrix);

                sensor_msgs::PointCloud2 velo_msg;
                pcl::toROSMsg(out_cloud, velo_msg);
                velo_msg.header.stamp = ros::Time::now();
                velo_msg.header.frame_id = "base_link";
                pub_velo.publish(velo_msg);

                velo_tf_initd = true;
            }else
            {
                // tf::Transform t_transform;
                // matrix_to_transform(velo_tf_matrix,t_transform);
                // tf_br.sendTransform(tf::StampedTransform(t_transform, ros::Time::now(), "velo_sensor", "base_link"));

                pcl::PointCloud<PointType>  out_cloud;
                pcl::transformPointCloud (full_cloud_in , full_cloud_in, init_tf);
                pcl::transformPointCloud (full_cloud_in , out_cloud, velo_tf_matrix);

                sensor_msgs::PointCloud2 velo_msg;
                pcl::toROSMsg(out_cloud, velo_msg);
                velo_msg.header.stamp = ros::Time::now();
                velo_msg.header.frame_id = "base_link";
                pub_velo.publish(velo_msg);
            }
        }

        void os0_cloud_handler(const sensor_msgs::PointCloud2ConstPtr& pointCloudIn)
        {

            pcl::PointCloud<PointType>  full_cloud_in;
            pcl::fromROSMsg(*pointCloudIn, full_cloud_in);
            pcl_conversions::toPCL(pointCloudIn->header, full_cloud_in.header);

            Eigen::AngleAxisf init_rot_x( 0.0 , Eigen::Vector3f::UnitX());
            Eigen::AngleAxisf init_rot_y( 0.0 , Eigen::Vector3f::UnitY());
            Eigen::AngleAxisf init_rot_z(  M_PI / 4 , Eigen::Vector3f::UnitZ());
            Eigen::Translation3f init_trans(0.0,0.0,0.0);
            Eigen::Matrix4f init_tf = (init_trans * init_rot_z * init_rot_y * init_rot_x).matrix();

            Eigen::Matrix3f rot_matrix = init_tf.block(0,0,3,3);
            Eigen::Vector3f trans_vector = init_tf.block(0,3,3,1);

            if(!os0_tf_initd){
                 std::cout << "OS0 -> base_link " << trans_vector.transpose()
                    << " " << rot_matrix.eulerAngles(2,1,0).transpose() << " /" << "os0_sensor"
                    << " /" << "base_link" << " 10" << std::endl;

                os0_tf_initd = true;
            }

            pcl::PointCloud<PointType>  out_cloud;
            pcl::transformPointCloud (full_cloud_in , full_cloud_in, init_tf);

            os0_cloud.clear();
            os0_cloud += full_cloud_in;
            os0_received = true;
            // pcl::transformPointCloud (full_cloud_in , out_cloud, os0_tf_matrix);

            sensor_msgs::PointCloud2 os0_msg;
            pcl::toROSMsg(os0_cloud, os0_msg);
            os0_msg.header.stamp = ros::Time::now();
            os0_msg.header.frame_id = "base_link";
            pub_os0.publish(os0_msg);

        }

        void os1_cloud_handler(const sensor_msgs::PointCloud2ConstPtr& pointCloudIn)
        {
            if(!os0_received) return;

            pcl::PointCloud<PointType>  full_cloud_in;
            pcl::fromROSMsg(*pointCloudIn, full_cloud_in);
            pcl_conversions::toPCL(pointCloudIn->header, full_cloud_in.header);

            Eigen::AngleAxisf init_rot_x( 0.0 , Eigen::Vector3f::UnitX());
            Eigen::AngleAxisf init_rot_y( 0.0 , Eigen::Vector3f::UnitY());
            Eigen::AngleAxisf init_rot_z(  -M_PI / 4 , Eigen::Vector3f::UnitZ());

            Eigen::Translation3f init_trans(0.0,0.0,0.0);
            Eigen::Matrix4f init_tf = (init_trans * init_rot_z * init_rot_y * init_rot_x).matrix();

            if(!os1_tf_initd){
                pcl::transformPointCloud (full_cloud_in , full_cloud_in, init_tf);

                ROS_INFO("\n\n\n  Calibrate OS1 ...");
                calibrate_PCLICP(full_cloud_in.makeShared(), os0_cloud.makeShared(), os1_tf_matrix);
                os1_tf_matrix = os1_tf_matrix * init_tf;
                Eigen::Matrix3f rot_matrix = os1_tf_matrix.block(0,0,3,3);
                Eigen::Vector3f trans_vector = os1_tf_matrix.block(0,3,3,1);

                // tf::Transform t_transform;
                // matrix_to_transform(os1_tf_matrix,t_transform);
                // tf_br.sendTransform(tf::StampedTransform(t_transform, ros::Time::now(), "os1_sensor", "base_link"));

                std::cout << "OS1 -> base_link " << trans_vector.transpose()
                    << " " << rot_matrix.eulerAngles(2,1,0).transpose() << " /" << "os1_sensor"
                    << " /" << "base_link" << " 10" << std::endl;

                // publish result
                pcl::PointCloud<PointType>  out_cloud;
                pcl::transformPointCloud (full_cloud_in , out_cloud, os1_tf_matrix);

                sensor_msgs::PointCloud2 os1_msg;
                pcl::toROSMsg(out_cloud, os1_msg);
                os1_msg.header.stamp = ros::Time::now();
                os1_msg.header.frame_id = "base_link";
                pub_os1.publish(os1_msg);

                os1_tf_initd = true;
            }else
            {
                // tf::Transform t_transform;
                // matrix_to_transform(os1_tf_matrix,t_transform);
                // tf_br.sendTransform(tf::StampedTransform(t_transform, ros::Time::now(), "os1_sensor", "base_link"));

                pcl::PointCloud<PointType>  out_cloud;
                // pcl::transformPointCloud (full_cloud_in , full_cloud_in, init_tf);
                pcl::transformPointCloud (full_cloud_in , out_cloud, os1_tf_matrix);

                sensor_msgs::PointCloud2 os1_msg;
                pcl::toROSMsg(out_cloud, os1_msg);
                os1_msg.header.stamp = ros::Time::now();
                os1_msg.header.frame_id = "base_link";
                pub_os1.publish(os1_msg);
            }

        }

        void calibrate_PCLICP(pcl::PointCloud<PointType>::Ptr source_cloud,
        pcl::PointCloud<PointType>::Ptr target_cloud, Eigen::Matrix4f &tf_marix, bool save_pcd =true)
        {
            std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
            std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
            std::chrono::duration<double, std::ratio<1, 1000>> time_span =
            std::chrono::duration_cast<std::chrono::duration<double, std::ratio<1, 1000>>>(t2 - t1);

            std::cout << "------------checking PCL GICP---------------- "<< std::endl;
            int pCount = source_cloud->size();

            pcl::PointCloud<PointType>::Ptr ds_cloud_in (new pcl::PointCloud<PointType> );
            pcl::PointCloud<PointType>::Ptr dstf_cloud_in (new pcl::PointCloud<PointType> );
            pcl::PointCloud<PointType>::Ptr ds_cloud_out (new pcl::PointCloud<PointType> );

            // // Remove the noise points
            pcl::PointCloud<PointType>::Ptr cloudin_filtered (new pcl::PointCloud<PointType>);
            // pcl::StatisticalOutlierRemoval<PointType> sor;
            // sor.setInputCloud (source_cloud);
            // sor.setMeanK (50);
            // sor.setStddevMulThresh (1.0);
            // sor.filter (*cloudin_filtered);
            // ROS_INFO_STREAM("CloudIn-> Removing noise clouds "<< source_cloud->size() << " "<<  cloudin_filtered->size() );
            removeFarPointCloud(*source_cloud, *cloudin_filtered, 50);
            ROS_INFO_STREAM("CloudIn-> Removing noise clouds "<< source_cloud->size() << " "<<  cloudin_filtered->size() );

            // Remove the noise points
            pcl::PointCloud<PointType>::Ptr cloudout_filtered (new pcl::PointCloud<PointType>);
            // sor.setInputCloud (target_cloud);
            // sor.setMeanK (50);
            // sor.setStddevMulThresh (1.0);
            // sor.filter (*cloudout_filtered);
            // ROS_INFO_STREAM("CloudOut-> Removing noise clouds "<< target_cloud->size() << " "<<  cloudout_filtered->size() );
            removeFarPointCloud(*target_cloud, *cloudout_filtered, 50);
            ROS_INFO_STREAM("CloudOut-> Removing noise clouds "<< target_cloud->size() << " "<<  cloudout_filtered->size() );

            // Create the filtering object
            pcl::VoxelGrid< PointType> vox;
            vox.setInputCloud (cloudin_filtered);
            vox.setLeafSize (0.05f, 0.05f, 0.05f);
            vox.filter(*ds_cloud_in);
            // std::cout << "Source DS: " << source_cloud->size() << " ->  " << ds_cloud_in->size()<< std::endl;

            // Create the filtering object
            vox.setInputCloud (cloudout_filtered);
            vox.setLeafSize (0.05f, 0.05f, 0.05f);
            vox.filter(*ds_cloud_out);
            // std::cout << "Target DS: " << target_cloud->size() << " -> " << ds_cloud_out->size()<< std::endl;

            // Init Rotation:
            // std::cout << "Rotating DS source  .... " << ds_cloud_in->size()<< std::endl;
            // Eigen::Quaterniond rotate_tf =  Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
            //                                 * Eigen::AngleAxisd(0,  Eigen::Vector3d::UnitY())
            //                                 * Eigen::AngleAxisd( M_PI/4, Eigen::Vector3d::UnitZ());
            // Eigen::Vector3d    trans_tf {0,0,0};
            // *dstf_cloud_in =  *dstf_cloud_in + *(transformCloud(*ds_cloud_in, rotate_tf, trans_tf));
            *dstf_cloud_in =  *dstf_cloud_in + *ds_cloud_in;

            std::cout << "GICP start  .... " << ds_cloud_in->size() << " to "<< ds_cloud_out->size()<< std::endl;
            pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;
            gicp.setTransformationEpsilon(0.001);
            gicp.setMaxCorrespondenceDistance(2);
            gicp.setMaximumIterations(500);
            gicp.setRANSACIterations(12);
            gicp.setInputSource(dstf_cloud_in);
            gicp.setInputTarget(ds_cloud_out);

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

            t1 = std::chrono::steady_clock::now();
            gicp.align(*transformedP);
            t2 = std::chrono::steady_clock::now();
            time_span = std::chrono::duration_cast<std::chrono::duration<double, std::ratio<1, 1000>>>(t2 - t1);
            // std::cout << "PCL gicp.align Time: " << time_span.count() << " ms."<< std::endl;
            std::cout << "has converged: " << gicp.hasConverged() << " score: " <<
                gicp.getFitnessScore() << std::endl;

            auto transformation_matrix =  gicp.getFinalTransformation ();
            // std::cout << "transformation_matrix:\n"<<transformation_matrix << std::endl;
            // std::cout << std::endl;

            auto cloudSrc = dstf_cloud_in;
            auto cloudDst = ds_cloud_out;

            pcl::PointCloud<PointType> input_transformed;
            pcl::transformPointCloud (*cloudSrc, input_transformed, transformation_matrix);

            Eigen::Matrix3f rot_matrix = transformation_matrix.block<3,3>(0,0);
            Eigen::Vector3f euler = rot_matrix.eulerAngles(2, 1, 0);
            // std::cout << "Rotation    : "<<  euler.x() << " " << euler.y() << " " << euler.z()<< std::endl;
            // std::cout << "Rotation_matrix:\n"<<rot_matrix << std::endl;

            tf_marix = transformation_matrix;

            if(save_pcd)
            {
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSrcRGB (new pcl::PointCloud<pcl::PointXYZRGB>(cloudSrc->size(),1));
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudDstRGB (new pcl::PointCloud<pcl::PointXYZRGB>(cloudDst->size(),1));
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudALL (new pcl::PointCloud<pcl::PointXYZRGB>(cloudSrc->size() + cloudDst->size(),1));

                // Fill in the CloudIn data
                for (int i = 0; i < cloudSrc->size(); i++)
                {
                    pcl::PointXYZRGB &pointin = (*cloudSrcRGB)[i];
                    pointin.x = (input_transformed)[i].x;
                    pointin.y = (input_transformed)[i].y;
                    pointin.z = (input_transformed)[i].z;
                    pointin.r = 255;
                    pointin.g = 0;
                    pointin.b = 0;
                    (*cloudALL)[i] = pointin;
                }
                for (int i = 0; i < cloudDst->size(); i++)
                {
                    pcl::PointXYZRGB &pointout = (*cloudDstRGB)[i];
                    pointout.x = (*cloudDst)[i].x;
                    pointout.y = (*cloudDst)[i].y;
                    pointout.z = (*cloudDst)[i].z;
                    pointout.r = 0;
                    pointout.g = 255;
                    pointout.b = 255;
                    (*cloudALL)[i+cloudSrc->size()] = pointout;
                }
                pcl::io::savePCDFile<pcl::PointXYZRGB> ("/home/qing/icp_ICP.pcd", *cloudALL);
            }
        }
};

int
main(int argc, char **argv)
{
    ros::init(argc, argv, "Lidar Calibrate");

    LidarsCalibrater swo;
    ROS_INFO("\033[1;32m---->\033[0m Lidar Calibrate Started.");

    ros::Rate r(10);
    while(ros::ok()){

        ros::spinOnce();
        r.sleep();

    }

    return 0;
}


/// \file
/// Maintainer: Felice Serena
///

#include "disparity_registration_cpu_optimized.h"
#include <boost/log/trivial.hpp>
#include <omp.h>

namespace MouseTrack {

PointCloud DisparityRegistrationCpuOptimized::
operator()(const FrameWindow &window) const {
  const auto &frames = window.frames();

  // absolute transformation matrix relative to first camera
  auto Ts = absoluteTransformations(window);

  std::vector<Inverse> inverses(frames.size());
  for (size_t i = 0; i < frames.size(); i += 1) {
    Eigen::Matrix4d mat = frames[i].rotationCorrection * Ts[i];
    inverses[i] = prepareInverseTransformation(mat);
  }

  // store stuff in local variables
  const int border = frameBorder();
  const double xshift = correctingXShift();
  const double yshift = correctingYShift();
  const double minDisp = minDisparity() / 255.0;

  // flexibility to easily change row/column major
  typedef Eigen::Matrix<double, 4, Eigen::Dynamic, Eigen::RowMajor> Mat;
  // name indices
  const int X = 0;
  const int Y = 1;
  const int D = 2;
  const int H = 3;

  // collect data in "frame" vectors
  std::vector<Mat> framePoints(frames.size());
  // point index to (x,y) coordinate
  std::vector<std::vector<std::pair<int, int>>> coordinates(frames.size());

  // go through each frame, converting the disparity values to 3d points
  // relative to first camera
#pragma omp parallel for
  for (size_t i = 0; i < frames.size(); i += 1) {
    const auto &f = frames[i];
    const auto &disp = f.normalizedDisparityMap;
    int next_insert = 0;
    int expected = disp.size();
    // [x,y,disparity, 1]
    Mat pixels(4, expected);
    std::vector<std::pair<int, int>> coordinate(expected);
    // convert each pixel
    for (int y = border - 1; y < disp.rows() - border; y += 1) {
      for (int x = border - 1; x < disp.cols() - border; x += 1) {
        double disparity = disp(y, x);
        if (disparity < minDisp) {
          // just skip those points
          continue;
        }
        pixels(X, next_insert) = x;
        pixels(Y, next_insert) = y;
        pixels(D, next_insert) = disparity;
        coordinate[next_insert].first = x;
        coordinate[next_insert].second = y;
        next_insert += 1;
      }
    }

    // shrink down/create view with correct size
    auto hom = pixels.block(0, 0, 4, next_insert);
    coordinate.resize(next_insert);

    // disparity is returned between [0,1], but originally stored as [0,255]
    // 255*(f.baseline / disparity)
    hom.row(D) = (f.baseline / 255.0) / hom.row(D).array();

    // p.x() = (x + xshift - f.ccx) * (f.baseline * invDisparity);
    hom.row(X) = (hom.row(X).array() + (xshift - f.ccx)) * hom.row(D).array();

    // p.y() = (y + yshift - f.ccy) * (f.baseline * invDisparity);
    hom.row(Y) = (hom.row(Y).array() + (yshift - f.ccy)) * hom.row(D).array();

    // p.z() = f.focallength * (f.baseline * invDisparity);
    hom.row(D) = f.focallength * hom.row(D);

    // homogeneous 1
    hom.row(H).setConstant(1.0);

    // hom now holds homogeneous coordinates, find 3D points
    framePoints[i] = applyInverseTransformation(inverses[i], hom);
    coordinates[i] = std::move(coordinate);
  }

  // merge into single point cloud
  int points_count = 0;
  for (auto &i : coordinates) {
    points_count += i.size();
  }
  PointCloud cloud;
  cloud.resize(points_count, frames[0].labels.size());
  // copy to cloud
  int next_insert = 0;
  for (size_t f = 0; f < frames.size(); ++f) {
    const Frame &frame = frames[f];
#pragma omp parallel for
    for (size_t i = 0; i < coordinates[f].size(); ++i) {
      auto p = cloud[next_insert + i];
      p.x(framePoints[f](X, i));
      p.y(framePoints[f](Y, i));
      p.z(framePoints[f](D, i));
      const int x = coordinates[f][i].first;
      const int y = coordinates[f][i].second;
      p.intensity(frame.referencePicture(y, x));
      PointCloud::LabelVec labels(frame.labels.size());
      for (int l = 0; l < labels.size(); ++l) {
        labels[l] = frame.labels[l](y, x);
      }
      p.labels(std::move(labels));
    }
    next_insert += coordinates[f].size();
  }

  auto min = cloud.posMin();
  auto max = cloud.posMax();

  BOOST_LOG_TRIVIAL(debug) << "Found point cloud with " << cloud.size()
                           << " points, xyz-min: [" << min[0] << ", " << min[1]
                           << ", " << min[2] << "], xyz-max: [" << max[0]
                           << ", " << max[1] << ", " << max[2] << "]"
                           << std::flush;
  return cloud;
}

} // namespace MouseTrack


#include "unproject_on_plane.h"
#include "projection_constraint.h"
#include <Eigen/Dense>

template <
  typename DerivedUV,
  typename DerivedM,
  typename DerivedVP,
  typename DerivedP,
  typename DerivedZ>
void igl::unproject_on_plane(
  const Eigen::MatrixBase<DerivedUV> & UV,
  const Eigen::MatrixBase<DerivedM> & M,
  const Eigen::MatrixBase<DerivedVP> & VP,
  const Eigen::MatrixBase<DerivedP> & P,
  Eigen::PlainObjectBase<DerivedZ> & Z)
{
  using namespace Eigen;
  typedef typename DerivedZ::Scalar Scalar;
  Matrix<Scalar,2,3> A;
  Matrix<Scalar,2,1> B;
  projection_constraint(UV,M,VP,A,B);
  Matrix<Scalar,3,3> AA;
  AA.topRows(2) = A.template cast<Scalar>();
  AA.row(2) = P.head(3).template cast<Scalar>();
  Matrix<Scalar,3,1> BB;
  BB.head(2) = B.template cast<Scalar>();
  BB(2) = -P(3);
  Z = AA.fullPivHouseholderQr().solve(BB);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::unproject_on_plane<Eigen::Matrix<double, 2, 1, 0, 2, 1>, Eigen::Matrix<float, 4, 4, 0, 4, 4>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Matrix<double, 1, 4, 1, 1, 4>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 4, 0, 4, 4> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 4, 1, 1, 4> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&);
#endif


//
//  cal_fe_global_vary_ke.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/7/18.
//
//

#ifndef cal_fe_global_vary_ke_hpp
#define cal_fe_global_vary_ke_hpp

#include <stdio.h>
#include <iostream>
#include <Eigen/Eigen>
#include "maplocal.hpp"
#include "mapglobal.hpp"

using namespace Eigen;
extern double time_fem;
void cal_fe_global_vary_ke(int n_nodes, int n_el, MatrixXi &index_store, double q, VectorXd &u_n, VectorXd &v_n, int Ndofn,
                           std::vector<MatrixXd> &ke, VectorXd &fe_global);

#endif /* cal_fe_global_vary_ke_hpp */


/**
 * Copyright (c) 2020, Ouster, Inc.
 * All rights reserved.
 */

#include "camera.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <iostream>

#include "ouster/point_viz.h"

namespace ouster {
namespace viz {

using Eigen::Vector3d;
using Translation3d = Eigen::Translation<double, 3>;

namespace {

/* initial distance 50 m */
constexpr double log_distance_0 = 50.0;

/*
 * Note that most rotations are done in terms of integral decidegrees
 * (tenths of a degree). This is so that the camera will be able to
 * deterministically return to its original orientation if needed, without
 * accumulating floating point error, but at the same time having fine
 * enough subdivisions to have smooth user experience.
 *
 * For your convenience the user-defined literal _deg is defined so that
 * you can convert from degree literals to decidegree easily.
 */
using decidegree = int;

/*
 * Convert integral degrees to decidegrees
 */
constexpr decidegree operator""_deg(unsigned long long int angle) {
    return angle * 10;
}

/*
 * Convert and round floating point degrees to decidegrees
 */
constexpr decidegree operator""_deg(long double angle) {
    return static_cast<decidegree>(angle * 10);
}

/*
 * Convert decidegrees to radians
 */
template <class T>
double decidegree2radian(T angle) {
    return static_cast<double>(M_PI * angle / 180.0_deg);
}

int dd(float degrees) { return std::lround(degrees * 10); }

}  // namespace

/*
 * The camera class computes the folowing matrices:
 *
 * - Projection matrix proj, determined by view angle
 * - View matrix, relative to the target matrix, controlled by the user
 * - Target matrix, usually close to car pose (identity if not using SLAM viz)
 *
 * The final camera matrix is proj * view * target.
 *
 * The camera class performs computations in double because for the SLAM viz,
 * the camera's position may have moved very far from the origin, so extra
 * precision is needed to prevent floating point error.
 */
Camera::Camera()
    : target_{1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1},
      view_offset_{0, 0, 0},
      yaw_{0},
      pitch_{-45_deg},
      log_distance_{0},
      orthographic_{false},
      fov_{90_deg},
      proj_offset_x_{0},
      proj_offset_y_{0} {}

double Camera::view_distance() const {
    return log_distance_0 * std::exp(log_distance_ * 0.01);
}

void Camera::reset() {
    view_offset_ = {0, 0, 0};
    yaw_ = 0;
    pitch_ = -45_deg;
    log_distance_ = 0;
    fov_ = 90_deg;
}

// left positive, right negative
void Camera::yaw(float degrees) {
    yaw_ = (yaw_ + 360_deg + dd(degrees)) % 360_deg;
}

// down positive, up negative
void Camera::pitch(float degrees) {
    pitch_ = std::max(-180_deg, std::min(0, pitch_ + dd(degrees)));
}

// in is positive, out is negative
void Camera::dolly(int amount) {
    log_distance_ = std::max(-500, std::min(500, log_distance_ - amount));
}

void Camera::dolly_xy(double x, double y) {
    // OpenGL y goes from top to bottom
    Vector3d v{x, -y, 0};
    // convert from window coordinates to actual size
    double scale = std::tan(M_PI / 3600.0 * fov_);
    v *= view_distance() * scale;
    // apply 3d offset in the direction of the camera view
    auto rot =
        (Eigen::AngleAxisd{decidegree2radian(pitch_), Vector3d::UnitX()} *
         Eigen::AngleAxisd{decidegree2radian(yaw_), Vector3d::UnitZ()})
            .matrix();
    Eigen::Map<Eigen::Vector3d>{view_offset_.data()} += rot.transpose() * v;
}

void Camera::set_fov(float degrees) {
    fov_ = std::max(0.0_deg, std::min(360.0_deg, dd(degrees)));
}

void Camera::set_orthographic(bool b) { orthographic_ = b; }

void Camera::set_proj_offset(float x, float y) {
    proj_offset_x_ = x, proj_offset_y_ = y;
}

/*
 * Calculate camera matrices.
 *
 * View is relative to target 'look at' point. Current parametrization is:
 * - view_offset: translation to support dollying left/right
 * - yaw/pitch: camera rotation
 * - view_distance: z offset for dollying in/out
 *
 * view_offset is separate from target so we can dolly the camera while still
 * drawing some things (like distance rings) relative to target. It's applied
 * before yaw/pitch so that the camera orbits around view_offset * target.
 *
 * Unlike the usual opengl projection matrix function, this uses a diagonal fov.
 * For the orthographic projection, the fov and camera distance are used to
 * scale the viewing volume to make zoom and dolly work more or less
 * intuitively.
 *
 * reference:
 * https://www.scratchapixel.com/lessons/3d-basic-rendering/perspective-and-orthographic-projection-matrix
 */
impl::CameraData Camera::matrices(double aspect) const {
    // calculate view matrix
    Eigen::Matrix4d view =
        (Translation3d{Vector3d::UnitZ() * -view_distance()} *
         Eigen::AngleAxisd{decidegree2radian(pitch_), Vector3d::UnitX()} *
         Eigen::AngleAxisd{decidegree2radian(yaw_), Vector3d::UnitZ()} *
         Translation3d{Eigen::Map<const Eigen::Vector3d>{view_offset_.data()}})
            .matrix();

    // calculate projection matrix
    const double scale = std::tan(M_PI / 3600.0 * fov_);
    const double view_dist = view_distance();
    const double far_dist = std::min(1000.0, 100 * view_dist);
    const double near_dist = 0.1;

    // for diagonal fov, use ratio of each dimension to diagonal
    double a = std::atan(aspect);
    double aspect_w = std::sin(a);
    double aspect_h = std::cos(a);

    Eigen::Matrix4d proj = Eigen::Matrix4d::Zero();
    if (orthographic_) {
        proj(0, 0) = 1 / (aspect_w * scale * view_dist);
        proj(0, 3) = -proj_offset_x_;
        proj(1, 1) = 1 / (aspect_h * scale * view_dist);
        proj(1, 3) = -proj_offset_y_;
        proj(2, 2) = -2 / (far_dist - near_dist);
        proj(2, 3) = -(far_dist + near_dist) / (far_dist - near_dist);
        proj(3, 3) = 1;
    } else {
        proj(0, 0) = 1 / (aspect_w * scale);
        proj(0, 2) = proj_offset_x_;
        proj(1, 1) = 1 / (aspect_h * scale);
        proj(1, 2) = proj_offset_y_;
        proj(2, 2) = -(far_dist + near_dist) / (far_dist - near_dist);
        proj(2, 3) = -2 * far_dist * near_dist / (far_dist - near_dist);
        proj(3, 2) = -1;
    }

    return {proj, view, Eigen::Map<const Eigen::Matrix4d>{target_.data()}};
}

}  // namespace viz
}  // namespace ouster


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NETKET_VARIATIONALMONTECARLO_HPP
#define NETKET_VARIATIONALMONTECARLO_HPP

#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <complex>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>
#include "Machine/machine.hpp"
#include "Operator/abstract_operator.hpp"
#include "Optimizer/optimizer.hpp"
#include "Output/json_output_writer.hpp"
#include "Sampler/abstract_sampler.hpp"
#include "Stats/stats.hpp"
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "matrix_replacement.hpp"

namespace netket {

// Variational Monte Carlo schemes to learn the ground state
// Available methods:
// 1) Stochastic reconfiguration optimizer
//   both direct and sparse version
// 2) Gradient Descent optimizer
class VariationalMonteCarlo {
  using GsType = std::complex<double>;

  using VectorT = Eigen::Matrix<typename AbstractMachine<GsType>::StateType,
                                Eigen::Dynamic, 1>;
  using MatrixT = Eigen::Matrix<typename AbstractMachine<GsType>::StateType,
                                Eigen::Dynamic, Eigen::Dynamic>;

  const AbstractOperator &ham_;
  AbstractSampler<AbstractMachine<GsType>> &sampler_;
  AbstractMachine<GsType> &psi_;

  std::vector<std::vector<int>> connectors_;
  std::vector<std::vector<double>> newconfs_;
  std::vector<std::complex<double>> mel_;

  Eigen::VectorXcd elocs_;
  MatrixT Ok_;
  VectorT Okmean_;

  Eigen::MatrixXd vsamp_;

  Eigen::VectorXcd grad_;
  Eigen::VectorXcd gradprev_;

  double sr_diag_shift_;
  bool sr_rescale_shift_;
  bool use_iterative_;

  int totalnodes_;
  int mynode_;

  // This optional will contain a value iff the MPI rank is 0.
  nonstd::optional<JsonOutputWriter> output_;

  AbstractOptimizer &opt_;

  std::vector<AbstractOperator *> obs_;
  std::vector<std::string> obsnames_;
  ObsManager obsmanager_;

  bool dosr_;

  bool use_cholesky_;

  int nsamples_;
  int nsamples_node_;
  int ninitsamples_;
  int ndiscardedsamples_;
  int niter_opt_;

  std::complex<double> elocmean_;
  double elocvar_;
  int npar_;

 public:
  VariationalMonteCarlo(const AbstractOperator &ham,
                        AbstractSampler<AbstractMachine<GsType>> &sampler,
                        AbstractOptimizer &opt, int nsamples, int niter_opt,
                        std::string output_file, int discarded_samples = -1,
                        int discarded_samples_on_init = 0,
                        std::string method = "Sr", double diagshift = 0.01,
                        bool rescale_shift = false, bool use_iterative = false,
                        bool use_cholesky = true, int save_every = 50)
      : ham_(ham),
        sampler_(sampler),
        psi_(sampler.GetMachine()),
        opt_(opt),
        elocvar_(0.) {
    Init(nsamples, niter_opt, discarded_samples, discarded_samples_on_init,
         method, diagshift, rescale_shift, use_iterative, use_cholesky);

    InitOutput(output_file, save_every);
  }

  void Init(int nsamples, int niter_opt, int discarded_samples,
            int discarded_samples_on_init, const std::string &method,
            double diagshift, bool rescale_shift, bool use_iterative,
            bool use_cholesky) {
    npar_ = psi_.Npar();

    opt_.Init(psi_.GetParameters());

    grad_.resize(npar_);
    Okmean_.resize(npar_);

    setSrParameters();

    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    nsamples_ = nsamples;

    nsamples_node_ = int(std::ceil(double(nsamples_) / double(totalnodes_)));

    ninitsamples_ = discarded_samples_on_init;

    if (discarded_samples == -1) {
      ndiscardedsamples_ = 0.1 * nsamples_node_;
    } else {
      ndiscardedsamples_ = discarded_samples;
    }

    niter_opt_ = niter_opt;

    if (method == "Gd") {
      dosr_ = false;
    } else {
      setSrParameters(diagshift, rescale_shift, use_iterative, use_cholesky);
    }

    if (dosr_) {
      InfoMessage() << "Using the Stochastic reconfiguration method"
                    << std::endl;

      if (use_iterative_) {
        InfoMessage() << "With iterative solver" << std::endl;
      } else {
        if (use_cholesky_) {
          InfoMessage() << "Using Cholesky decomposition" << std::endl;
        }
      }
    } else {
      InfoMessage() << "Using a gradient-descent based method" << std::endl;
    }

    InfoMessage() << "Variational Monte Carlo running on " << totalnodes_
                  << " processes" << std::endl;

    MPI_Barrier(MPI_COMM_WORLD);
  }

  void InitOutput(std::string filebase, int freqbackup) {
    if (mynode_ == 0) {
      output_ =
          JsonOutputWriter(filebase + ".log", filebase + ".wf", freqbackup);
    }
  }

  void AddObservable(AbstractOperator &ob, const std::string &obname) {
    obs_.push_back(&ob);
    obsnames_.push_back(obname);
  }

  void InitSweeps() {
    sampler_.Reset();

    for (int i = 0; i < ninitsamples_; i++) {
      sampler_.Sweep();
    }
  }

  void Sample() {
    sampler_.Reset();

    for (int i = 0; i < ndiscardedsamples_; i++) {
      sampler_.Sweep();
    }

    vsamp_.resize(nsamples_node_, psi_.Nvisible());

    for (int i = 0; i < nsamples_node_; i++) {
      sampler_.Sweep();
      vsamp_.row(i) = sampler_.Visible();
    }
  }

  void Gradient() {
    obsmanager_.Reset("Energy");
    obsmanager_.Reset("EnergyVariance");

    for (const auto &obname : obsnames_) {
      obsmanager_.Reset(obname);
    }

    const int nsamp = vsamp_.rows();
    elocs_.resize(nsamp);
    Ok_.resize(nsamp, psi_.Npar());

    for (int i = 0; i < nsamp; i++) {
      elocs_(i) = Eloc(vsamp_.row(i));
      Ok_.row(i) = psi_.DerLog(vsamp_.row(i));
      obsmanager_.Push("Energy", elocs_(i).real());

      for (std::size_t on = 0; on < obs_.size(); on++) {
        obsmanager_.Push(obsnames_[on], ObSamp(*obs_[on], vsamp_.row(i)));
      }
    }

    elocmean_ = elocs_.mean();
    SumOnNodes(elocmean_);
    elocmean_ /= double(totalnodes_);

    Okmean_ = Ok_.colwise().mean();
    SumOnNodes(Okmean_);
    Okmean_ /= double(totalnodes_);

    Ok_ = Ok_.rowwise() - Okmean_.transpose();

    elocs_ -= elocmean_ * Eigen::VectorXd::Ones(nsamp);

    for (int i = 0; i < nsamp; i++) {
      obsmanager_.Push("EnergyVariance", std::norm(elocs_(i)));
    }

    grad_ = 2. * (Ok_.adjoint() * elocs_);

    // Summing the gradient over the nodes
    SumOnNodes(grad_);
    grad_ /= double(totalnodes_ * nsamp);
  }

  std::complex<double> Eloc(const Eigen::VectorXd &v) {
    ham_.FindConn(v, mel_, connectors_, newconfs_);

    assert(connectors_.size() == mel_.size());

    auto logvaldiffs = (psi_.LogValDiff(v, connectors_, newconfs_));

    assert(mel_.size() == std::size_t(logvaldiffs.size()));

    std::complex<double> eloc = 0;

    for (int i = 0; i < logvaldiffs.size(); i++) {
      eloc += mel_[i] * std::exp(logvaldiffs(i));
    }

    return eloc;
  }

  double ObSamp(AbstractOperator &ob, const Eigen::VectorXd &v) {
    ob.FindConn(v, mel_, connectors_, newconfs_);

    assert(connectors_.size() == mel_.size());

    auto logvaldiffs = (psi_.LogValDiff(v, connectors_, newconfs_));

    assert(mel_.size() == std::size_t(logvaldiffs.size()));

    std::complex<double> obval = 0;

    for (int i = 0; i < logvaldiffs.size(); i++) {
      obval += mel_[i] * std::exp(logvaldiffs(i));
    }

    return obval.real();
  }

  double ElocMean() { return elocmean_.real(); }

  double Elocvar() { return elocvar_; }

  void Run() {
    opt_.Reset();

    InitSweeps();

    for (int i = 0; i < niter_opt_; i++) {
      Sample();

      Gradient();

      UpdateParameters();

      PrintOutput(i);
    }
  }

  void UpdateParameters() {
    auto pars = psi_.GetParameters();

    if (dosr_) {
      const int nsamp = vsamp_.rows();

      Eigen::VectorXcd b = Ok_.adjoint() * elocs_;
      SumOnNodes(b);
      b /= double(nsamp * totalnodes_);

      if (!use_iterative_) {
        // Explicit construction of the S matrix
        Eigen::MatrixXcd S = Ok_.adjoint() * Ok_;
        SumOnNodes(S);
        S /= double(nsamp * totalnodes_);

        // Adding diagonal shift
        S += Eigen::MatrixXd::Identity(pars.size(), pars.size()) *
             sr_diag_shift_;

        Eigen::VectorXcd deltaP;
        if (use_cholesky_ == false) {
          Eigen::FullPivHouseholderQR<Eigen::MatrixXcd> qr(S.rows(), S.cols());
          qr.setThreshold(1.0e-6);
          qr.compute(S);
          deltaP = qr.solve(b);
        } else {
          Eigen::LLT<Eigen::MatrixXcd> llt(S.rows());
          llt.compute(S);
          deltaP = llt.solve(b);
        }
        // Eigen::VectorXcd deltaP=S.jacobiSvd(ComputeThinU |
        // ComputeThinV).solve(b);

        assert(deltaP.size() == grad_.size());
        grad_ = deltaP;

        if (sr_rescale_shift_) {
          std::complex<double> nor = (deltaP.dot(S * deltaP));
          grad_ /= std::sqrt(nor.real());
        }

      } else {
        Eigen::ConjugateGradient<MatrixReplacement, Eigen::Lower | Eigen::Upper,
                                 Eigen::IdentityPreconditioner>
            it_solver;
        // Eigen::GMRES<MatrixReplacement, Eigen::IdentityPreconditioner>
        // it_solver;
        it_solver.setTolerance(1.0e-3);
        MatrixReplacement S;
        S.attachMatrix(Ok_);
        S.setShift(sr_diag_shift_);
        S.setScale(1. / double(nsamp * totalnodes_));

        it_solver.compute(S);
        auto deltaP = it_solver.solve(b);

        grad_ = deltaP;
        if (sr_rescale_shift_) {
          auto nor = deltaP.dot(S * deltaP);
          grad_ /= std::sqrt(nor.real());
        }

        // if(mynode_==0){
        //   std::cerr<<it_solver.iterations()<<"
        //   "<<it_solver.error()<<std::endl;
        // }
        MPI_Barrier(MPI_COMM_WORLD);
      }
    }

    opt_.Update(grad_, pars);

    SendToAll(pars);

    psi_.SetParameters(pars);
    MPI_Barrier(MPI_COMM_WORLD);
  }

  void PrintOutput(int i) {
    // Note: This has to be called in all MPI processes, because converting
    // the ObsManager to JSON performs a MPI reduction.
    auto obs_data = json(obsmanager_);
    obs_data["Acceptance"] = sampler_.Acceptance();

    if (output_.has_value()) {  // output_.has_value() iff the MPI rank is 0, so
                                // the output is only written once
      output_->WriteLog(i, obs_data);
      output_->WriteState(i, psi_);
    }
    MPI_Barrier(MPI_COMM_WORLD);
  }

  void setSrParameters(double diagshift = 0.01, bool rescale_shift = false,
                       bool use_iterative = false, bool use_cholesky = true) {
    sr_diag_shift_ = diagshift;
    sr_rescale_shift_ = rescale_shift;
    use_iterative_ = use_iterative;
    dosr_ = true;
    use_cholesky_ = use_cholesky;
  }
};

}  // namespace netket

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_MANTISSA_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_MANTISSA_HPP_INCLUDED
#include <boost/simd/ieee/functions/mantissa.hpp>
#include <boost/dispatch/meta/adapted_traits.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/ldexpmask.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/scalar/is_invalid.hpp>
#include <boost/simd/include/functions/scalar/bitwise_and.hpp>
#include <boost/simd/include/functions/scalar/bitwise_or.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::mantissa_, tag::cpu_
                            , (A0)
                            , (scalar_< fundamental_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      if(!a0) return a0;
      if(is_invalid(a0)) return a0;
      typedef typename dispatch::meta::as_integer<A0, unsigned>::type int_type;
      const int_type n1 = (((Maxexponent<A0>()<<1)+1)<< Nbmantissabits<A0>());
      const int_type n2 = (sizeof(int_type)-2);
      const int_type mask0 = ((n1<<2)>>2);
      const int_type mask1 = ((~n1)|n2);
      return b_or(b_and(a0, mask1),mask0);
    }
  };
} } }

#endif


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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/
#ifndef __itkAnalyticalDiffusionQballReconstructionImageFilter_cpp
#define __itkAnalyticalDiffusionQballReconstructionImageFilter_cpp

#include <itkAnalyticalDiffusionQballReconstructionImageFilter.h>
#include <itkImageRegionConstIterator.h>
#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkImageRegionIterator.h>
#include <itkArray.h>
#include <vnl/vnl_vector.h>

#include <boost/version.hpp>
#include <stdio.h>
#include <locale>

#define _USE_MATH_DEFINES
#include <math.h>

#include <boost/math/special_functions.hpp>

#include "itkPointShell.h"

using namespace boost::math;

namespace itk {

#define QBALL_ANAL_RECON_PI       M_PI

template< class T, class TG, class TO, int L, int NODF>
AnalyticalDiffusionQballReconstructionImageFilter<T,TG,TO,L,NODF>
::AnalyticalDiffusionQballReconstructionImageFilter() :
    m_GradientDirectionContainer(NULL),
    m_NumberOfGradientDirections(0),
    m_NumberOfBaselineImages(1),
    m_Threshold(NumericTraits< ReferencePixelType >::NonpositiveMin()),
    m_BValue(1.0),
    m_Lambda(0.0),
    m_DirectionsDuplicated(false),
    m_Delta1(0.001),
    m_Delta2(0.001),
    m_UseMrtrixBasis(false)
{
    // At least 1 inputs is necessary for a vector image.
    // For images added one at a time we need at least six
    this->SetNumberOfRequiredInputs( 1 );
}

template<
        class TReferenceImagePixelType,
        class TGradientImagePixelType,
        class TOdfPixelType,
        int NOrderL,
        int NrOdfDirections>
typename itk::AnalyticalDiffusionQballReconstructionImageFilter<
TReferenceImagePixelType,TGradientImagePixelType,TOdfPixelType,
NOrderL,NrOdfDirections>::OdfPixelType
itk::AnalyticalDiffusionQballReconstructionImageFilter
<TReferenceImagePixelType, TGradientImagePixelType, TOdfPixelType,
NOrderL, NrOdfDirections>
::Normalize( OdfPixelType odf,
             typename NumericTraits<ReferencePixelType>::AccumulateType b0 )
{
    switch( m_NormalizationMethod )
    {
    case QBAR_STANDARD:
    {
        TOdfPixelType sum = 0;

        for(int i=0; i<NrOdfDirections; i++)
        {
            sum += odf[i];
        }
        if(sum>0)
            odf /= sum;

        return odf;
        break;
    }
    case QBAR_B_ZERO_B_VALUE:
    {
        for(int i=0; i<NrOdfDirections; i++)
        {
            odf[i] = ((TOdfPixelType)log((TOdfPixelType)b0)-odf[i])/m_BValue;
        }
        return odf;
        break;
    }
    case QBAR_B_ZERO:
    {
        odf *= 1.0/b0;
        return odf;
        break;
    }
    case QBAR_NONE:
    {
        return odf;
        break;
    }
    case QBAR_ADC_ONLY:
    {
        for(int i=0; i<NrOdfDirections; i++)
        {
            odf[i] = ((TOdfPixelType)log((TOdfPixelType)b0)-odf[i])/m_BValue;
        }
        return odf;
        break;
    }
    case QBAR_RAW_SIGNAL:
    {
        return odf;
        break;
    }
    case QBAR_SOLID_ANGLE:
    {
        for(int i=0; i<NrOdfDirections; i++)
            odf[i] *= QBALL_ANAL_RECON_PI*4/NrOdfDirections;

        break;
    }
    case QBAR_NONNEG_SOLID_ANGLE:
    {
        break;
    }
    }

    return odf;
}

template<
        class TReferenceImagePixelType,
        class TGradientImagePixelType,
        class TOdfPixelType,
        int NOrderL,
        int NrOdfDirections>
vnl_vector<TOdfPixelType>
itk::AnalyticalDiffusionQballReconstructionImageFilter
<TReferenceImagePixelType, TGradientImagePixelType, TOdfPixelType,
NOrderL, NrOdfDirections>
::PreNormalize( vnl_vector<TOdfPixelType> vec,
                typename NumericTraits<ReferencePixelType>::AccumulateType b0 )
{
    switch( m_NormalizationMethod )
    {
    case QBAR_STANDARD:
    {
        return vec;
        break;
    }
    case QBAR_B_ZERO_B_VALUE:
    {
        int n = vec.size();
        for(int i=0; i<n; i++)
        {
            if (vec[i]<=0)
                vec[i] = 0.001;

            vec[i] = log(vec[i]);
        }
        return vec;
        break;
    }
    case QBAR_B_ZERO:
    {
        return vec;
        break;
    }
    case QBAR_NONE:
    {
        return vec;
        break;
    }
    case QBAR_ADC_ONLY:
    {
        int n = vec.size();
        for(int i=0; i<n; i++)
        {
            if (vec[i]<=0)
                vec[i] = 0.001;

            vec[i] = log(vec[i]);
        }
        return vec;
        break;
    }
    case QBAR_RAW_SIGNAL:
    {
        return vec;
        break;
    }
    case QBAR_SOLID_ANGLE:
    case QBAR_NONNEG_SOLID_ANGLE:
    {
        int n = vec.size();
        double b0f = (double)b0;
        for(int i=0; i<n; i++)
        {
            vec[i] = vec[i]/b0f;

            if (vec[i]<0)
                vec[i] = m_Delta1;
            else if (vec[i]<m_Delta1)
                vec[i] = m_Delta1/2 + vec[i]*vec[i]/(2*m_Delta1);
            else if (vec[i]>=1)
                vec[i] = 1-m_Delta2/2;
            else if (vec[i]>=1-m_Delta2)
                vec[i] = 1-m_Delta2/2-(1-vec[i])*(1-vec[i])/(2*m_Delta2);

            vec[i] = log(-log(vec[i]));
        }
        return vec;
        break;
    }
    }

    return vec;
}

template< class T, class TG, class TO, int L, int NODF>
void AnalyticalDiffusionQballReconstructionImageFilter<T,TG,TO,L,NODF>
::BeforeThreadedGenerateData()
{
    // If we have more than 2 inputs, then each input, except the first is a
    // gradient image. The number of gradient images must match the number of
    // gradient directions.
    //const unsigned int numberOfInputs = this->GetNumberOfInputs();

    // There need to be at least 6 gradient directions to be able to compute the
    // tensor basis
    if( m_NumberOfGradientDirections < (L*L + L + 2)/2 + L )
    {
        itkExceptionMacro( << "Not enough gradient directions supplied (" << m_NumberOfGradientDirections << "). At least " << (L*L + L + 2)/2 + L << " needed for SH-order " << L);
    }

    // Input must be an itk::VectorImage.
    std::string gradientImageClassName(
                this->ProcessObject::GetInput(0)->GetNameOfClass());
    if ( strcmp(gradientImageClassName.c_str(),"VectorImage") != 0 )
    {
        itkExceptionMacro( <<
                           "There is only one Gradient image. I expect that to be a VectorImage. "
                           << "But its of type: " << gradientImageClassName );
    }

    this->ComputeReconstructionMatrix();

    typename GradientImagesType::Pointer img = static_cast< GradientImagesType * >(
                this->ProcessObject::GetInput(0) );

    m_BZeroImage = BZeroImageType::New();
    m_BZeroImage->SetSpacing( img->GetSpacing() );   // Set the image spacing
    m_BZeroImage->SetOrigin( img->GetOrigin() );     // Set the image origin
    m_BZeroImage->SetDirection( img->GetDirection() );  // Set the image direction
    m_BZeroImage->SetLargestPossibleRegion( img->GetLargestPossibleRegion());
    m_BZeroImage->SetBufferedRegion( img->GetLargestPossibleRegion() );
    m_BZeroImage->Allocate();

    m_ODFSumImage = BZeroImageType::New();
    m_ODFSumImage->SetSpacing( img->GetSpacing() );   // Set the image spacing
    m_ODFSumImage->SetOrigin( img->GetOrigin() );     // Set the image origin
    m_ODFSumImage->SetDirection( img->GetDirection() );  // Set the image direction
    m_ODFSumImage->SetLargestPossibleRegion( img->GetLargestPossibleRegion());
    m_ODFSumImage->SetBufferedRegion( img->GetLargestPossibleRegion() );
    m_ODFSumImage->Allocate();

    m_CoefficientImage = CoefficientImageType::New();
    m_CoefficientImage->SetSpacing( img->GetSpacing() );   // Set the image spacing
    m_CoefficientImage->SetOrigin( img->GetOrigin() );     // Set the image origin
    m_CoefficientImage->SetDirection( img->GetDirection() );  // Set the image direction
    m_CoefficientImage->SetLargestPossibleRegion( img->GetLargestPossibleRegion());
    m_CoefficientImage->SetBufferedRegion( img->GetLargestPossibleRegion() );
    m_CoefficientImage->Allocate();

    if(m_NormalizationMethod == QBAR_SOLID_ANGLE || m_NormalizationMethod == QBAR_NONNEG_SOLID_ANGLE)
        m_Lambda = 0.0;
}

template< class T, class TG, class TO, int L, int NODF>
void AnalyticalDiffusionQballReconstructionImageFilter<T,TG,TO,L,NODF>
::ThreadedGenerateData(const OutputImageRegionType& outputRegionForThread,
                       ThreadIdType )
{
    typename OutputImageType::Pointer outputImage =
            static_cast< OutputImageType * >(this->ProcessObject::GetPrimaryOutput());

    ImageRegionIterator< OutputImageType > oit(outputImage, outputRegionForThread);
    oit.GoToBegin();

    ImageRegionIterator< BZeroImageType > oit2(m_BZeroImage, outputRegionForThread);
    oit2.GoToBegin();

    ImageRegionIterator< FloatImageType > oit3(m_ODFSumImage, outputRegionForThread);
    oit3.GoToBegin();

    ImageRegionIterator< CoefficientImageType > oit4(m_CoefficientImage, outputRegionForThread);
    oit4.GoToBegin();

    typedef ImageRegionConstIterator< GradientImagesType > GradientIteratorType;
    typedef typename GradientImagesType::PixelType         GradientVectorType;
    typename GradientImagesType::Pointer gradientImagePointer = NULL;

    // Would have liked a dynamic_cast here, but seems SGI doesn't like it
    // The enum will ensure that an inappropriate cast is not done
    gradientImagePointer = static_cast< GradientImagesType * >(
                this->ProcessObject::GetInput(0) );

    GradientIteratorType git(gradientImagePointer, outputRegionForThread );
    git.GoToBegin();

    // Compute the indicies of the baseline images and gradient images
    std::vector<unsigned int> baselineind; // contains the indicies of
    // the baseline images
    std::vector<unsigned int> gradientind; // contains the indicies of
    // the gradient images

    for(GradientDirectionContainerType::ConstIterator gdcit = this->m_GradientDirectionContainer->Begin();
        gdcit != this->m_GradientDirectionContainer->End(); ++gdcit)
    {
        if(gdcit.Value().one_norm() <= 0.0)
            baselineind.push_back(gdcit.Index());
        else
            gradientind.push_back(gdcit.Index());
    }

    if( m_DirectionsDuplicated )
    {
        int gradIndSize = gradientind.size();
        for(int i=0; i<gradIndSize; i++)
            gradientind.push_back(gradientind[i]);
    }

    while( !git.IsAtEnd() )
    {
        GradientVectorType b = git.Get();

        typename NumericTraits<ReferencePixelType>::AccumulateType b0 = NumericTraits<ReferencePixelType>::Zero;

        // Average the baseline image pixels
        for(unsigned int i = 0; i < baselineind.size(); ++i)
        {
            b0 += b[baselineind[i]];
        }
        b0 /= this->m_NumberOfBaselineImages;

        OdfPixelType odf(0.0);
        typename CoefficientImageType::PixelType coeffPixel(0.0);
        vnl_vector<TO> B(m_NumberOfGradientDirections);

        if( (b0 != 0) && (b0 >= m_Threshold) )
        {
            for( unsigned int i = 0; i< m_NumberOfGradientDirections; i++ )
            {
                B[i] = static_cast<TO>(b[gradientind[i]]);
            }

            B = PreNormalize(B, b0);
            if(m_NormalizationMethod == QBAR_SOLID_ANGLE)
            {
                vnl_vector<TO> coeffs(m_NumberCoefficients);
                coeffs = ( (*m_CoeffReconstructionMatrix) * B );
                coeffs[0] += 1.0/(2.0*sqrt(QBALL_ANAL_RECON_PI));
                odf = ( (*m_SphericalHarmonicBasisMatrix) * coeffs ).data_block();
                coeffPixel = coeffs.data_block();
            }
            else if(m_NormalizationMethod == QBAR_NONNEG_SOLID_ANGLE)
            {
                /** this would be the place to implement a non-negative
              * solver for quadratic programming problem:
              * min .5*|| Bc-s ||^2 subject to -CLPc <= 4*pi*ones
              * (refer to MICCAI 2009 Goh et al. "Estimating ODFs with PDF constraints")
              * .5*|| Bc-s ||^2 == .5*c'B'Bc - x'B's + .5*s's
              */

                itkExceptionMacro( << "Nonnegative Solid Angle not yet implemented");
            }
            else
            {
                vnl_vector<TO> coeffs(m_NumberCoefficients);
                coeffs = ( (*m_CoeffReconstructionMatrix) * B );
                coeffs[0] += 1.0/(2.0*sqrt(QBALL_ANAL_RECON_PI));
                coeffPixel = coeffs.data_block();
                odf = ( (*m_ReconstructionMatrix) * B ).data_block();
            }
            odf = Normalize(odf, b0);

        }

        oit.Set( odf );
        oit2.Set( b0 );
        float sum = 0;
        for (unsigned int k=0; k<odf.Size(); k++)
            sum += (float) odf[k];
        oit3.Set( sum-1 );
        oit4.Set(coeffPixel);
        ++oit;  // odf image iterator
        ++oit3; // odf sum image iterator
        ++oit2; // b0 image iterator
        ++oit4; // coefficient image iterator
        ++git;  // Gradient  image iterator
    }

    std::cout << "One Thread finished reconstruction" << std::endl;
}

template< class T, class TG, class TO, int L, int NODF>
void AnalyticalDiffusionQballReconstructionImageFilter<T,TG,TO,L,NODF>
::tofile2(vnl_matrix<double> *pA, std::string fname)
{
    vnl_matrix<double> A = (*pA);
    ofstream myfile;
    std::locale C("C");
    std::locale originalLocale = myfile.getloc();
    myfile.imbue(C);

    myfile.open (fname.c_str());
    myfile << "A1=[";
    for(unsigned int i=0; i<A.rows(); i++)
    {
        for(unsigned int j=0; j<A.columns(); j++)
        {
            myfile << A(i,j) << " ";
            if(j==A.columns()-1 && i!=A.rows()-1)
                myfile << ";";
        }
    }
    myfile << "];";
    myfile.close();

    myfile.imbue( originalLocale );
}

template< class T, class TG, class TO, int L, int NODF>
void AnalyticalDiffusionQballReconstructionImageFilter<T,TG,TO,L,NODF>
::Cart2Sph(double x, double y, double z, double *spherical)
{
    double phi, theta, r;
    r = sqrt(x*x+y*y+z*z);

    if( r<mitk::eps )
    {
        theta = M_PI/2;
        phi = M_PI/2;
    }
    else
    {
        theta = acos(z/r);
        phi = atan2(y, x);
    }
    spherical[0] = phi;
    spherical[1] = theta;
    spherical[2] = r;
}

template< class T, class TG, class TO, int L, int NODF>
double AnalyticalDiffusionQballReconstructionImageFilter<T,TG,TO,L,NODF>
::Yj(int m, int l, double theta, double phi, bool useMRtrixBasis)
{
    if (!useMRtrixBasis)
    {
        if (m<0)
            return sqrt(2.0)*spherical_harmonic_r(l, -m, theta, phi);
        else if (m==0)
            return spherical_harmonic_r(l, m, theta, phi);
        else
            return pow(-1.0,m)*sqrt(2.0)*spherical_harmonic_i(l, m, theta, phi);
    }
    else
    {
        double plm = legendre_p<double>(l,abs(m),-cos(theta));
        double mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;
        if (m>0)
            return mag*cos(m*phi);
        else if (m==0)
            return mag;
        else
            return mag*sin(-m*phi);
    }

    return 0;
}

template< class T, class TG, class TO, int L, int NODF>
double AnalyticalDiffusionQballReconstructionImageFilter<T,TG,TO,L,NODF>
::Legendre0(int l)
{
    if( l%2 != 0 )
    {
        return 0;
    }
    else
    {
        double prod1 = 1.0;
        for(int i=1;i<l;i+=2) prod1 *= i;
        double prod2 = 1.0;
        for(int i=2;i<=l;i+=2) prod2 *= i;
        return pow(-1.0,l/2.0)*(prod1/prod2);
    }
}

template< class T, class TG, class TO, int L, int NODF>
void AnalyticalDiffusionQballReconstructionImageFilter<T,TG,TO,L,NODF>
::ComputeReconstructionMatrix()
{

    //for(int i=-6;i<7;i++)
    //  std::cout << boost::math::legendre_p<double>(6, i, 0.65657) << std::endl;

    if( m_NumberOfGradientDirections < (L*L + L + 2)/2 + L )
    {
        itkExceptionMacro( << "Not enough gradient directions supplied (" << m_NumberOfGradientDirections << "). At least " << (L*L + L + 2)/2 + L << " needed for SH-order " << L);
    }

    {
        // check for duplicate diffusion gradients
        bool warning = false;
        for(GradientDirectionContainerType::ConstIterator gdcit1 = this->m_GradientDirectionContainer->Begin();
            gdcit1 != this->m_GradientDirectionContainer->End(); ++gdcit1)
        {
            for(GradientDirectionContainerType::ConstIterator gdcit2 = this->m_GradientDirectionContainer->Begin();
                gdcit2 != this->m_GradientDirectionContainer->End(); ++gdcit2)
            {
                if(gdcit1.Value() == gdcit2.Value() && gdcit1.Index() != gdcit2.Index())
                {
                    itkWarningMacro( << "Some of the Diffusion Gradients equal each other. Corresponding image data should be averaged before calling this filter." );
                    warning = true;
                    break;
                }
            }
            if (warning) break;
        }

        // handle acquisition schemes where only half of the spherical
        // shell is sampled by the gradient directions. In this case,
        // each gradient direction is duplicated in negative direction.
        vnl_vector<double> centerMass(3);
        centerMass.fill(0.0);
        int count = 0;
        for(GradientDirectionContainerType::ConstIterator gdcit1 = this->m_GradientDirectionContainer->Begin();
            gdcit1 != this->m_GradientDirectionContainer->End(); ++gdcit1)
        {
            if(gdcit1.Value().one_norm() > 0.0)
            {
                centerMass += gdcit1.Value();
                count ++;
            }
        }
        centerMass /= count;
        if(centerMass.two_norm() > 0.1)
        {
            m_DirectionsDuplicated = true;
            m_NumberOfGradientDirections *= 2;
        }
    }

    vnl_matrix<double> *Q = new vnl_matrix<double>(3, m_NumberOfGradientDirections);

    {
        int i = 0;
        for(GradientDirectionContainerType::ConstIterator gdcit = this->m_GradientDirectionContainer->Begin();
            gdcit != this->m_GradientDirectionContainer->End(); ++gdcit)
        {
            if(gdcit.Value().one_norm() > 0.0)
            {
                double x = gdcit.Value().get(0);
                double y = gdcit.Value().get(1);
                double z = gdcit.Value().get(2);
                double cart[3];
                Cart2Sph(x,y,z,cart);
                (*Q)(0,i) = cart[0];
                (*Q)(1,i) = cart[1];
                (*Q)(2,i++) = cart[2];
            }
        }
        if(m_DirectionsDuplicated)
        {
            for(GradientDirectionContainerType::ConstIterator gdcit = this->m_GradientDirectionContainer->Begin();
                gdcit != this->m_GradientDirectionContainer->End(); ++gdcit)
            {
                if(gdcit.Value().one_norm() > 0.0)
                {
                    double x = gdcit.Value().get(0);
                    double y = gdcit.Value().get(1);
                    double z = gdcit.Value().get(2);
                    double cart[3];
                    Cart2Sph(x,y,z,cart);
                    (*Q)(0,i) = cart[0];
                    (*Q)(1,i) = cart[1];
                    (*Q)(2,i++) = cart[2];
                }
            }
        }
    }

    int l = L;
    m_NumberCoefficients = (int)(l*l + l + 2.0)/2.0 + l;
    vnl_matrix<double>* B = new vnl_matrix<double>(m_NumberOfGradientDirections,m_NumberCoefficients);
    vnl_matrix<double>* _L = new vnl_matrix<double>(m_NumberCoefficients,m_NumberCoefficients);
    _L->fill(0.0);
    vnl_matrix<double>* LL = new vnl_matrix<double>(m_NumberCoefficients,m_NumberCoefficients);
    LL->fill(0.0);
    vnl_matrix<double>* P = new vnl_matrix<double>(m_NumberCoefficients,m_NumberCoefficients);
    P->fill(0.0);
    vnl_matrix<double>* Inv = new vnl_matrix<double>(m_NumberCoefficients,m_NumberCoefficients);
    P->fill(0.0);
    vnl_vector<int>* lj = new vnl_vector<int>(m_NumberCoefficients);
    m_LP = new vnl_vector<double>(m_NumberCoefficients);

    for(unsigned int i=0; i<m_NumberOfGradientDirections; i++)
    {
        for(int k=0; k<=l; k+=2)
        {
            for(int m=-k; m<=k; m++)
            {
                int j = (k*k + k + 2)/2 + m - 1;
                (*_L)(j,j) = -k*(k+1);
                (*m_LP)(j) = -k*(k+1);
                (*lj)[j] = k;
                double phi = (*Q)(0,i);
                double th = (*Q)(1,i);
                (*B)(i,j) = Yj(m,k,th,phi, m_UseMrtrixBasis);
            }
        }
    }

    //vnl_matrix<double> temp((*_L)*(*_L));
    LL->update(*_L);
    *LL *= *_L;
    //tofile2(LL,"LL");

    for(int i=0; i<m_NumberCoefficients; i++)
    {
        // here we leave out the 2*pi multiplication from Descoteaux
        // this is because we do not want the real integral value
        // but an average value over the equator.

        if(m_NormalizationMethod == QBAR_SOLID_ANGLE || m_NormalizationMethod == QBAR_NONNEG_SOLID_ANGLE)
        {
            (*P)(i,i) = 2.0*QBALL_ANAL_RECON_PI*Legendre0((*lj)[i]);
            (*m_LP)(i) *= (*P)(i,i);
        }
        else
        {
            (*P)(i,i) = Legendre0((*lj)[i]);
        }
    }
    m_B_t = new vnl_matrix<double>(B->transpose());
    //tofile2(&m_B_t,"m_B_t");
    vnl_matrix<double> B_t_B = (*m_B_t) * (*B);
    //tofile2(&B_t_B,"B_t_B");
    vnl_matrix<double> lambdaLL(m_NumberCoefficients,m_NumberCoefficients);
    lambdaLL.update((*LL));
    lambdaLL *= m_Lambda;
    //tofile2(&lambdaLL,"lLL");

    vnl_matrix<double> tmp( B_t_B + lambdaLL);
    vnl_matrix_inverse<double> *pseudoInverse
            = new vnl_matrix_inverse<double>( tmp );

    (*Inv) = pseudoInverse->pinverse();
    //tofile2(Inv,"Inv");
    vnl_matrix<double> temp((*Inv) * (*m_B_t));
    double fac1 = (1.0/(16.0*QBALL_ANAL_RECON_PI*QBALL_ANAL_RECON_PI));
    switch(m_NormalizationMethod)
    {
    case QBAR_ADC_ONLY:
    case QBAR_RAW_SIGNAL:
        break;
    case QBAR_STANDARD:
    case QBAR_B_ZERO_B_VALUE:
    case QBAR_B_ZERO:
    case QBAR_NONE:
        temp = (*P) * temp;
        break;
    case QBAR_SOLID_ANGLE:
        temp = fac1 * (*P) * (*_L) * temp;
        break;
    case QBAR_NONNEG_SOLID_ANGLE:
        break;
    }

    //tofile2(&temp,"A");

    m_CoeffReconstructionMatrix = new vnl_matrix<TO>(m_NumberCoefficients,m_NumberOfGradientDirections);
    for(int i=0; i<m_NumberCoefficients; i++)
    {
        for(unsigned int j=0; j<m_NumberOfGradientDirections; j++)
        {
            (*m_CoeffReconstructionMatrix)(i,j) = (float) temp(i,j);
        }
    }

    // this code goes to the image adapter coeffs->odfs later

    int NOdfDirections = NODF;
    vnl_matrix_fixed<double, 3, NODF>* U =
            itk::PointShell<NODF, vnl_matrix_fixed<double, 3, NODF> >::DistributePointShell();

    m_SphericalHarmonicBasisMatrix  = new vnl_matrix<TO>(NOdfDirections,m_NumberCoefficients);
    vnl_matrix<double>* sphericalHarmonicBasisMatrix2
            = new vnl_matrix<double>(NOdfDirections,m_NumberCoefficients);
    for(int i=0; i<NOdfDirections; i++)
    {
        double x = (*U)(0,i);
        double y = (*U)(1,i);
        double z = (*U)(2,i);
        double cart[3];
        Cart2Sph(x,y,z,cart);
        (*U)(0,i) = cart[0];
        (*U)(1,i) = cart[1];
        (*U)(2,i) = cart[2];
    }

    for(int i=0; i<NOdfDirections; i++)
    {
        for(int k=0; k<=l; k+=2)
        {
            for(int m=-k; m<=k; m++)
            {
                int j = (k*k + k + 2)/2 + m - 1;
                double phi = (*U)(0,i);
                double th = (*U)(1,i);
                (*m_SphericalHarmonicBasisMatrix)(i,j) = Yj(m,k,th,phi,m_UseMrtrixBasis);
                (*sphericalHarmonicBasisMatrix2)(i,j) = (*m_SphericalHarmonicBasisMatrix)(i,j);
            }
        }
    }

    m_ReconstructionMatrix = new vnl_matrix<TO>(NOdfDirections,m_NumberOfGradientDirections);
    *m_ReconstructionMatrix = (*m_SphericalHarmonicBasisMatrix) * (*m_CoeffReconstructionMatrix);

}

template< class T, class TG, class TO, int L, int NODF>
void AnalyticalDiffusionQballReconstructionImageFilter<T,TG,TO,L,NODF>
::SetGradientImage(const GradientDirectionContainerType *gradientDirection,
                   const GradientImagesType *gradientImage )
{
    // Copy Gradient Direction Container
    this->m_GradientDirectionContainer = GradientDirectionContainerType::New();
    for(GradientDirectionContainerType::ConstIterator it = gradientDirection->Begin();
        it != gradientDirection->End(); it++)
    {
        this->m_GradientDirectionContainer->push_back(it.Value());
    }

    unsigned int numImages = gradientDirection->Size();
    this->m_NumberOfBaselineImages = 0;
    for(GradientDirectionContainerType::Iterator it = this->m_GradientDirectionContainer->Begin();
        it != this->m_GradientDirectionContainer->End(); it++)
    {
        if(it.Value().one_norm() <= 0.0)
        {
            this->m_NumberOfBaselineImages++;
        }
        else // Normalize non-zero gradient directions
        {
            it.Value() = it.Value() / it.Value().two_norm();
        }
    }

    this->m_NumberOfGradientDirections = numImages - this->m_NumberOfBaselineImages;

    // ensure that the gradient image we received has as many components as
    // the number of gradient directions
    if( gradientImage->GetVectorLength() != this->m_NumberOfBaselineImages + m_NumberOfGradientDirections )
    {
        itkExceptionMacro( << m_NumberOfGradientDirections << " gradients + " << this->m_NumberOfBaselineImages
                           << "baselines = " << m_NumberOfGradientDirections + this->m_NumberOfBaselineImages
                           << " directions specified but image has " << gradientImage->GetVectorLength()
                           << " components.");
    }

    this->ProcessObject::SetNthInput( 0, const_cast< GradientImagesType* >(gradientImage) );
}

template< class T, class TG, class TO, int L, int NODF>
void AnalyticalDiffusionQballReconstructionImageFilter<T,TG,TO,L,NODF>
::PrintSelf(std::ostream& os, Indent indent) const
{
    std::locale C("C");
    std::locale originalLocale = os.getloc();
    os.imbue(C);

    Superclass::PrintSelf(os,indent);

    os << indent << "OdfReconstructionMatrix: " << m_ReconstructionMatrix << std::endl;
    if ( m_GradientDirectionContainer )
        os << indent << "GradientDirectionContainer: " << m_GradientDirectionContainer << std::endl;
    else
        os << indent << "GradientDirectionContainer: (Gradient directions not set)" << std::endl;

    os << indent << "NumberOfGradientDirections: " <<  m_NumberOfGradientDirections << std::endl;
    os << indent << "NumberOfBaselineImages: " << m_NumberOfBaselineImages << std::endl;
    os << indent << "Threshold for reference B0 image: " << m_Threshold << std::endl;
    os << indent << "BValue: " << m_BValue << std::endl;
    os.imbue( originalLocale );
}

}

#endif // __itkAnalyticalDiffusionQballReconstructionImageFilter_cpp