#include <boost/test/unit_test.hpp>
#include "small_skin_data.h"
#include "splice_fc60_data.h"
#include "sonar_fc60_data.h"
#include "diabetes_fc8_data.h"
#include "../src/svm_impl/classifiers_generator.h"
#include "../src/svm_impl/svm_model.h"
//#include <chrono>
//#include <iostream>

BOOST_AUTO_TEST_CASE(test_prediction_svm_binary_correct_skin_3_features)
{
    // running on train data of 10K samples of 3 features data set
    // data is taken from ./binary_models_files/skin_data
    const auto gamma = 2;
    const auto cost = 512;
    // this test uses the predfined parameters for the testing
    // we are just run train with c_svm and RBF, then compare
    // the results we have to the ones we collected from libsvm
    using namespace mlmodels;
    auto model = svm::classifier::create_model(svm::classifier::c_rbf_train{
                                            svm::classifier::c_rbf_train::base_1
                                            {
                                                svm::rbf_args{gamma},
                                                svm::base_train_params{}
                                            },
                                            svm::classifier::c_rbf_train::base_2{
                                                cost, 0, 0
                                            }
                                        },
                                        small_skin_train_features, small_skin_labels
                                    );
   BOOST_TEST((model.get() != nullptr), "we should get a valid model here");
   auto predictions = svm::predict(model, small_skin_test_features);
   BOOST_TEST_REQUIRE(not predictions.empty(), "we should be getting a valid list of prediction here");
   BOOST_TEST_REQUIRE(predictions.size() == small_skin_predictions.size());
    auto expted_begin = std::begin(small_skin_predictions);
    unsigned int at = 0;
    for (auto our_results : predictions) {
        BOOST_TEST(our_results == *expted_begin, "["<<at<<"] our results "<<our_results<<" do not match expected results "<<*expted_begin);
        ++expted_begin;
        ++at;
    }
}

BOOST_AUTO_TEST_CASE(test_prediction_svm_binary_correct_splice60)
{
    // running on small dataset with 14 features
    // data was taken from ./binary_models_files/australian
    const auto gamma = 0.03125;
    const auto cost = 2;
    // this test uses the predfined parameters for the testing
    // we are just run train with c_svm and RBF, then compare
    // the results we have to the ones we collected from libsvm
    using namespace mlmodels;
    auto model = svm::classifier::create_model(svm::classifier::c_rbf_train{
                                            svm::classifier::c_rbf_train::base_1
                                            {
                                                svm::rbf_args{gamma},
                                                svm::base_train_params{}
                                            },
                                            svm::classifier::c_rbf_train::base_2{
                                                cost, 0, 0
                                            }
                                        },
                                        splice_fc60_train_features, splice_fc60_labels
                                    );
   BOOST_TEST((model.get() != nullptr), "we should get a valid model here");
   auto predictions = svm::predict(model, splice_fc60_test_features);
   BOOST_TEST_REQUIRE(not predictions.empty(), "we should be getting a valid list of prediction here");
   BOOST_TEST_REQUIRE(predictions.size() == splice_fc60_predictions.size());
    auto expted_begin = std::begin(splice_fc60_predictions);
    unsigned int at = 0;
    for (auto our_results : predictions) {
        BOOST_TEST(our_results == *expted_begin, "["<<at<<"] our results "<<our_results<<" do not match expected results "<<*expted_begin);
        ++expted_begin;
        ++at;
    }
}

BOOST_AUTO_TEST_CASE(test_prediction_svm_binary_correct_sonar60)
{
    // running on small dataset with 60 features
    // data was taken from ./binary_models_files/sonar
    const auto gamma = 0.125;
    const auto cost = 2;
    // this test uses the predfined parameters for the testing
    // we are just run train with c_svm and RBF, then compare
    // the results we have to the ones we collected from libsvm
    using namespace mlmodels;
    auto model = svm::classifier::create_model(svm::classifier::c_rbf_train{
                                            svm::classifier::c_rbf_train::base_1
                                            {
                                                svm::rbf_args{gamma},
                                                svm::base_train_params{}
                                            },
                                            svm::classifier::c_rbf_train::base_2{
                                                cost, 0, 0
                                            }
                                        },
                                        sonar_fc60_train_features, sonar_fc60_labels
                                    );
   BOOST_TEST((model.get() != nullptr), "we should get a valid model here");
   auto predictions = svm::predict(model, sonar_fc60_test_features);
   BOOST_TEST_REQUIRE(not predictions.empty(), "we should be getting a valid list of prediction here");
   BOOST_TEST_REQUIRE(predictions.size() == sonar_fc60_predictions.size());
    auto expted_begin = std::begin(sonar_fc60_predictions);
    unsigned int at = 0;
    for (auto our_results : predictions) {
        BOOST_TEST(our_results == *expted_begin, "["<<at<<"] our results "<<our_results<<" do not match expected results "<<*expted_begin);
        ++expted_begin;
        ++at;
    }
}

BOOST_AUTO_TEST_CASE(test_prediction_svm_binary_correct_diabet8)
{
    // running on small dataset with 8 features
    // data was taken from ./binary_models_files/diabetes
    const auto gamma = 0.03125;
    const auto cost = 8;
    // this test uses the predfined parameters for the testing
    // we are just run train with c_svm and RBF, then compare
    // the results we have to the ones we collected from libsvm
    using namespace mlmodels;
    auto model = svm::classifier::create_model(svm::classifier::c_rbf_train{
                                            svm::classifier::c_rbf_train::base_1
                                            {
                                                svm::rbf_args{gamma},
                                                svm::base_train_params{}
                                            },
                                            svm::classifier::c_rbf_train::base_2{
                                                cost, 0, 0
                                            }
                                        },
                                        diabetes_fc8_train_features, diabetes_fc8_labels
                                    );
   BOOST_TEST((model.get() != nullptr), "we should get a valid model here");
   auto predictions = svm::predict(model, diabetes_fc8_test_features);
   BOOST_TEST_REQUIRE(not predictions.empty(), "we should be getting a valid list of prediction here");
   BOOST_TEST_REQUIRE(predictions.size() == diabetes_fc8_predictions.size());
    auto expted_begin = std::begin(diabetes_fc8_predictions);
    unsigned int at = 0;
    for (auto our_results : predictions) {
        BOOST_TEST(our_results == *expted_begin, "["<<at<<"] our results "<<our_results<<" do not match expected results "<<*expted_begin);
        ++expted_begin;
        ++at;
    }
}


/*
Gadget for deblurring of real-time spiral images
Processes B0 field map and then calls the MFI operator to do deblurring based
on multi-frequency interpolation.
Input: Image data and B0 field map attached as reference
 - Image data in rbit_[0].data_.data_
 - Map data in rbit_[0].ref_->data_ (N dimension should be "set", 0th N-dim is TE0, 1st N-dim is TE1)
 Output: three image series
 - Deblurred image
 - Uncorrected image
 - B0 map
*/
#include "gpuSpiralDeblurGadget.h"
#include "vds.h"
#include "GPUTimer.h"
#include "GenericReconJob.h"
#include "b1_map.h"
#include "check_CUDA.h"
#include "cuNDArray_elemwise.h"
#include "cuNDArray_fileio.h"
#include "cuNDArray_reductions.h"
#include "cuNDArray_utils.h"
#include "hoArmadillo.h"
#include "hoNDArray_fileio.h"
#include "hoNDArray_utils.h"
#include "ismrmrd/xml.h"
#include "sense_utilities.h"
#include "vector_td.h"
#include "vector_td_operators.h"
#include "vector_td_utilities.h"
#include <algorithm>
#include <vector>

#include "MFIOperator.h"

#define ARMA_64BIT_WORD

#ifdef USE_ARMADILLO
	#include <armadillo>
#endif

#ifdef USE_OMP
    #include <omp.h>
#endif

using namespace std;
// using namespace Gadgetron;

namespace Gadgetron {

// Define desired precision
typedef float _real;
typedef complext<_real> _complext;
typedef reald<_real,2>::Type _reald2;
typedef cuNFFT_impl<_real,2> plan_type;

  gpuSpiralDeblurGadget::gpuSpiralDeblurGadget()
    : samples_to_skip_start_(0)
    , samples_to_skip_end_(0)
    , prepared_(false)
		, prepared_B0_(false)
  {
  }

  gpuSpiralDeblurGadget::~gpuSpiralDeblurGadget() {}

  int gpuSpiralDeblurGadget::process_config(ACE_Message_Block* mb)
  {

    int number_of_devices = 0;
    if (cudaGetDeviceCount(&number_of_devices)!= cudaSuccess) {
      GDEBUG( "Error: unable to query number of CUDA devices.\n" );
      return GADGET_FAIL;
    }

    if (number_of_devices == 0) {
      GDEBUG( "Error: No available CUDA devices.\n" );
      return GADGET_FAIL;
    }

    device_number_ = deviceno.value();

    if (device_number_ >= number_of_devices) {
      GDEBUG("Adjusting device number from %d to %d\n", device_number_,  (device_number_%number_of_devices));
      device_number_ = (device_number_%number_of_devices);
    }

    if (cudaSetDevice(device_number_)!= cudaSuccess) {
      GDEBUG( "Error: unable to set CUDA device.\n" );
      return GADGET_FAIL;
    }

    cudaDeviceProp deviceProp;
    if( cudaGetDeviceProperties( &deviceProp, device_number_ ) != cudaSuccess) {
      GDEBUG( "Error: unable to query device properties.\n" );
      return GADGET_FAIL;
    }

	unsigned int warp_size = deviceProp.warpSize;

    // Start parsing the ISMRMRD XML header
    //

    ISMRMRD::IsmrmrdHeader h;
    ISMRMRD::deserialize(mb->rd_ptr(),h);

	// Get the encoding space and trajectory description
    ISMRMRD::EncodingSpace e_space = h.encoding[0].encodedSpace;
    ISMRMRD::EncodingSpace r_space = h.encoding[0].reconSpace;
    ISMRMRD::EncodingLimits e_limits = h.encoding[0].encodingLimits;
    ISMRMRD::TrajectoryDescription traj_desc;

    if (h.encoding[0].trajectoryDescription) {
      traj_desc = *h.encoding[0].trajectoryDescription;
    } else {
      GDEBUG("Trajectory description missing");
      return GADGET_FAIL;
    }

    if (traj_desc.identifier != "HargreavesVDS2000") {
      GDEBUG("Expected trajectory description identifier 'HargreavesVDS2000', not found.");
      return GADGET_FAIL;
    }

    long interleaves = -1;
    long fov_coefficients = -1;
    long sampling_time_ns = -1;
    double max_grad = -1.0;
    double max_slew = -1.0;
    double fov_coeff = -1.0;
    double kr_max = -1.0;

    for (std::vector<ISMRMRD::UserParameterLong>::iterator i (traj_desc.userParameterLong.begin()); i != traj_desc.userParameterLong.end(); ++i) {
      if (i->name == "interleaves") {
        interleaves = i->value;
      } else if (i->name == "fov_coefficients") {
        fov_coefficients = i->value;
      } else if (i->name == "SamplingTime_ns") {
        sampling_time_ns = i->value;
      } else {
        GDEBUG("WARNING: unused trajectory parameter %s found\n", i->name.c_str());
      }
    }

    for (std::vector<ISMRMRD::UserParameterDouble>::iterator i (traj_desc.userParameterDouble.begin()); i != traj_desc.userParameterDouble.end(); ++i) {
      if (i->name == "MaxGradient_G_per_cm") {
	max_grad = i->value;
      } else if (i->name == "MaxSlewRate_G_per_cm_per_s") {
	max_slew = i->value;
      } else if (i->name == "FOVCoeff_1_cm") {
	fov_coeff = i->value;
      } else if (i->name == "krmax_per_cm") {
	kr_max= i->value;
      } else {
	GDEBUG("WARNING: unused trajectory parameter %s found\n", i->name.c_str());
      }
    }

	krmax_ = kr_max/2;
	sample_time = (1.0*sampling_time_ns) * 1e-9;
    gmax_ = max_grad;
    smax_ = max_slew;
    krmax_ = kr_max*1.1;
    fov_ = fov_coeff;

    // Determine reconstruction matrix sizes
    //
	fov_vec_.push_back(r_space.fieldOfView_mm.x);
    fov_vec_.push_back(r_space.fieldOfView_mm.y);
    fov_vec_.push_back(r_space.fieldOfView_mm.z);

	kernel_width_ = buffer_convolution_kernel_width.value();
    oversampling_factor_ = buffer_convolution_oversampling_factor.value();

    image_dimensions_recon_.push_back(((static_cast<unsigned int>(std::ceil(e_space.matrixSize.x*reconstruction_os_factor_x.value()))+warp_size-1)/warp_size)*warp_size);
    image_dimensions_recon_.push_back(((static_cast<unsigned int>(std::ceil(e_space.matrixSize.y*reconstruction_os_factor_y.value()))+warp_size-1)/warp_size)*warp_size);

    image_dimensions_recon_os_ = uint64d2
      (((static_cast<unsigned int>(std::ceil(image_dimensions_recon_[0]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size,
       ((static_cast<unsigned int>(std::ceil(image_dimensions_recon_[1]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size);

	deblur_ = do_deblurring.value();

	return GADGET_OK;
  }

    int gpuSpiralDeblurGadget::process(Gadgetron::GadgetContainerMessage< IsmrmrdReconData >* m1)
    {

			//Image data in rbit_[0].data_.data_
			//Map data in rbit_[0].ref_->data_ (N dimension should be "set", 0th N-dim is TE0, 1st N-dim is TE1)

		IsmrmrdReconData* recon_bit_ = m1->getObjectPtr();

		// Allocate various counters if they are NULL
		if( !image_counter_.get() ){
			image_counter_ = boost::shared_array<long>(new long[1]);
			image_counter_[0] = 0;
		}

		//Get data from ReconDataBuffer if exists
		hoNDArray<std::complex<float>> host_data = recon_bit_->rbit_[0].data_.data_;
		hoNDArray<std::complex<float>> B0_data;
    if(recon_bit_->rbit_[0].ref_){
				B0_data = recon_bit_->rbit_[0].ref_->data_;
    }

		//Set up image containers, trajectory, and NFFT_plan - if not already prepared
		ISMRMRD::AcquisitionHeader& curr_header = recon_bit_->rbit_[0].data_.headers_(0,0,0,0,0);
		if(!prepared_ && host_data.get_size(0) > 0){ //TODO: move to process_config?
		    Prepare_Plan(recon_bit_->rbit_[0].data_);
		}

		//Set up B0 map containers, trajectory, and NFFT_plan - if not already prepared
		if(!prepared_B0_ && recon_bit_->rbit_[0].ref_){
				Prepare_B0_Plan(*recon_bit_->rbit_[0].ref_);
		}

		//If there is reference data then we need to re-compute the B0 map
		if(recon_bit_->rbit_[0].ref_){
				Calc_B0Map(B0_data, &B0_map);
		}

		//If there is image data (i.e. reconbit isn't only map data), then we reconstruct and deblur
		if(host_data.get_number_of_elements() > 0){

			cuNDArray<complext<float>> gpu_data((hoNDArray<float_complext>*)&host_data);
			nfft_plan_->compute( &gpu_data, image, &gpu_weights, NFFT_comp_mode::BACKWARDS_NC2C );
			csm_ = estimate_b1_map<float,2>( &image );
			csm_mult_MH<float,2>(&image, &reg_image, &csm_);
			host_image = *(reg_image.to_host());

			gpu_traj = host_traj;
			if(MFI.prepare(nfft_plan_, gpu_traj, *host_data.get_dimensions(), image_dimensions_recon_,
                                image_dimensions_recon_os_, sample_time, .001)){
				output_image = MFI.MFI_apply(host_image, B0_map);
			}
			host_image = *(reg_image.to_host()); //calling MFI_apply corrupts host_image, Recall it from GPU;
			//queue deblurred im
			GadgetContainerMessage<ISMRMRD::ImageHeader> *header = get_image_header(curr_header,1);
			GadgetContainerMessage< hoNDArray< std::complex<float> > >* cm2 = new GadgetContainerMessage<hoNDArray< std::complex<float> > >();
			cm2->getObjectPtr()->create(host_image.dimensions());
			memcpy(cm2->getObjectPtr()->get_data_ptr(), output_image.get_data_ptr(), output_image.get_number_of_elements()*sizeof(std::complex<float>));
			header->cont(cm2);
			if (this->next()->putq(header) < 0) {
			  GDEBUG("Failed to put job on queue.\n");
			  header->release();
			  return GADGET_FAIL;
			}

			//queue original im
			GadgetContainerMessage<ISMRMRD::ImageHeader> *header2 = get_image_header(curr_header,10);
			GadgetContainerMessage<hoNDArray< std::complex<float> > >* cm3 = new GadgetContainerMessage<hoNDArray< std::complex<float> > >();
			cm3->getObjectPtr()->create(host_image.dimensions());
			memcpy(cm3->getObjectPtr()->get_data_ptr(), host_image.get_data_ptr(), host_image.get_number_of_elements()*sizeof(std::complex<float>));
			header2->cont(cm3);
			if (this->next()->putq(header2) < 0) {
				GDEBUG("Failed to put job on queue.\n");
				header->release();
				return GADGET_FAIL;
			}

			//queue B0 map
			GadgetContainerMessage<ISMRMRD::ImageHeader> *header3 = get_image_header(curr_header,20);
			GadgetContainerMessage<hoNDArray< std::complex<float> > >* cm4 = new GadgetContainerMessage<hoNDArray< std::complex<float> > >();
			hoNDArray< std::complex<float> >B0_image;
			B0_image.create(B0_map.dimensions());
			for(int i = 0; i<B0_map.get_number_of_elements(); i++){
				B0_image[i] = std::complex<float>(B0_map[i],0.0);
			}
			cm4->getObjectPtr()->create(B0_image.dimensions());
			memcpy(cm4->getObjectPtr()->get_data_ptr(), B0_image.get_data_ptr(), B0_image.get_number_of_elements()*sizeof(std::complex<float>));header3->cont(cm4);
			if (this->next()->putq(header3) < 0) {
				GDEBUG("Failed to put job on queue.\n");
				header->release();
				return GADGET_FAIL;
			}

			image_counter_[0]++; //increment image counter
		}

		m1->release();
		return GADGET_OK;
	}

	GadgetContainerMessage<ISMRMRD::ImageHeader>* gpuSpiralDeblurGadget::get_image_header(ISMRMRD::AcquisitionHeader& curr_header, int series_index){
		// Prepare an image header for this frame
		GadgetContainerMessage<ISMRMRD::ImageHeader> *header = new GadgetContainerMessage<ISMRMRD::ImageHeader>();

			{
		// Initialize header to all zeroes (there are a few fields we do not set yet)
		ISMRMRD::ImageHeader tmp;
		*(header->getObjectPtr()) = tmp;
			}

		header->getObjectPtr()->version = curr_header.version;

		header->getObjectPtr()->matrix_size[0] = image_dimensions_recon_[0];
		header->getObjectPtr()->matrix_size[1] = image_dimensions_recon_[1];
		header->getObjectPtr()->matrix_size[2] = 1;

		header->getObjectPtr()->field_of_view[0] = fov_vec_[0];
		header->getObjectPtr()->field_of_view[1] = fov_vec_[1];
		header->getObjectPtr()->field_of_view[2] = fov_vec_[2];
		header->getObjectPtr()->channels = 1;

		header->getObjectPtr()->slice = curr_header.idx.slice;
		header->getObjectPtr()->set = curr_header.idx.set;

		header->getObjectPtr()->acquisition_time_stamp = curr_header.acquisition_time_stamp;
		memcpy(header->getObjectPtr()->physiology_time_stamp, curr_header.physiology_time_stamp, sizeof(uint32_t)*ISMRMRD::ISMRMRD_PHYS_STAMPS);

		memcpy(header->getObjectPtr()->position, curr_header.position, sizeof(float)*3);
		memcpy(header->getObjectPtr()->read_dir, curr_header.read_dir, sizeof(float)*3);
		memcpy(header->getObjectPtr()->phase_dir, curr_header.phase_dir, sizeof(float)*3);
		memcpy(header->getObjectPtr()->slice_dir, curr_header.slice_dir, sizeof(float)*3);
		memcpy(header->getObjectPtr()->patient_table_position, curr_header.patient_table_position, sizeof(float)*3);

		header->getObjectPtr()->data_type = ISMRMRD::ISMRMRD_CXFLOAT;
		header->getObjectPtr()->image_index = image_counter_[0];
		header->getObjectPtr()->image_series_index = series_index;

		return header;
	}

	void gpuSpiralDeblurGadget::Calc_B0Map(hoNDArray<std::complex<float>>& B0_data, hoNDArray<float>* B0_map){
		size_t R0 = B0_data.get_size(0);
		size_t E1 = B0_data.get_size(1);
		size_t E2 = B0_data.get_size(2);
		size_t CHA = B0_data.get_size(3);
		size_t N = B0_data.get_size(4);

		//Low pass filter map data since conjugate phase reconstruction assumes smoothly varying B0 map
		for(int i =0; i < B0_data.get_number_of_elements(); i++){
			B0_data[i] *= exp(-.5*pow((i%R0)/(R0/3),2.) );
		}
		//Recon B0 images
		hoNDArray<std::complex<float>>* B0_data_1 = new hoNDArray<std::complex<float>>(R0,E1,E2,CHA,B0_data.get_data_ptr());
		cuNDArray<complext<float>> gpu_B0_data((hoNDArray<float_complext>*)B0_data_1);
		nfft_plan_B0_->compute( &gpu_B0_data, image, &gpu_weights_B0, NFFT_comp_mode::BACKWARDS_NC2C );
		csm_ = estimate_b1_map<float,2>( &image );
		csm_mult_MH<float,2>(&image, &reg_image, &csm_);
		hoNDArray<complext<float>> B0_temp_0 = *reg_image.to_host();
		hoNDArray<std::complex<float>>* B0_data_2 = new hoNDArray<std::complex<float>>(R0,E1,E2,CHA,B0_data.get_data_ptr()+R0*E1*E2*CHA);//Start at index N = 1
		gpu_B0_data = *((hoNDArray<float_complext>*)B0_data_2);
		nfft_plan_B0_->compute( &gpu_B0_data, image, &gpu_weights_B0, NFFT_comp_mode::BACKWARDS_NC2C );
		csm_mult_MH<float,2>(&image, &reg_image, &csm_);
		hoNDArray<complext<float>> B0_temp_1 = *reg_image.to_host();
		//Compute map
		B0_map->clear();
		B0_map->create(B0_temp_0.dimensions());
		B0_map->fill(0.0);
		auto map_ptr = B0_map->get_data_ptr();
		for (int i = 0; i < B0_temp_0.get_number_of_elements(); i++) {
				if(abs(B0_temp_1[i]) > 3){ //assumes SNR units and filters out noise < 3 std
					map_ptr[i] = Gadgetron::arg(B0_temp_0[i]*Gadgetron::conj(B0_temp_1[i]))/( 2*M_PI*.001 );//delTE = 1 ms
				}
		}

	}

	void gpuSpiralDeblurGadget::Prepare_Plan(IsmrmrdDataBuffered& data){
			size_t R0 = data.data_.get_size(0);
			size_t E1 = data.data_.get_size(1);
			size_t E2 = data.data_.get_size(2);
			size_t CHA = data.data_.get_size(3);

			//Setup image arrays
			image_dimensions_recon_.push_back(CHA);
			image.create(&image_dimensions_recon_);
			image_dimensions_recon_.pop_back();
			reg_image.create(&image_dimensions_recon_);

			host_traj.create(R0*E1);
			host_weights.create(R0*E1);

			//Trajectory should be attached, but if it isn't we call calc_vds
			ISMRMRD::AcquisitionHeader& curr_header = data.headers_(0,0,0,0,0);
			if (curr_header.trajectory_dimensions != 3) {
				//Setup calc_vds parameters
				int     nfov   = 1;
				int     ngmax  = 1e7;       /*  maximum number of gradient samples      */
				double  *xgrad;             /*  x-component of gradient.                */
				double  *ygrad;             /*  y-component of gradient.                */
				double  *x_trajectory;
				double  *y_trajectory;
				double  *weighting;
				int     ngrad;
				//Map trajecotry is different, parameters defined in user_floats
				calc_vds(smax_,gmax_,sample_time,sample_time,E1,&fov_,nfov,krmax_,ngmax,&xgrad,&ygrad,&ngrad);
				calc_traj(xgrad, ygrad, R0, E1, sample_time, krmax_, &x_trajectory, &y_trajectory, &weighting);

				for (int i = 0; i < (R0*E1); i++) {
					host_traj[i]   = floatd2(-x_trajectory[i]/2.,-y_trajectory[i]/2.);
					host_weights[i] = weighting[i];
				}

				delete [] xgrad;
				delete [] ygrad;
				delete [] x_trajectory;
				delete [] y_trajectory;
				delete [] weighting;
			}
			//If traj attached:
			else{
				for (int i = 0; i < (R0*E1); i++) {
					auto trajectory = data.trajectory_->get_data_ptr();
					host_traj[i]   = floatd2(trajectory[i*3],trajectory[i*3+1]);
					host_weights[i] = trajectory[i*3+2];
				}
			}

		//upload to gpu
		gpu_traj = host_traj;
		gpu_weights = host_weights;
		//pre-process
                if (nfft_plan_) nfft_plan_->reconfigure(from_std_vector<size_t,2>(image_dimensions_recon_),image_dimensions_recon_os_,kernel_width_);
            	else nfft_plan_ = NFFT<cuNDArray,_real,2>::make_plan( from_std_vector<size_t,2>(image_dimensions_recon_), image_dimensions_recon_os_, kernel_width_ );
		nfft_plan_->preprocess(&gpu_traj, NFFT_prep_mode::ALL);
		prepared_ = true;
	}

	void gpuSpiralDeblurGadget::Prepare_B0_Plan(IsmrmrdDataBuffered& data){
		ISMRMRD::AcquisitionHeader& B0_header = data.headers_(0,0,0,0,0);
		size_t R0 = data.data_.get_size(0);
		size_t E1 = data.data_.get_size(1);

		B0_traj.create(R0*E1);
		B0_weights.create(R0*E1);

		float krmaxB0_ = 2.*(B0_header.user_float[4])/10000.; //TODO: B0 acquisition info currently embedded into user parameters. In the future this should be in Encoding[1]. Requires amending XSL.
		if (B0_header.trajectory_dimensions != 3) {
			//Setup calc_vds parameters
			const int     nfov   = 2;
			int     ngmax  =1e7;       /*  maximum number of gradient samples      */
			double  *xgrad;             /*  x-component of gradient.                */
			double  *ygrad;             /*  y-component of gradient.                */
			double  *x_trajectory;
			double  *y_trajectory;
			double  *weighting;
			int     ngrad;
			//Map trajecotry is different, parameters defined in user_floats
			std::cout << "fov " << B0_header.user_float[5] << std::endl;
			std::cout << "smax " << 3*B0_header.user_float[3]/10. << std::endl;
			std::cout << "gmax " << B0_header.user_float[1]/10. << std::endl;
			std::cout << "krmax " << 2*((B0_header.user_float[4])/10000.) << std::endl;
			std::cout << "sampling_time " << sample_time << std::endl;
			double fov2_[nfov] = {B0_header.user_float[5], -1/(1.1*2*((B0_header.user_float[4])/10000.))*B0_header.user_float[5]};
			calc_vds(3.*((B0_header.user_float[3])/10.),(B0_header.user_float[1])/10.,sample_time,sample_time,E1,&fov2_[0],nfov,2.*((B0_header.user_float[4])/10000.),ngmax,&xgrad,&ygrad,&ngrad);
			calc_traj(xgrad, ygrad, R0, E1, sample_time, krmaxB0_, &x_trajectory, &y_trajectory, &weighting);

			for (int i = 0; i < (R0*E1); i++) {
				B0_traj[i]   = floatd2(-x_trajectory[i]/(2.),-y_trajectory[i]/(2.));
				B0_weights[i] = weighting[i];
			}

			delete [] xgrad;
			delete [] ygrad;
			delete [] x_trajectory;
			delete [] y_trajectory;
			delete [] weighting;
		}
		else{
			for (int i = 0; i < (R0*E1); i++) {
				auto B0_trajectory = data.trajectory_->get_data_ptr();
				B0_traj[i]   = floatd2(B0_trajectory[i*3],B0_trajectory[i*3+1]);
				B0_weights[i] = B0_trajectory[i*3+2];
			}
		}

		//upload to gpu
		gpu_traj = B0_traj;
		gpu_weights_B0 = B0_weights;

		//pre-process
                if (nfft_plan_B0_) nfft_plan_B0_->reconfigure(from_std_vector<size_t,2>(image_dimensions_recon_),image_dimensions_recon_os_,kernel_width_);
		else nfft_plan_B0_ = NFFT<cuNDArray,_real,2>::make_plan( from_std_vector<size_t,2>(image_dimensions_recon_), image_dimensions_recon_os_, kernel_width_ );
		nfft_plan_B0_->preprocess(&gpu_traj, NFFT_prep_mode::NC2C);
		prepared_B0_= true;
	}

  GADGET_FACTORY_DECLARE(gpuSpiralDeblurGadget)
}


//==================================================================================================
/*!
  @file

  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_EXPO_BASE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_EXPO_BASE_HPP_INCLUDED

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/simd/if_allbits_else.hpp>
#include <boost/simd/function/simd/is_nan.hpp>
#endif

#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/if_zero_else.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/arch/common/detail/scalar/expo_reduction.hpp>
#include <boost/simd/arch/common/detail/scalar/expo_scale.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bs = boost::simd;
    template < class A0, class Tag>
    struct exponential < A0, Tag, tag::simd_type, double>
    {
      typedef exp_reduction<A0,Tag>                        reduc_t;
      // compute exp(ax)
      static inline A0 expa(const A0& a0)
      {
        A0 hi, lo, x;
        A0 k = reduc_t::reduce(a0, hi, lo, x);
        A0 c = reduc_t::approx(x);
        c = reduc_t::finalize(x, c, hi, lo);
        c = bs::if_zero_else(reduc_t::isleminlog(a0), scale(c, k));
        c = bs::if_else(reduc_t::isgemaxlog(a0), bs::Inf<A0>(), c);
#ifdef BOOST_SIMD_NO_INVALIDS
        return c;
#else
        return if_allbits_else(is_nan(a0), c);
#endif
      }
    };

    template < class A0, class Tag >
    struct exponential < A0, Tag, tag::simd_type, float>
    {
      typedef exp_reduction<A0,Tag>                        reduc_t;
      // compute exp(ax)
      static inline A0 expa(const A0& a0)
      {
        A0 x;
        A0 k = reduc_t::reduce(a0, x);
        x = reduc_t::approx(x);
        x = bs::if_zero_else(reduc_t::isleminlog(a0), scale(x, k));
        x = bs::if_else(reduc_t::isgemaxlog(a0), bs::Inf<A0>(), x);
#ifdef BOOST_SIMD_NO_INVALIDS
        return x;
#else
        return if_allbits_else(is_nan(a0), x);
#endif
      }
    };

  }
} }

#endif


#define BOOST_TEST_MODULE NormalizePolytopeTestSuite
#define BOOST_TEST_DYN_LINK

#include <boost/test/included/unit_test.hpp>
#include <cmath>
#include <hops/Polytope/NormalizePolytope.hpp>

BOOST_AUTO_TEST_SUITE(NormalizePolytope)

    BOOST_AUTO_TEST_CASE(NormalizeSimplex) {
        Eigen::MatrixXd expectedA(4, 3);
        expectedA << 1. / std::sqrt(3), 1. / std::sqrt(3), 1. / std::sqrt(3),
                1, 0, 0,
                0, 1, 0,
                0, 0, 1;
        Eigen::VectorXd expectedB(4);
        expectedB << 1. / std::sqrt(3), 1, 1, 1;

        Eigen::MatrixXd A(4, 3);
        A << 1, 1, 1,
                1, 0, 0,
                0, 1, 0,
                0, 0, 1;
        Eigen::VectorXd b = Eigen::VectorXd::Ones(4);

        hops::normalizePolytope(A, b);

        BOOST_CHECK(A.isApprox(expectedA));
        BOOST_CHECK(b.isApprox(expectedB));
    }

BOOST_AUTO_TEST_SUITE_END()


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <string>

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "tudat/math/basic/linearAlgebra.h"
#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/astro/basic_astro/orbitalElementConversions.h"

#include "tudat/interface/spice/spiceInterface.h"
#include "tudat/math/integrators/rungeKuttaCoefficients.h"
#include "tudat/math/interpolators/lagrangeInterpolator.h"
#include "tudat/astro/basic_astro/accelerationModel.h"
#include "tudat/astro/basic_astro/keplerPropagator.h"
#include "tudat/io/basicInputOutput.h"

#include "tudat/astro/basic_astro/orbitalElementConversions.h"
#include "tudat/simulation/environment_setup/body.h"
#include "tudat/astro/propagators/nBodyCowellStateDerivative.h"
#include "tudat/simulation/propagation_setup/dynamicsSimulator.h"
#include "tudat/math/integrators/createNumericalIntegrator.h"
#include "tudat/simulation/environment_setup/createBodies.h"
#include "tudat/simulation/estimation_setup/createNumericalSimulator.h"
#include "tudat/simulation/environment_setup/defaultBodies.h"

namespace tudat
{

namespace unit_tests
{

//Using declarations.
using namespace tudat::ephemerides;
using namespace tudat::interpolators;
using namespace tudat::numerical_integrators;
using namespace tudat::spice_interface;
using namespace tudat::simulation_setup;
using namespace tudat::basic_astrodynamics;
using namespace tudat::orbital_element_conversions;
using namespace tudat::propagators;
using namespace tudat;

BOOST_AUTO_TEST_SUITE( test_global_frame_origin )

//! Test to ensure that a point-mass acceleration on a body produces a Kepler orbit (to within
//! numerical error bounds).
template< typename TimeType, typename StateScalarType >
Eigen::Matrix< StateScalarType, 6, 1 > testGlobalFrameOrigin(
        const std::string& globalFrameOrigin, const std::string& moonEphemerisOrigin  )
{
    std::cout << "Testing with origin: ************************************** " << globalFrameOrigin << std::endl;
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Define bodies in simulation.
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Earth" );
    bodyNames.push_back( "Moon" );
    bodyNames.push_back( "Sun" );
    bodyNames.push_back( "Mars" );
    bodyNames.push_back( "Venus" );

    // Specify initial time
    double initialEphemerisTime = 1.0E7;
    double finalEphemerisTime = initialEphemerisTime + 56.0 * 86400.0;
    double maximumTimeStep = 3600.0;
    double buffer = 5.0 * maximumTimeStep;

    // Create bodies needed in simulation
    BodyListSettings bodySettings =
            getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer,
                                    globalFrameOrigin, "ECLIPJ2000" );

    if( std::is_same< long double, StateScalarType >::value )
    {
        std::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >(
                    bodySettings.at( "Moon" )->ephemerisSettings )->setUseLongDoubleStates( 1 );
    }

    // Change ephemeris origins to test full functionality
    std::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >( bodySettings.at( "Moon" )->ephemerisSettings )->
            resetFrameOrigin( moonEphemerisOrigin );
    std::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >( bodySettings.at( "Earth" )->ephemerisSettings )->
            resetFrameOrigin( "Sun" );
    std::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >( bodySettings.at( "Mars" )->ephemerisSettings )->
            resetFrameOrigin( "Sun" );
    std::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >( bodySettings.at( "Venus" )->ephemerisSettings )->
            resetFrameOrigin( "SSB" );

    SystemOfBodies bodies = createSystemOfBodies( bodySettings );

    // Set accelerations between bodies that are to be taken into account.
    SelectedAccelerationMap accelerationMap;
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfMoon;
    accelerationsOfMoon[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationsOfMoon[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationsOfMoon[ "Mars" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationsOfMoon[ "Venus" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationMap[ "Moon" ] = accelerationsOfMoon;

    // Propagate the moon only
    std::vector< std::string > bodiesToIntegrate;
    bodiesToIntegrate.push_back( "Moon" );
    std::vector< std::string > centralBodies;
    centralBodies.push_back( "Earth" );

    // Define settings for numerical integrator.
    std::shared_ptr< IntegratorSettings< TimeType > > integratorSettings =
            std::make_shared< IntegratorSettings< TimeType > >
            ( rungeKutta4, initialEphemerisTime, 300.0 );

    // Create acceleration models and propagation settings.
    Eigen::Matrix< StateScalarType, 6, 1  > systemInitialState =
                spice_interface::getBodyCartesianStateAtEpoch(
                  bodiesToIntegrate[ 0 ], centralBodies[ 0 ], "ECLIPJ2000", "NONE", initialEphemerisTime ).
                template cast< StateScalarType >( );
    AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodies, accelerationMap, bodiesToIntegrate, centralBodies );
    std::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > propagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< StateScalarType > >
            ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, finalEphemerisTime );

    // Create dynamics simulation object.
    SingleArcDynamicsSimulator< StateScalarType, TimeType > dynamicsSimulator(
                bodies, integratorSettings, propagatorSettings, true, false, true );

    return dynamicsSimulator.getEquationsOfMotionNumericalSolution( ).rbegin( )->second;
}

BOOST_AUTO_TEST_CASE( testCowellPropagatorKeplerCompare )
{
    std::vector< std::string > origins;

    origins.push_back( "SSB" );
    origins.push_back( "Earth" );
    origins.push_back( "Mars" );
    origins.push_back( "Venus" );
    origins.push_back( "Sun" );

    // Get final state with Earth ephemeris origin and SSB global frame origin
    Eigen::Vector6d benchmarkFinalState = testGlobalFrameOrigin< double, double >( "SSB", "Earth" );

    // Iterate over all combinations of ephemeris/global frame origins
    for( unsigned int i = 0; i < origins.size( ); i++ )
    {
        for( unsigned int j = 0; j < origins.size( ); j++ )
        {
            Eigen::Vector6d currentFinalState = testGlobalFrameOrigin< double, double >(
                        origins.at( i ), origins.at( j ) );
            for( unsigned int k = 0; k < 3 ; k++ )
            {
                BOOST_CHECK_SMALL( std::fabs( benchmarkFinalState( k ) - currentFinalState( k ) ), 1.0E-4 );
                BOOST_CHECK_SMALL( std::fabs( benchmarkFinalState( k + 3 ) - currentFinalState( k + 3 ) ), 1.0E-9 );
            }

        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


#include "search_chargingpile_manager.h"

#include <boost/bind.hpp>
#include <sensor_msgs/PointCloud.h>
#include <algorithm>
#include <boost/make_shared.hpp>

#include "search_chargingpile_FSM.h"
#include "weighted_fit.h"

namespace peter_chargingpile_search {

#define PAI 3.1415927

const int SearchChargingPileManager::Const_Queue_Length = 1500;             //sourceNum = 1080
const double SearchChargingPileManager::Recognization_Angle = 160.0;        //unit:angle
const double SearchChargingPileManager::LaserXOffsetToRobot = 0.2;          //unit:m

//dynamic_reconfigure
const double SearchChargingPileManager::Break_Distance = 0.02;              //unit:m
const int SearchChargingPileManager::Ignore_Point_Num = 25;                 //unit:none
const double SearchChargingPileManager::Max_Salient_Tolerance = 0.005;      //unit:m
const double SearchChargingPileManager::Max_FestureAngle_Tolerance = 1.5;   //unit:angle
const double SearchChargingPileManager::Max_Variance_Tolerance = 0.01;      //unit:Standard Deviation

//SearchChargingPileManager::
SearchChargingPileManager::SearchChargingPileManager(ros::NodeHandle nh) : _nh(nh)
{

    /*
    _param_break_distance               = _nh.param("peter_break_distance", Break_Distance);
    _param_ignore_point_num             = _nh.param("peter_ignore_point_num", Ignore_Point_Num);
    _param_max_salient_tolerance        = _nh.param("peter_max_salient_tolerance", Max_Salient_Tolerance);
    _param_max_variance_tolerance       = _nh.param("peter_max_variance_tolerance", Max_Variance_Tolerance);
    _param_max_festureangle_tolerance   = _nh.param("peter_max_festureangle_tolerance", Max_FestureAngle_Tolerance);
*/

    _param_break_distance               = Break_Distance;
    _param_ignore_point_num             = Ignore_Point_Num;
    _param_max_salient_tolerance        = Max_Salient_Tolerance;
    _param_max_variance_tolerance       = Max_Variance_Tolerance;
    _param_max_festureangle_tolerance   = Max_FestureAngle_Tolerance;

    _param_linear_vel = 0.1;
    _param_angular_vel = 0.5;

    // xiwrong-->todo temp _chargeOrderFlag
    _chargeOrderFlag = true;
    _powerStatusFlag = false;

    //    _isExistValidObj = false;
    _searchFSM = boost::make_shared<SearchChargingPileFSM>();

    _pointsPub = _nh.advertise<sensor_msgs::PointCloud>("debug/cloud", 50);
    _ctrlCmdVelPub = _nh.advertise<geometry_msgs::Twist>("ctrl_cmd_vel", 10);

    ros::Duration period = ros::Duration(1./20);
    _ctrlCmdVelTimer = _nh.createTimer(period, &SearchChargingPileManager::onTimerCtrlCmdVel, this);

}

void SearchChargingPileManager::addLaserScanMsg(const sensor_msgs::LaserScanConstPtr& msg)
{
    size_t index_sum = (msg->angle_max - msg->angle_min)/ msg->angle_increment;
    _sourcePoints.clear();
    double angle, range;
    for (size_t index = 0; index <= index_sum; index++)
    {
        PointInfo point;
        angle = msg->angle_min + index* msg->angle_increment;
        range = msg->ranges[index];
        point.x =  range* cos(angle);
        point.y = -range* sin(angle);
        point.index = index;
        point.angle = angle;
        point.range = range;

        _sourcePoints.push_back(point);
    }

    // ===============deal with points to find objective==================
    splitLaserWithRange();                                      //calculate _breakedLaser...
    filterSplitLaser(_breakedLaserAngle, _breakedLaserRange);   //calculate _filterLaser...
    changeRangetoXY(_filterLaserAngle, _filterLaserRange);
    PointWithTimeStamp keyPoint;

    // ====== method 1 ======
    //    bool flag = findKeyPointFromLines(keyPoint);
    // ====== method 2 ======
    bool flag = recurFindKeyPointFromLines(keyPoint);

    // ===============show pointcloud============
    publishPointCloud(_filterLaserAngle, _filterLaserRange, keyPoint, flag);

    // ===============send packet================
    if (flag)
    {
        UpdateDataPacket packet;
        packet.objPosition = keyPoint;
        packet.objPosition.x += LaserXOffsetToRobot;    //change laser coordinate to robot coordinate

        packet.chargeOrderFlag = _chargeOrderFlag;
        packet.powerStatusFlag = _powerStatusFlag;

        tf::Transform transform;
        _searchFSM->update(packet, transform);
        //        std::cout << transform.getOrigin().x() << " " << transform.getOrigin().y() << std::endl;

        _vel_msg.linear.x  = _param_linear_vel* sqrt(pow(transform.getOrigin().x(), 2.0) + pow(transform.getOrigin().y(), 2.0));
        _vel_msg.angular.z = (transform.getOrigin().x() > 0.05)?
                    _param_angular_vel* atan2(transform.getOrigin().y(), transform.getOrigin().x()) :
                    _param_angular_vel* transform.getRotation().getAngle();
        //        boost::mutex::scoped_lock lock(_ctrlCmdVelMutex);
        //        _isExistValidObj = true;

    }

}

void SearchChargingPileManager::onTimerCtrlCmdVel(const ros::TimerEvent& t)
{
    _ctrlCmdVelPub.publish(_vel_msg);

}

void SearchChargingPileManager::publishPointCloud(std::vector<double> angles,
                                                  std::vector<double> ranges,
                                                  PointWithTimeStamp keyPoint,
                                                  bool isShowKeyPoint)
{
    if (angles.size() != ranges.size())
        return;

    int CirclePointNum = isShowKeyPoint? 50: 0;
    int ArrowPointNum = isShowKeyPoint? 30: 0;
    double CircleRadius = 0.05;
    double ArrowLength = 0.15;

    int num_points = angles.size();
    sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "laser";
    cloud.points.resize(num_points + CirclePointNum + ArrowPointNum);

    cloud.channels.resize(1);
    cloud.channels[0].name = "intensities";
    cloud.channels[0].values.resize(num_points + CirclePointNum + ArrowPointNum);

    double x, y;
    double color = 2.0;
    for(int i = 0; i < num_points; i++)
    {
        if (ranges.at(i) < -0.1)
        {
            color += 1.0;
            continue;
        }

        assert(ranges.at(i) > -0.1);
        x = ranges.at(i)* cos(angles.at(i));
        y = ranges.at(i)* sin(angles.at(i));
        cloud.points[i].x = x;
        cloud.points[i].y = y;
        cloud.points[i].z = 0;
        cloud.channels[0].values[i] = color;
    }

    //draw  feature point using circle
    for (int i=0; i < CirclePointNum; i++ )
    {
        cloud.points[i+num_points].x = keyPoint.x + CircleRadius*cos(2*PAI*i/CirclePointNum);
        cloud.points[i+num_points].y = keyPoint.y + CircleRadius*sin(2*PAI*i/CirclePointNum);
        cloud.points[i+num_points].z = 0;
        cloud.channels[0].values[i+num_points] = 0;
    }
    //draw orientation using line
    for (int i=0; i < ArrowPointNum; i++)
    {
        cloud.points[CirclePointNum + num_points + i].x = keyPoint.x + cos(keyPoint.theta)*i*ArrowLength/ArrowPointNum;
        cloud.points[CirclePointNum + num_points + i].y = keyPoint.y + sin(keyPoint.theta)*i*ArrowLength/ArrowPointNum;
        cloud.points[CirclePointNum + num_points + i].z = 0;
        cloud.channels[0].values[CirclePointNum + num_points + i] = 0;
    }

    _pointsPub.publish(cloud);
}

int  SearchChargingPileManager::splitLaserWithRange()
{
    int breakCount = 0;
    double distance = 0;
    int breakPointNum = 0;

    double range = 0;
    double lastRange = _sourcePoints.at(0).range;
    double angle = _sourcePoints.at(0).angle;

    _breakedLaserRange.clear();
    _breakedLaserAngle.clear();

    _breakedLaserRange.push_back(lastRange);
    _breakedLaserAngle.push_back(angle);

    for (int i = 1; i< _sourcePoints.size(); i++)
    {
        breakPointNum++;
        range = _sourcePoints.at(i).range;
        angle = _sourcePoints.at(i).angle;
        distance = fabs(range - lastRange);

        if (distance > _param_break_distance)
        {
            _breakedLaserRange.push_back(-1.0*breakPointNum);
            _breakedLaserAngle.push_back(1000.0);
            breakCount++;
            breakPointNum = 0;
        }
        _breakedLaserRange.push_back(range);
        _breakedLaserAngle.push_back(angle);
        lastRange = range;
    }
    _breakedLaserRange.push_back(-1.0*(breakPointNum+1));
    _breakedLaserAngle.push_back(1000.0);
    //cout<<"breakCnt: "<<breakCnt<<endl;
    return breakCount;
}

void SearchChargingPileManager::filterSplitLaser(std::vector<double> angles, std::vector<double> ranges)
{
    if (angles.size() != ranges.size())
    {
        return;
    }

    _filterLaserAngle.clear();
    _filterLaserRange.clear();

    int num_points = angles.size();

    double x, y;
    int pointNum;

    for(int i = num_points - 1; i >= 0; i--)
    {
        if (ranges.at(i) < -0.1)
        {
            //xiwrong-->todo  //risk come from double --> int
            pointNum = (int)fabs(ranges.at(i)); //calculate number of points in one split area
        }
        if (pointNum > _param_ignore_point_num)
        {
            _filterLaserAngle.push_back(angles.at(i));
            _filterLaserRange.push_back(ranges.at(i));
        }
    }
    std::reverse(_filterLaserAngle.begin(), _filterLaserAngle.end());
    std::reverse(_filterLaserRange.begin(), _filterLaserRange.end());
}

void SearchChargingPileManager::changeRangetoXY(std::vector<double> angles, std::vector<double> ranges)
{
    // =========================== change double-angles&ranges to vector(x, y)

    if (angles.size() != ranges.size())
        return;
    _splitLines.clear();
    int num_points = angles.size();

    std::vector<XYPoint> oneLine;

    for (int i = num_points - 1; i >=0; i--)
    {
        if (ranges.at(i) < 0)
        {
            _splitLines.push_back(oneLine);
            oneLine.clear();
            continue;
        }
        XYPoint onePoint;
        onePoint.x = ranges.at(i)* cos(angles.at(i));
        onePoint.y = ranges.at(i)* sin(angles.at(i));
        oneLine.push_back(onePoint);
    }
}

void SearchChargingPileManager::recurSplitPointsVector(std::vector<XYPoint> line, double eps,
                                                       std::vector<weighted_fit::LinePara>& lineParas)
{
    int n = line.size();
    if (n < _param_ignore_point_num)
        return;  // exit recursion

    double dis = sqrt(pow(line.at(0).x - line.at(n-1).x, 2.0) + pow(line.at(0).y - line.at(n-1).y, 2.0));
    double cosTheta =  (line.at(n-1).x - line.at(0).x)/ dis;
    double sinTheta = -(line.at(n-1).y - line.at(0).y)/ dis;
    double maxDis = 0;
    int maxDisIndex = 0;
    double dbDis;

    for(int i = 1 ; i < n-1 ; i++)
    {
        dbDis = fabs((line.at(i).y - line.at(0).y)*cosTheta + (line.at(i).x - line.at(0).x)*sinTheta);
        if( dbDis > maxDis)
        {
            maxDis = dbDis;
            maxDisIndex = i;
        }
    }

    if(maxDis > eps)
    {
        //xiwrong-->todo
        std::vector<XYPoint> splitLine1(line.begin(), line.begin() + maxDisIndex);
        std::vector<XYPoint> splitLine2(line.begin() + maxDisIndex, line.end());
        recurSplitPointsVector(splitLine1, eps, lineParas);
        recurSplitPointsVector(splitLine2, eps, lineParas);
    }
    else
    {
        weighted_fit::LinePara tempLinePara;
        double tempx[Const_Queue_Length];
        double tempy[Const_Queue_Length];

        for (int j = 0; j < line.size(); j++)
        {
            tempx[j] = line.at(j).x;
            tempy[j] = line.at(j).y;
        }

        weighted_fit::weightedFit(tempx, tempy, line.size(), &tempLinePara);

        if (tempLinePara.standardDeviation < Max_Variance_Tolerance)
        {
            lineParas.push_back(tempLinePara);
        }
    }
}

bool SearchChargingPileManager::recurFindKeyPointFromLines(PointWithTimeStamp& keyPoint)
{
    bool findResultFlag = false;

    double tempVariance = 0.0;
    double minVariance = 100.0;
    weighted_fit::LinePara tmpLinePara1;
    weighted_fit::LinePara tmpLinePara2;
    double changedAngle1 = 0.0;
    double changedAngle2 = 0.0;

    for (int i = 0; i < _splitLines.size(); i++)
    {
        std::vector<weighted_fit::LinePara> tempLineParas;
        std::vector<XYPoint> tempLine = _splitLines.at(i);
        recurSplitPointsVector(tempLine, _param_max_salient_tolerance, tempLineParas);

        if (tempLineParas.size() < 2)
            continue;
        for (int j = 0; j < tempLineParas.size()-1; j++)
        {
            for (int k = j+1; k < tempLineParas.size(); k++)
            {
                tmpLinePara1 = tempLineParas[j];
                tmpLinePara2 = tempLineParas[k];
                double theta = (tmpLinePara1.Rho - tmpLinePara2.Rho)*180/PAI;
                double variance1 = tmpLinePara1.standardDeviation;
                double variance2 = tmpLinePara2.standardDeviation;
                if ((fabs( theta - 180 + Recognization_Angle) < _param_max_festureangle_tolerance ||
                     fabs( theta + Recognization_Angle ) < _param_max_festureangle_tolerance) &&
                        variance1 < _param_max_variance_tolerance && variance2 < _param_max_variance_tolerance) //filter [angle=160]
                {
                    tempVariance = std::max(variance1, variance2);
                    if (tempVariance < minVariance)
                    {
                        minVariance = tempVariance;
                        keyPoint.timestamp = ros::Time::now();

                        keyPoint.x = (tmpLinePara2.b - tmpLinePara1.b)/ (tmpLinePara1.a - tmpLinePara2.a);
                        keyPoint.y = tmpLinePara1.a* keyPoint.x + tmpLinePara1.b;
                        keyPoint.z = 0;

                        changedAngle2 = (tmpLinePara2.Rho > 0)? tmpLinePara2.Rho: PAI+tmpLinePara2.Rho;
                        changedAngle1 = (tmpLinePara1.Rho < 0 || tmpLinePara2.Rho < 0)? PAI+tmpLinePara1.Rho: tmpLinePara1.Rho;
//                        changedAngle1 = (tmpLinePara1.Rho > 0)? tmpLinePara1.Rho: PAI+tmpLinePara1.Rho;

                        double average = (changedAngle1 + changedAngle2)/2 - PAI/2;
                        keyPoint.theta = average;

//                        ROS_INFO_STREAM(" para-r1: " << tmpLinePara1.Rho/PAI*180 <<
//                                        " para-r2: " << tmpLinePara2.Rho/PAI*180 <<
//                                        " average: " << average/PAI*180);

                        findResultFlag = true;
                    }
                }
            }
        }
    }

    return findResultFlag;

}

int  SearchChargingPileManager::findSalientIndexByVector(std::vector<XYPoint> line, double eps)
{
    int n = line.size();
    if (n < _param_ignore_point_num)
        return 0;
    double dis = sqrt(pow(line.at(0).x - line.at(n-1).x, 2.0) + pow(line.at(0).y - line.at(n-1).y, 2.0));
    double cosTheta =  (line.at(n-1).x - line.at(0).x)/ dis;
    double sinTheta = -(line.at(n-1).y - line.at(0).y)/ dis;
    double maxDis = 0;
    int    maxDisIndex = 0;
    double dbDis;

    for(int i = 1 ; i < n-1 ; i++)
    {
        dbDis = fabs((line.at(i).y - line.at(0).y)*cosTheta + (line.at(i).x - line.at(0).x)*sinTheta);
        if( dbDis > maxDis)
        {
            maxDis = dbDis;
            maxDisIndex = i;
        }
    }
    if(maxDis > eps)
    {
        return maxDisIndex;
    }
    return 0;
}

bool SearchChargingPileManager::findKeyPointFromLines(PointWithTimeStamp& keyPoint)
{
    bool findResultFlag = false;
    int returnValue;
    double tempx[Const_Queue_Length];
    double tempy[Const_Queue_Length];

    weighted_fit::LinePara tmpLinePara1;
    weighted_fit::LinePara tmpLinePara2;

    double tempVariance = 0.0;
    double minVariance = 100.0;
    double changedAngle1 = 0.0;
    double changedAngle2 = 0.0;

    for (int i = 0; i < _splitLines.size(); i++)
    {
        std::vector<XYPoint> tempLine = _splitLines.at(i);
        for (int j = 0; j < tempLine.size(); j++)
        {
            tempx[j] = tempLine.at(j).x;
            tempy[j] = tempLine.at(j).y;
        }

        returnValue = findSalientIndexByVector(_splitLines.at(i), _param_max_salient_tolerance);
        if (returnValue == 0 || returnValue < _param_ignore_point_num ||
                tempLine.size() - returnValue < _param_ignore_point_num)
            continue;
        //xiwrong-->todo      //Temporary processing--split one line into two lines -- should consider recursion
        //        double variance1 =
        weighted_fit::weightedFit(tempx, tempy, returnValue, &tmpLinePara1);
        //        double variance2 =
        weighted_fit::weightedFit(tempx + returnValue, tempy + returnValue, tempLine.size() - returnValue, &tmpLinePara2);
        double variance1 = tmpLinePara1.standardDeviation;
        double variance2 = tmpLinePara2.standardDeviation;

        double theta = (tmpLinePara1.Rho - tmpLinePara2.Rho)*180/PAI;

        if ((fabs( theta - 180 + Recognization_Angle) < _param_max_festureangle_tolerance ||
             fabs( theta + Recognization_Angle ) < _param_max_festureangle_tolerance) &&
                variance1 < _param_max_variance_tolerance && variance2 < _param_max_variance_tolerance)    //filter [angle=160]
        {
            tempVariance = std::max(variance1, variance2);
            if (tempVariance < minVariance)
            {
                minVariance = tempVariance;

                keyPoint.timestamp = ros::Time::now();

                keyPoint.x = (tmpLinePara2.b - tmpLinePara1.b)/ (tmpLinePara1.a - tmpLinePara2.a);
                keyPoint.y = tmpLinePara1.a* keyPoint.x + tmpLinePara1.b;
                keyPoint.z = 0;
                //               std::cout << " tmpLinePara1 = "  << tmpLinePara1.Rho*180/PAI <<
                //                            " tmpLinePara2 = "  << tmpLinePara2.Rho*180/PAI << std::endl;

                changedAngle1 = (tmpLinePara1.Rho > 0)? tmpLinePara1.Rho: PAI+tmpLinePara1.Rho;
                changedAngle2 = (tmpLinePara2.Rho > 0)? tmpLinePara2.Rho: PAI+tmpLinePara2.Rho;
                double average = (changedAngle1 + changedAngle2)/2 - PAI/2;
                keyPoint.theta = average;

                findResultFlag = true;
            }
        }
    }

    return findResultFlag;

}

}


/**
* This file is part of Ewok.
*
* Copyright 2017 Vladyslav Usenko, Technical University of Munich.
* Developed by Vladyslav Usenko <vlad dot usenko at tum dot de>,
* for more information see <http://vision.in.tum.de/research/robotvision/replanning>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Ewok is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Ewok is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Ewok. If not, see <http://www.gnu.org/licenses/>.
*/

#include <thread>
#include <chrono>
#include <map>

#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_srvs/Empty.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

#include <visualization_msgs/MarkerArray.h>

#include <ewok/polynomial_3d_optimization.h>
#include <ewok/uniform_bspline_3d_optimization.h>

const int POW = 6;

double dt ;
int num_opt_points;

bool initialized = false;

std::ofstream f_time, opt_time;

ewok::PolynomialTrajectory3D<10>::Ptr traj;
ewok::EuclideanDistanceRingBuffer<POW>::Ptr edrb;
ewok::UniformBSpline3DOptimization<6>::Ptr spline_optimization;

ros::Publisher occ_marker_pub, free_marker_pub, dist_marker_pub, trajectory_pub, current_traj_pub;
tf::TransformListener * listener;

void depthImageCallback(const sensor_msgs::Image::ConstPtr& msg)
{
    //ROS_INFO("recieved depth image");

    cv_bridge::CvImageConstPtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvShare(msg);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    const float fx = 554.254691191187;
    const float fy = 554.254691191187;
    const float cx = 320.5;
    const float cy = 240.5;

    tf::StampedTransform transform;

    try{

        listener->lookupTransform("world", msg->header.frame_id,
                                  msg->header.stamp, transform);
    }
    catch (tf::TransformException &ex) {
        ROS_INFO("Couldn't get transform");
        ROS_WARN("%s",ex.what());
        return;
    }

    Eigen::Affine3d dT_w_c;
    tf::transformTFToEigen(transform, dT_w_c);

    Eigen::Affine3f T_w_c = dT_w_c.cast<float>();

    float * data = (float *) cv_ptr->image.data;

    auto t1 = std::chrono::high_resolution_clock::now();

    ewok::EuclideanDistanceRingBuffer<POW>::PointCloud cloud1;

    for(int u=0; u < cv_ptr->image.cols; u+=4) {
        for(int v=0; v < cv_ptr->image.rows; v+=4) {
            float val = data[v*cv_ptr->image.cols + u];

            //ROS_INFO_STREAM(val);

            if(std::isfinite(val)) {
                Eigen::Vector4f p;
                p[0] = val*(u - cx)/fx;
                p[1] = val*(v - cy)/fy;
                p[2] = val;
                p[3] = 1;

                p = T_w_c * p;

                cloud1.push_back(p);
            }
        }
    }

    Eigen::Vector3f origin = (T_w_c * Eigen::Vector4f(0,0,0,1)).head<3>();

    auto t2 = std::chrono::high_resolution_clock::now();

    if(!initialized) {
        Eigen::Vector3i idx;
        edrb->getIdx(origin, idx);

        ROS_INFO_STREAM("Origin: " << origin.transpose() << " idx " << idx.transpose());

        edrb->setOffset(idx);

        initialized = true;
    } else {
        Eigen::Vector3i origin_idx, offset, diff;
        edrb->getIdx(origin, origin_idx);

        offset = edrb->getVolumeCenter();
        diff = origin_idx - offset;

        while(diff.array().any()) {
            //ROS_INFO("Moving Volume");
            edrb->moveVolume(diff);

            offset = edrb->getVolumeCenter();
            diff = origin_idx - offset;
        }

    }

    //ROS_INFO_STREAM("cloud1 size: " << cloud1.size());

    auto t3 = std::chrono::high_resolution_clock::now();

    edrb->insertPointCloud(cloud1, origin);

    auto t4 = std::chrono::high_resolution_clock::now();

    f_time << std::chrono::duration_cast<std::chrono::nanoseconds>(t2-t1).count() << " " <<
              std::chrono::duration_cast<std::chrono::nanoseconds>(t3-t2).count() << " " <<
              std::chrono::duration_cast<std::chrono::nanoseconds>(t4-t3).count() << std::endl;

    visualization_msgs::Marker m_occ, m_free;
    edrb->getMarkerOccupied(m_occ);
    edrb->getMarkerFree(m_free);

    occ_marker_pub.publish(m_occ);
    free_marker_pub.publish(m_free);

}

void sendCommandCallback(const ros::TimerEvent& e) {

    auto t1 = std::chrono::high_resolution_clock::now();

    edrb->updateDistance();

    visualization_msgs::MarkerArray traj_marker;

    auto t2 = std::chrono::high_resolution_clock::now();
    spline_optimization->optimize();
    auto t3 = std::chrono::high_resolution_clock::now();

    opt_time << std::chrono::duration_cast<std::chrono::nanoseconds>(t2-t1).count() << " "
        << std::chrono::duration_cast<std::chrono::nanoseconds>(t3-t2).count() << std::endl;

    Eigen::Vector3d pc = spline_optimization->getFirstOptimizationPoint();

    geometry_msgs::Point pp;
    pp.x = pc[0];
    pp.y = pc[1];
    pp.z = pc[2];

    trajectory_pub.publish(pp);

    spline_optimization->getMarkers(traj_marker);
    current_traj_pub.publish(traj_marker);

    spline_optimization->addLastControlPoint();

    visualization_msgs::Marker m_dist;
    edrb->getMarkerDistance(m_dist, 0.5);
    dist_marker_pub.publish(m_dist);
}

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

    std::string path = ros::package::getPath("ewok_simulation") + "/benchmarking/";

    ROS_INFO_STREAM("path: " << path);

    f_time.open(path + "mapping_time.txt");
    opt_time.open(path + "optimization_time.txt");

    listener = new tf::TransformListener;

    occ_marker_pub = nh.advertise<visualization_msgs::Marker>("ring_buffer/occupied", 5);
    free_marker_pub = nh.advertise<visualization_msgs::Marker>("ring_buffer/free", 5);
    dist_marker_pub = nh.advertise<visualization_msgs::Marker>("ring_buffer/distance", 5);

    trajectory_pub =
            nh.advertise<geometry_msgs::Point>(
                    "command/point", 10);

    ros::Publisher traj_marker_pub = nh.advertise<visualization_msgs::MarkerArray>("global_trajectory", 1, true);

    current_traj_pub = nh.advertise<visualization_msgs::MarkerArray>("optimal_trajectory", 1, true);

    message_filters::Subscriber<sensor_msgs::Image> depth_image_sub_ ;
    depth_image_sub_.subscribe(nh, "camera/depth/image_raw", 5);

    tf::MessageFilter<sensor_msgs::Image> tf_filter_(depth_image_sub_, *listener, "world", 5);
    tf_filter_.registerCallback(depthImageCallback);

    double max_velocity, max_acceleration;
    pnh.param("max_velocity", max_velocity, 2.0);
    pnh.param("max_acceleration", max_acceleration, 5.0);

    Eigen::Vector4d limits(max_velocity, max_acceleration, 0, 0);

    double resolution;
    pnh.param("resolution", resolution, 0.15);
    edrb.reset(new ewok::EuclideanDistanceRingBuffer<POW>(resolution, 1.0));

    double distance_threshold;
    pnh.param("distance_threshold", distance_threshold, 0.5);

    double start_x, start_y, start_z, start_yaw;
    pnh.param("start_x", start_x, 0.0);
    pnh.param("start_y", start_y, 0.0);
    pnh.param("start_z", start_z, 0.0);
    pnh.param("start_yaw", start_yaw, 0.0);

    double middle_x, middle_y, middle_z, middle_yaw;
    pnh.param("middle_x", middle_x, 0.0);
    pnh.param("middle_y", middle_y, 0.0);
    pnh.param("middle_z", middle_z, 0.0);
    pnh.param("middle_yaw", middle_yaw, 0.0);

    double stop_x, stop_y, stop_z, stop_yaw;
    pnh.param("stop_x", stop_x, 0.0);
    pnh.param("stop_y", stop_y, 0.0);
    pnh.param("stop_z", stop_z, 0.0);
    pnh.param("stop_yaw", stop_yaw, 0.0);

    pnh.param("dt", dt, 0.5);
    pnh.param("num_opt_points", num_opt_points, 7);

    ROS_INFO("Started hovering example with parameters: start - %f %f %f %f, middle - %f %f %f %f, stop - %f %f %f %f",
             start_x, start_y, start_z, start_yaw,
             middle_x, middle_y, middle_z, middle_yaw,
             stop_x, stop_y, stop_z, stop_yaw);

    ROS_INFO("dt: %f, num_opt_points: %d", dt, num_opt_points);

    ewok::Polynomial3DOptimization<10> to(limits*0.6);

    {
        typename ewok::Polynomial3DOptimization<10>::Vector3Array path;

        path.push_back(Eigen::Vector3d(start_x, start_y, start_z));
        path.push_back(Eigen::Vector3d(middle_x, middle_y, middle_z));
        path.push_back(Eigen::Vector3d(stop_x, stop_y, stop_z));

        traj = to.computeTrajectory(path);

        visualization_msgs::MarkerArray traj_marker;
        traj->getVisualizationMarkerArray(traj_marker, "gt", Eigen::Vector3d(1,0,1));
        traj_marker_pub.publish(traj_marker);

    }

    spline_optimization.reset(new ewok::UniformBSpline3DOptimization<6>(traj, dt));

    for (int i = 0; i < num_opt_points; i++) {
        spline_optimization->addControlPoint(Eigen::Vector3d(start_x, start_y, start_z));
    }

    spline_optimization->setNumControlPointsOptimized(num_opt_points);
    spline_optimization->setDistanceBuffer(edrb);
    spline_optimization->setDistanceThreshold(distance_threshold);
    spline_optimization->setLimits(limits);

    std_srvs::Empty srv;
    bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
    unsigned int i = 0;

    // Trying to unpause Gazebo for 10 seconds.
    while (i <= 10 && !unpaused) {
        ROS_INFO("Wait for 1 second before trying to unpause Gazebo again.");
        std::this_thread::sleep_for(std::chrono::seconds(1));
        unpaused = ros::service::call("/gazebo/unpause_physics", srv);
        ++i;
    }

    if (!unpaused) {
        ROS_FATAL("Could not wake up Gazebo.");
        return -1;
    }
    else {
        ROS_INFO("Unpaused the Gazebo simulation.");
    }

    // Wait for 5 seconds to let the Gazebo GUI show up.
    ros::Duration(5.0).sleep();

    geometry_msgs::Point p;
    p.x = start_x;
    p.y = start_y;
    p.z = start_z;

    //Eigen::Vector3d desired_position(start_x, start_y, start_z);
    double desired_yaw = start_yaw;

    ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f, %f].",
             nh.getNamespace().c_str(),
             start_x,
             start_y,
             start_z, start_yaw);

    trajectory_pub.publish(p);

    ros::Duration(5.0).sleep();

    ros::Timer timer = nh.createTimer(ros::Duration(dt), sendCommandCallback);

    ros::spin();

    f_time.close();
    opt_time.close();
}


// File: matrix2.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

template <typename Matrix>
void fill_and_print(Matrix& A, char name)
{
    // Set values in traditional way
    A= 1.2, 3.4,
       5.6, 7.8;

    // Just print them
    std::cout << name << " is \n" << A << "\n";
}

int main(int, char**)
{
#if defined(MTL_WITH_VARIADIC_TEMPLATE) && defined(MTL_WITH_TEMPLATE_ALIAS)
    using namespace mtl;

    // Compressed matrix
    matrix<float, compressed>                 A(2, 2);
    fill_and_print(A, 'A');

    // Banded matrix
    matrix<float, sparse, banded>             B(2, 2);
    fill_and_print(B, 'B');

    // Matrix in the ELLPACK format
    matrix<double, ellpack>                   C(2, 2);
    fill_and_print(C, 'C');

    // Coordinate matrix
    matrix<float, coordinate>                 D(2, 2);
    fill_and_print(D, 'D');

    // Morton-order matrix with the default mask
    matrix<double, morton>                    E(2, 2);
    fill_and_print(E, 'E');

    // Matrix with a Morton mask is of course a Morton-order matrix
    matrix<double, mask<shark_z_64_row_mask>> F(2, 2);
    fill_and_print(F, 'F');
#endif
    return 0;
}


// Copyright Oleg Maximenko 2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/definitions.hpp>
#include <svgpp/detail/adapt_context.hpp>
#include <boost/optional.hpp>
#include <boost/noncopyable.hpp>
#include <boost/mpl/set.hpp>

namespace svgpp
{

typedef boost::mpl::set3<tag::attribute::cx, tag::attribute::cy, tag::attribute::r>
  circle_shape_attributes;

template<class Length>
class collect_circle_attributes_adapter: boost::noncopyable
{
public:
  template<class Context>
  bool on_exit_attributes(Context & context) const
  {
    typedef detail::unwrap_context<Context, tag::length_policy> length_policy_context;
    typedef typename length_policy_context::policy length_policy_t;

    typename length_policy_t::length_factory_type & converter
      = length_policy_t::length_factory(length_policy_context::get(context));

    typename length_policy_t::length_factory_type::coordinate_type
      cx = 0, cy = 0, r = 0;
    if (cx_)
      cx = converter.length_to_user_coordinate(*cx_, tag::length_dimension::width());
    if (cy_)
      cy = converter.length_to_user_coordinate(*cy_, tag::length_dimension::height());
    if (r_) // required attribute
    {
      r = converter.length_to_user_coordinate(*r_, tag::length_dimension::not_width_nor_height());
      if (r < 0)
      {
        typedef detail::unwrap_context<Context, tag::error_policy> error_policy;
        return error_policy::policy::negative_value(error_policy::get(context), tag::attribute::r());
      }

      if (r == 0)
        return true;
      typedef detail::unwrap_context<Context, tag::basic_shapes_events_policy> basic_shapes_events;
      basic_shapes_events::policy::set_circle(basic_shapes_events::get(context), cx, cy, r);
    }
    return true;
  }

  void set(tag::attribute::cx, Length const & val) { cx_ = val; }
  void set(tag::attribute::cy, Length const & val) { cy_ = val; }
  void set(tag::attribute::r, Length const & val)  { r_ = val; }

private:
  boost::optional<Length> cx_, cy_, r_;
};

struct circle_to_path_adapter
{
  template<class Context, class Coordinate>
  static void set_circle(Context & context, Coordinate cx, Coordinate cy, Coordinate r)
  {
    typedef detail::unwrap_context<Context, tag::path_events_policy> path_events;

    typename path_events::type & path_context = path_events::get(context);
    path_events::policy::path_move_to(path_context, cx + r, cy, tag::coordinate::absolute());
    path_events::policy::path_elliptical_arc_to(path_context,
      r, r, Coordinate(0), false, true, cx - r, cy, tag::coordinate::absolute());
    path_events::policy::path_elliptical_arc_to(path_context,
      r, r, Coordinate(0), false, true, cx + r, cy, tag::coordinate::absolute());
    path_events::policy::path_close_subpath(path_context);
    path_events::policy::path_exit(path_context);
  }
};

}


#include <iostream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/timer.hpp>

#include <vector>
#include <cmath>
using namespace std;

#include "NRGclasses.hpp"
//#include "NRGfunctions.hpp"

void RotateMatrix(CNRGmatrix* pMat, CNRGbasisarray* pAcut,
		  CNRGmatrix* pMatRot){

  pMatRot->ClearAll();

  //pMatRot->SyncNRGarray(*pAcut);

  pMatRot->MatBlockMap=pMat->MatBlockMap;
  pMatRot->UpperTriangular=pMat->UpperTriangular;
  pMatRot->IsComplex=pMat->IsComplex;

  // Same Block strucuture as pAcut (THIS IS CRUCIAL!)
  pMatRot->NQNumbers=pAcut->NQNumbers;
  pMatRot->QNumbers=pAcut->QNumbers;
  if (pAcut->BlockBegEndBC.size()!=0)
    pMatRot->BlockBegEnd=pAcut->BlockBegEndBC;
  else
    pMatRot->BlockBegEnd=pAcut->BlockBegEnd;

  // Need to Check Sync!!
  bool ChkSync=pMat->ChkSync(pAcut);

  if (!ChkSync){
    cout << "RotateMatrix: pMat and pAcut not in sync" << endl;
    return;
  }

  // Writing the blocks of pMat to the basis in pAcut

  int i1=0;
  for (int iMatbl=0; iMatbl<pMat->NumMatBlocks(); iMatbl++){

    int ibl1=0;
    int ibl2=0;

    pMatRot->MatBlockBegEnd.push_back(i1);

    pMat->GetBlocksFromMatBlock(iMatbl,ibl1,ibl2);

    int Nst_basis1=pAcut->GetBlockSizeBC(ibl1); // Should work:
    int Nst_basis2=pAcut->GetBlockSizeBC(ibl2); // ibl1, ibl2 in the current
                                                // block structure

//      cout << " Rotating block " << iMatbl << " of " << pMat->NumMatBlocks()-1
//  	 << " Final Size: " << Nst_basis1 << " x " << Nst_basis2 << endl;

    // Boost matrices
    boost::numeric::ublas::matrix<double> MatFinal(Nst_basis1,Nst_basis2);
    boost::numeric::ublas::matrix<complex<double> > cMatFinal(Nst_basis1,Nst_basis2);

    if (pMat->IsComplex){
      //cout << "pMat is complex " << endl;
       cMatFinal=pMat->cRotateBlock2BLAS(pAcut,ibl1,ibl2);
    }
    else
      MatFinal=pMat->RotateBlock2BLAS(pAcut,ibl1,ibl2);

    int j0=0;
    for (int ii=0;ii<Nst_basis1;ii++){
      if ( (pMatRot->UpperTriangular)&&(ibl1==ibl2) )
	j0=ii;
      else
	j0=0;
      for (int jj=j0;jj<Nst_basis2;jj++){
	if (pMatRot->IsComplex)
	  pMatRot->MatElCplx.push_back(cMatFinal(ii,jj));
	else
	  pMatRot->MatEl.push_back(MatFinal(ii,jj));
	i1++;
      }
    }
    // end loop in MatFinal
    pMatRot->MatBlockBegEnd.push_back(i1-1);
  }
  // end loop in MatBlocks
}
// end subroutine

// Rotate in the UNCUT eigenvector
void RotateMatrix_NoCut(CNRGmatrix* pMat, CNRGarray* pAeig,
		  CNRGmatrix* pMatRot, bool forward){

  pMatRot->ClearAll();

  //pMatRot->SyncNRGarray(*pAeig);

  pMatRot->MatBlockMap=pMat->MatBlockMap;
  pMatRot->UpperTriangular=pMat->UpperTriangular;
  pMatRot->IsComplex=pMat->IsComplex;

  // Same Block strucuture as pAeig (THIS IS CRUCIAL!)
  pMatRot->NQNumbers=pAeig->NQNumbers;
  pMatRot->QNumbers=pAeig->QNumbers;
  pMatRot->BlockBegEnd=pAeig->BlockBegEnd;

  // Need to Check Sync!!
  bool ChkSync=pMat->ChkSync(pAeig);

  if (!ChkSync){
    cout << "RotateMatrix: pMat and pAeig not in sync" << endl;
    return;
  }

  // Writing the blocks of pMat to the basis in pAeig

//   CNRGbasisarray Acut;
//   Acut.FalseCut(pAeig);
//   Acut.PrintAll();

  int i1=0;
  for (int iMatbl=0; iMatbl<pMat->NumMatBlocks(); iMatbl++){

    int ibl1=0;
    int ibl2=0;

    pMatRot->MatBlockBegEnd.push_back(i1);

    pMat->GetBlocksFromMatBlock(iMatbl,ibl1,ibl2);

    int Nst_basis1=pAeig->GetBlockSize(ibl1); // Should work:
    int Nst_basis2=pAeig->GetBlockSize(ibl2); // ibl1, ibl2 in the current
                                                // block structure

//      cout << " Rotating block " << iMatbl << " of " << pMat->NumMatBlocks()-1
//  	 << " Final Size: " << Nst_basis1 << " x " << Nst_basis2 << endl;

    // Boost matrices
    boost::numeric::ublas::matrix<double> MatFinal(Nst_basis1,Nst_basis2);
    boost::numeric::ublas::matrix<complex<double> > cMatFinal(Nst_basis1,Nst_basis2);

    //MatFinal=pMat->RotateBlock2BLAS(&Acut,ibl1,ibl2);

    if (pMat->IsComplex){
      cMatFinal=pMat->cRotateBlock2BLAS_NoCut(pAeig,ibl1,ibl2,forward);
    }
    else
      MatFinal=pMat->RotateBlock2BLAS_NoCut(pAeig,ibl1,ibl2,forward);

//     if ( (ibl1==3)&&(ibl2==6) ){
    //cout << " RotMat(ibl1="<<ibl1<<",ibl2="<<ibl2<<")="<< MatFinal << endl;
    //cout << " RotMat(ibl1="<<ibl1<<",ibl2="<<ibl2<<")="<< cMatFinal << endl;
//     }

    int j0=0;
    for (int ii=0;ii<Nst_basis1;ii++){
      if ( (pMatRot->UpperTriangular)&&(ibl1==ibl2) )
	j0=ii;
      else
	j0=0;
      for (int jj=j0;jj<Nst_basis2;jj++){
	if (pMatRot->IsComplex){
	  pMatRot->MatElCplx.push_back(cMatFinal(ii,jj));
	}
	else{
	  pMatRot->MatEl.push_back(MatFinal(ii,jj));
	}
	i1++;
      }
    }
    // end loop in MatFinal

    pMatRot->MatBlockBegEnd.push_back(i1-1);

  }
  // end loop in MatBlocks

}
// end subroutine

///////////////////////////////////////////////////////
///// Moving all RotateBlock2BLAS to here  ////////////
///////////////////////////////////////////////////////