// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is 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.
// __END_LICENSE__

#ifdef _MSC_VER
#pragma warning(disable:4244)
#pragma warning(disable:4267)
#pragma warning(disable:4996)
#endif

#include <iostream>
#include <fstream>
#include <string>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <vw/Math/Matrix.h>
#include <vw/Cartography/GeoReference.h>
#include <vw/Image/ImageView.h>
#include <vw/Image/MaskViews.h>
#include <vw/Image/Statistics.h>
#include <vw/InterestPoint.h>
#include <vw/FileIO/DiskImageResource.h>
#include <vw/FileIO/DiskImageView.h>

// Not sure which of these we need but we get run-time errors otherwise...
#include <vw/InterestPoint.h>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <vw/Stereo/PreFilter.h>
#include <vw/Stereo/CorrelationView.h>
#include <vw/Stereo/CostFunctions.h>
#include <vw/Stereo/DisparityMap.h>
#include <asp/Core/DemDisparity.h>
#include <asp/Core/LocalHomography.h>

//#include <asp/Tools/stereo.h>
#include <stereo.h> // Using local copy

/// \file pixelPairsFromStereo.cc Generates a list of pixel point pairs from the output of the stereo tool

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

  std::string inputImagePath, outputPath="";
  int pointSpacing=0;
  std::vector<float> leftAffineVals, rightAffineVals;

  // Affine transforms are identity matrix by default
  vw::Matrix<double> leftAffine, rightAffine;
  leftAffine  = vw::math::identity_matrix<3>();
  rightAffine = vw::math::identity_matrix<3>();
  // Note that the user inputs the matrices that were used in stereo,
  //  we need to compute the inverse of those matrices.

  po::options_description general_options("Options");
  general_options.add_options()
    ("help",        "Display this help message")
    ("pointSpacing", po::value<int        >(&pointSpacing)->default_value(100), "Selected pixels are this far apart")
    ("leftAffine",    po::value<std::vector<float> >(&leftAffineVals)->multitoken(),
                           "Affine transform which was applied to the left image pixels)")
    ("rightAffine",   po::value<std::vector<float> >(&rightAffineVals)->multitoken(),
                           "Affine transform which was applied to the right image pixels)");

  po::options_description positional("");
  positional.add_options()
    ("input-file",   po::value(&inputImagePath), "The input stereo file (xxx-D.tif)")
    ("output-file",  po::value(&outputPath),     "Specify an output text file to store the program output");

  po::positional_options_description positional_desc;
  positional_desc.add("input-file",  1);
  positional_desc.add("output-file", 1);

  std::string usage("[options] <input path> <output path>\n");
  po::variables_map vm;
  try {
    po::options_description all_options;
    all_options.add(general_options).add(positional);

    po::store( po::command_line_parser( argc, argv ).options(all_options).positional(positional_desc).style( po::command_line_style::unix_style ^ po::command_line_style::allow_short ).run(), vm );

    po::notify( vm );
  } catch (po::error const& e) {
    vw::vw_throw( vw::ArgumentErr() << "Error parsing input:\n"
                  << e.what() << "\n" << usage << general_options );
  }

  if ( !vm.count("input-file") || !vm.count("output-file") )
    vw_throw( vw::ArgumentErr() << "Requires <input path> and <output path> input in order to proceed.\n\n"
              << usage << general_options );

  const size_t NUM_AFFINE_ELEMENTS = 6; // The last three elements are 0 0 1
  if (vm.count("leftAffine"))
  {
    // Check input size
    if (leftAffineVals.size() != NUM_AFFINE_ELEMENTS)
      vw::vw_throw( vw::ArgumentErr() << "Incorrect number of left affine arguments passed in!\n"
                                      << usage << general_options );
    // Store affine values in a temp matrix object which is then inverted
    vw::Matrix<double> affineTemp = vw::math::identity_matrix<3>();
    for (int i=0; i<NUM_AFFINE_ELEMENTS; ++i)
      affineTemp.data()[i] = leftAffineVals[i];
    vw::vw_out() << "Read in left affine matrix:\n" << affineTemp << "\n";
    leftAffine = inverse(affineTemp);
  }
  if (vm.count("rightAffine"))
  {
    // Check input size
    if (rightAffineVals.size() != NUM_AFFINE_ELEMENTS)
      vw::vw_throw( vw::ArgumentErr() << "Incorrect number of right affine arguments passed in!\n"
                                      << usage << general_options );
    // Store affine values in a temp matrix object which is then inverted
    vw::Matrix<double> affineTemp = vw::math::identity_matrix<3>();
    for (int i=0; i<NUM_AFFINE_ELEMENTS; ++i)
      affineTemp.data()[i] = rightAffineVals[i];
    vw::vw_out() << "Read in right affine matrix:\n" << affineTemp << "\n";
    rightAffine = inverse(affineTemp);
  }

  try {

    std::ofstream outputFile(outputPath.c_str());
    if (outputFile.fail())
    {
      printf("Failed to open output file for writing\n");
      return 0;
    }

    printf("Loading image\n");

    vw::ImageViewRef<vw::PixelMask<vw::Vector2i> > inputImage = vw::DiskImageView<vw::PixelMask<vw::Vector2i> >(inputImagePath);

    printf("Done loading image\n");

    //if (!inputImage.is_valid_image())
    //{
    //  printf("Failed to load image!\n");
    //  return 0;
    //}
    printf("Image size: %d x %d\n", inputImage.rows(), inputImage.cols());

    vw::ImageViewRef<vw::PixelMask<vw::Vector2i> >::iterator iter = inputImage.begin();

    // First pass computes min, max, mean, and std_dev
    int count = 0;
    for (int row=0; row<inputImage.rows(); row+=1)
    {

      for (int col=0; col<inputImage.cols(); col+=1)
      {
        if ((row % pointSpacing == 0) && (col % pointSpacing == 0))
        {
          //printf("%d, %d\n", row, col);
          if (vw::is_valid(*iter)) // Skip invalid pixels
          {
            //printf("%d, %d\n", row, col);
            //std::cout << (*iter)[0] << std::endl;

            // Pixel in other image = original pixel location + stored offset
            int rightCol = floor(0.5 + col + (*iter)[0]);
            int rightRow = floor(0.5 + row + (*iter)[1]);

            // Apply the affine transforms to the left and right pixels
            vw::Vector3 leftCoord (col,      row, 1);
            vw::Vector3 rightCoord(rightCol, rightRow, 1);
            vw::Vector3 leftTransformed  = leftAffine * leftCoord;
            vw::Vector3 rightTransformed = rightAffine * rightCoord;

            // Write to file and record total
            outputFile << leftTransformed [0] <<","<< leftTransformed [1] <<","
                       << rightTransformed[0] <<","<< rightTransformed[1] << std::endl;
            ++count;
          } // End valid check
        } // End spacing check
        ++iter;
      } // End loop through cols

    } // End loop through rows

    // Done writing the output file
    outputFile.close();

    printf("Wrote %d pixel pairs\n", count);

  }
  catch (const vw::Exception& e) {
    std::cerr << "Error: " << e.what() << std::endl;
  }

  return 0;
}


#include <ignition/math/Pose3.hh>
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include "gazebo/gazebo.hh"
#include <boost/bind.hpp>
#include <iostream>
#include <string>
#include <fstream>
#include <mutex>
#include <map>
#include <vector>
#include <set>

#include <sstream>
#include <iomanip>
#include <ctime>

using namespace std;
namespace gazebo
{
	class WP_Swarm1 : public WorldPlugin
	{
		//Pointer to the update event connection
		private:
			event::ConnectionPtr updateConnection;
			physics::WorldPtr world;
			std::mutex mutex;
			transport::NodePtr node;
			transport::PublisherPtr pub_litter_pose;
			std::string world_info_string;//should state when simulation starts and ends per world
			bool world_info_bool;//used to indicate the world is loaded and ready to go
			transport::PublisherPtr pub_world_loaded;

			bool world_gov_experiment_control;// used by world governor to control start of simulation runs
			transport::SubscriberPtr sub_world_gov_experiment_control;

			double nei_sensing;
			bool param_set;
			gazebo::transport::SubscriberPtr sub_params;
			//This section of parameters are used to handle communication between robots
			std::map<std::string,transport::PublisherPtr> pub_commSignal;
			//std::map<std::string,transport::PublisherPtr> pub_attraction;

			std::map<std::string,int> seen_litter_map;
			gazebo::transport::SubscriberPtr sub_seen_litter;//Models publish seen litter to this topic

			std::map<std::string,std::string> robot_status_map;
			gazebo::transport::SubscriberPtr sub_robot_status;//Models publish their status to this sub

			gazebo::transport::PublisherPtr pub_repel_signal;
			gazebo::transport::PublisherPtr pub_attract_signal;
			gazebo::physics::Model_V robot_ptr;//consider changing it to set
			gazebo::physics::Model_V litter_ptr;
			double lit_threshold;

			//*********************START: TO HANDLE PARAMTER UPDATING AND SIMULATION RESETS
			bool start_sim;
			bool start_sim2;
			bool no_litter;
			gazebo::transport::PublisherPtr pub_start_sim;
			gazebo::transport::PublisherPtr pub_my_Init;
			gazebo::transport::PublisherPtr pub_model_log_control;
			transport::PublisherPtr physicsPub;
			bool set_max_step_size;
			double exp_dur; //maximum duration of experiment before activating  homing behaviour
			std::string params_str;

			//*******nest model pointer *********//
			gazebo::physics::ModelPtr m_nest;

			int litter_tot;//total litter in world
			int litter_in_nest;//total litter deposited at the nest
			gazebo::transport::SubscriberPtr sub_litter_in_nest;//subscriber to litter in nest topic
			double nest_distance_travelled;
			double max_nest_travel;
			//---------------end nest model pointer--------//

			//All params to be simulated are stored in a vector
			vector<string> my_params;
			std::vector<string>::iterator param_it;

			double world_size;
			//double world_sizeY;
			//double psec;
			//*********************END: TO HANDLE PARAMTER UPDATING AND SIMULATION RESETS

			//START: EXPERIMENT START STOP CONTROL
			gazebo::transport::PublisherPtr pub_experiment_control;
			bool end_experiment;
			//END: EXPERMEINT START STOP CONTROL

			double log_timer;
			double max_step_size;
			double log_rate;

			std::string com_model;
			int cap_com;
			// Sound source modelling parameters
			math::Vector3 signal_source_pos;
			double A0; //intensity from source
			double alpha; //attenuation factor
			double Ae; //ambient noise scale
			double noise_mean;//mean of the noise
			double noise_std;//standard deviation of the noise
			std::normal_distribution<double> noise_distro;//distribution for making sound noisy
			std::default_random_engine generator;
		public:
			void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/)
			{
				// std::cout<<"world loading"<<std::endl;
				this->node = transport::NodePtr(new transport::Node());
				this->node->Init();
				this->world = _parent;
				this->world_info_bool = true;//default value is true when world is loaded
				this->pub_world_loaded = this->node->Advertise<msgs::Any>("/world_loaded");

				this->world_gov_experiment_control = false;//default value is false. Set to true from subscribed topic
				this->sub_world_gov_experiment_control = this->node->Subscribe("/world_gov_experiment_control",&WP_Swarm1::world_gov_experiment_control_cb,this);

				this->physicsPub = node->Advertise<msgs::Physics>("~/physics");
				msgs::Physics physicsMsg;
				physicsMsg.set_type(msgs::Physics::ODE);
				physicsMsg.set_max_step_size(0.025);//manually increase the step size
				this->physicsPub->Publish(physicsMsg);

				this->set_max_step_size = true;
				//this->psec = 0;
				//exit(5);
				//load simulation paramters
				// ifstream myfile;
				// string line, header;
				// myfile.open("params/params.csv");
				// if(myfile.is_open())
				// {
				// 	getline(myfile,header);
				// 	while(getline(myfile,line))
				// 	{
				// 		size_t hsep1=0,hsep2=0,psep1=0,psep2=0;
				// 		string hparam,param_value;
				// 		string param_line = "";
				// 		while(header.find(",",hsep1) != string::npos and
				// 				line.find(",",psep1) != string::npos)
				// 		{
				// 			hsep2 = header.find(",",hsep1);
				// 			hparam = header.substr(hsep1,hsep2-hsep1);
				// 			psep2 = line.find(",",psep1);
				// 			param_value = line.substr(psep1,psep2-psep1);

				// 			param_line = param_line + hparam + ":" + param_value + ",";
				// 			hsep1 = hsep2 + 1;
				// 			psep1 = psep2 + 1;
				// 			if(hparam.compare("max_step_size") == 0)
				// 			{
				// 				transport::PublisherPtr physicsPub =
				// 				node->Advertise<msgs::Physics>("~/physics");
				// 				msgs::Physics physicsMsg;
				// 				physicsMsg.set_type(msgs::Physics::ODE);

				// 				double value = std::stod(param_value);
				// 				//this->max_step_size = value;
				// 				//value = 1.0/value;
				// 				physicsMsg.set_max_step_size(value);
				// 				physicsPub->Publish(physicsMsg);
				// 			}
				// 		}
				// 		this->my_params.push_back(param_line);
				// 	}
				// 	//iterator set to first parameter list
				// 	this->param_it = this->my_params.begin();
				// 	myfile.close();
				// }
				// else
				// {
				// 	//cout<<"unable to load file"<<endl;
				// 	//exit(5);
				// }

				if(this->world->GetModel("boundary1"))
				{
					math::Box boundary = this->world->GetModel("boundary1")->GetBoundingBox();
					this->world_size = boundary.GetXLength();
				}
				else
				{
					this->world_size = 9999999;
				}
				//std::cout<<boundary.GetXLength()<<":"<<boundary.GetYLength()<<":"<<boundary.GetZLength()<<std::endl;

				this->sub_params = this->node->Subscribe("/params_topic",&WP_Swarm1::Params_cb,this);
				this->sub_seen_litter = this->node->Subscribe("/topic_seen_litter",&WP_Swarm1::seen_litter_cb,this);
				this->sub_robot_status = this->node->Subscribe("/topic_robot_status",&WP_Swarm1::robot_status_cb,this);
				this->pub_litter_pose = this->node->Advertise<msgs::Any>("/topic_litter_pose");
				std::string all_litter_pos = "";
				//////////////////////////////////////////////////////////////
				this->m_nest = this->world->GetModel("m_nest");

				gazebo::physics::Model_V all_models = this->world->GetModels();
				this->sub_litter_in_nest = this->node->Subscribe("/litter_in_nest",&WP_Swarm1::litter_in_nest_cb,this);
				this->litter_in_nest = 0;

				this->litter_tot = 0;
				for(auto m : all_models)
				{

					std::string model_name = m->GetName();
					if(model_name.find("m_4wrobot") != std::string::npos)
					{
						this->robot_ptr.push_back(m);
						this->pub_commSignal[model_name] = this->node->Advertise<msgs::Any>("/"+model_name+"/comm_signal");
						//this->pub_attraction[model_name] = this->node->Advertise<msgs::Any>("/"+model_name+"/attract_signal");
						this->seen_litter_map[model_name] = 0;
						this->robot_status_map[model_name] = "searching";
					}

					if(model_name.find("litter") != std::string::npos)
					{
						this->litter_tot += 1;
						gazebo::math::Vector3 lit_loc = m->GetWorldPose().pos;
						this->litter_ptr.push_back(m);
						all_litter_pos = all_litter_pos + to_string(lit_loc.x) + ","
														+ to_string(lit_loc.y) + ":";
					}
				}
				msgs::Any all_litter_pos_msg;
				all_litter_pos_msg.set_type(msgs::Any::STRING);
				all_litter_pos_msg.set_string_value(all_litter_pos);
				this->pub_litter_pose->Publish(all_litter_pos_msg);//publish initial pos of all litter

				this->pub_repel_signal = this->node->Advertise<msgs::Any>("/repulsion_signal");
				this->pub_attract_signal = this->node->Advertise<msgs::Any>("/attraction_signal");

				this->pub_start_sim = this->node->Advertise<msgs::Any>("/start_sim");//control start and stop for simulations
				this->pub_my_Init = this->node->Advertise<msgs::Any>("/my_Init");//publish simulation parameters
				this->start_sim = false;
				this->start_sim2 = false;
				this->pub_model_log_control = this->node->Advertise<msgs::Any>("/topic_model_log");

				this->pub_experiment_control = this->node->Advertise<msgs::Any>("/experiment_control");
				this->end_experiment = false;
				//my_Init();
				this->param_set = false;//Initially params_str is null. so we know it has not been set.

				//set to invalid value to prevent premature end to simulation
				this->nest_distance_travelled = -900;
				this->exp_dur = 1800; //default experimemnt duration is 1800 except changed from params file
				//Handling communication among robots section
				/*for (int i = 1; i <= 10;i++)
				{
					std::string name = "m_4wrobot" + to_string(i);
					this->pub_repulsion[name] = this->node->Advertise<msgs::Any>("/"+name+"/comm_signal");

				}
				*/
				//////////////////////////////////////////////////////////
				this->updateConnection = event::Events::ConnectWorldUpdateBegin(
							boost::bind(&WP_Swarm1::OnUpdate,this,_1));
				// std::cout<<"world loaded"<<std::endl;
			}
			void litter_in_nest_cb(ConstAnyPtr &any)
			{
				std::lock_guard<std::mutex> lock(this->mutex);
				std::string nest_msg_str = any->string_value();
				if(nest_msg_str.find("t,") == std::string::npos){
					std::istringstream stream(nest_msg_str);

					std::string t;
					std::getline(stream,t,',');

					std::string litter_in_nest_str;
					std::getline(stream,litter_in_nest_str,',');

					std::string x,y,theta,nest_dst_travelled;
					std::getline(stream,x,',');
					std::getline(stream,y,',');
					std::getline(stream,theta,',');
					std::getline(stream,nest_dst_travelled,',');

					//distance travelled by nest
					this->nest_distance_travelled = std::stod(nest_dst_travelled);

					int l = std::stoi(litter_in_nest_str);
					this->litter_in_nest = l;
				}
			}
			void seen_litter_cb(ConstAnyPtr &any)
			{
				std::lock_guard<std::mutex> lock(this->mutex);
				std::string seen_litter_msg = any->string_value();
				size_t n_loc = seen_litter_msg.find(":");
				std::string model_name = seen_litter_msg.substr(0,n_loc);
				int seen_litter_num = std::stoi(seen_litter_msg.substr(n_loc+1));
				//update appropriate model with it's seen litter
				this->seen_litter_map[model_name] = seen_litter_num;
			}

			void robot_status_cb(ConstAnyPtr &any)
			{
				std::lock_guard<std::mutex> lock(this->mutex);

				std::string robot_status_msg = any->string_value();
				size_t n_loc = robot_status_msg.find(":");
				std::string model_name = robot_status_msg.substr(0,n_loc);
				std::string robot_status = robot_status_msg.substr(n_loc+1);

				this->robot_status_map[model_name] = robot_status;

				//std::cout<<model_name<<" "<<robot_status<<std::endl;
			}

			void my_Init()
			{
				// std::cout<<"Init called"<<std::endl;
				msgs::Any exp_control;
				exp_control.set_type(msgs::Any::STRING);
				std::string sim_params = "param:value pairs";

				if(this->params_str.compare("end_simulation") != 0)
				{
					// sim_params = *(this->param_it);
					sim_params = this->params_str;

					int area = (int)(this->world_size)*(this->world_size);
					std::string x = sim_params;
					x = x +
						"no_of_litter:" + to_string(this->litter_ptr.size())
						+ ",no_of_robots:" + to_string(this->robot_ptr.size())
						+ ",world_size:" +
						 to_string(area) + "sqm";
					exp_control.set_string_value(x);
					this->pub_experiment_control->Publish(exp_control);
				}
				if(this->params_str.compare("end_simulation") == 0)
				{
					// exp_control.set_string_value("end");
					// this->pub_experiment_control->Publish(exp_control);
					this->end_experiment = true;
					return;
				}
				//cout<<"called"<<endl;

				//stop all models by sending start_sim = false
				msgs::Any s_sim;
				s_sim.set_type(msgs::Any::BOOLEAN);
				this->start_sim = false;//to be double sure that simulation does not start
				s_sim.set_bool_value(this->start_sim);
				this->pub_start_sim->Publish(s_sim);

				//reset world
				this->world->Reset();

				//Extract all parameters for this simulation
				/*if(this->param_it == this->my_params.end())
				{
					std::cout<<"Simulations Ended"<<endl;
					exit(0);
				}*/

				//insert seed for each robot type.
				srand(std::time(nullptr));
				for (auto m:this->robot_ptr)
				{
					std::string robot_name = m->GetName();
					sim_params = sim_params + robot_name + ":" + to_string(rand()) + ",";
				}
				std::string sim_params_mine = sim_params;

				//set parameters of world plugin++++
				while(sim_params_mine.find(",") != std::string::npos)
				{//loop through all parameters and assign to appropriate variable in plugin
					size_t ploc = sim_params_mine.find(",");
					std::string temp = sim_params_mine.substr(0,ploc);

					size_t aloc = temp.find(":");
					std::string param_name = temp.substr(0,aloc);
					std::string param_value_str = temp.substr(aloc+1);
					// double value  = std::stod(param_value_str);//double value of parameter

					if(param_name.compare("nei_sensing") == 0)
					{
						this->nei_sensing = std::stod(param_value_str);//double value of parameter;
						//std::cout<<"nei_sensing = "<<this->nei_sensing<<endl;
						//break;
					}
					else if(param_name.compare("log_rate") == 0)
					{
						this->log_rate = std::stod(param_value_str);//double value of parameter;
					}
					else if(param_name.compare("max_step_size") == 0)
					{
						this->max_step_size = std::stod(param_value_str);//double value of parameter;
					}
					else if(param_name.compare("lit_threshold") == 0)
					{
						this->lit_threshold = std::stod(param_value_str);//double value of parameter;
					}

					else if(param_name.compare("A0") == 0)
					{
						this->A0 = std::stod(param_value_str);//double value of parameter;
					}
					else if (param_name.compare("Ae") == 0)
					{
						this->Ae = std::stod(param_value_str);//double value of parameter;
					}
					else if(param_name.compare("alpha") == 0)
					{
						this->alpha = std::stod(param_value_str);//double value of parameter;
					}
					else if(param_name.compare("noise_mean") == 0)
					{
						this->noise_mean = std::stod(param_value_str);//double value of parameter;
					}
					else if(param_name.compare("noise_std") == 0)
					{
						this->noise_std = std::stod(param_value_str);//double value of parameter;
					}
					else if(param_name.compare("com_model") == 0)
					{
						this->com_model = param_value_str;//communication model is a string
					}
					else if(param_name.compare("cap_com") == 0)
					{
						this->cap_com = std::stoi(param_value_str);//double value of parameter;
					}
					else if(param_name.compare("max_step_size") == 0 and this->set_max_step_size)
					{
						msgs::Physics physicsMsg;
						physicsMsg.set_type(msgs::Physics::ODE);

						double value = std::stod(param_value_str);
						//this->max_step_size = value;
						//value = 1.0/value;
						physicsMsg.set_max_step_size(value);
						this->physicsPub->Publish(physicsMsg);
						this->set_max_step_size = false;
					}
					else if(param_name.compare("exp_dur") == 0)
					{//initialize foraging length
						this->exp_dur = std::stod(param_value_str);
					}
					else if(param_name.compare("max_nest_travel") == 0)
					{
						this->max_nest_travel = std::stod(param_value_str);
					}
					else
					{
						//cout<<temp<<endl;
					}
					//cout<<temp<<endl;
					sim_params_mine = sim_params_mine.substr(ploc+1);

				}
				//cout<<"params set"<<endl;
				//this->nei_sensing = 5.0;

				//publish simulation parameters for this set of simulation.
				msgs::Any sim_params_msg;
				sim_params_msg.set_type(msgs::Any::STRING);
				sim_params_msg.set_string_value(sim_params);
				this->pub_my_Init->Publish(sim_params_msg);

				//noisy data based on normal distro
				this->generator = std::default_random_engine(rand());
				this->noise_distro = std::normal_distribution<double>(this->noise_mean,this->noise_std);

				// (this->param_it)++;//increment iterator to next parameter set;

				//this->param_set = false;
				this->world_info_string = "";

				this->start_sim2 = true;
				this->no_litter = false;//assume there is litter;

				this->log_timer = 0;//reset log timer.
				// std::cout<<"Init exited"<<std::endl;
				this->nest_distance_travelled = 0;

			}

			void Params_cb(ConstAnyPtr &a)
			{//set all parameters and set param_set = true when done.
				std::lock_guard<std::mutex> lock(this->mutex);
				this->params_str = a->string_value();
				// std::cout<<this->params_str<<std::endl;
				this->param_set = true;
			}

			void world_gov_experiment_control_cb(ConstAnyPtr &a){
				//controls when to start a simulation.
				std::lock_guard<std::mutex> lock(this->mutex);

				this->world_gov_experiment_control = a->bool_value();

				//world governor has been informed that the world is ready to go. Set the world_info_bool to false;
				this->world_info_bool = false;
			}
			void OnUpdate(const common::UpdateInfo &_info)
			{
				std::lock_guard<std::mutex> lock(this->mutex);
				// std::cout<<_info.simTime<<std::endl;
				if(this->param_set)
				{//check if parameters have been initialized.

					this->my_Init();
					this->param_set = false;
				}
				if(this->end_experiment)
				{//Publish this->begin_experiment
					//std::cout<<"Simulations Ended"<<endl;
					exit(0);
				}
				// if(_info.simTime.sec >= this->exp_dur)
				// { //set number of litter to picked litter, so that robots will activate homing state
				// 	this->no_litter = true;
				// 	this->litter_tot = this->litter_in_nest;
				// }
				if(this->world_info_bool){
					//At start of simulation, this informs the world governor that the world has been loaded and ready to go
					msgs::Any any;
					//any.set_type(msgs::Any::STRING);
					//any.set_string_value("end");
					any.set_type(msgs::Any::BOOLEAN);
					any.set_bool_value(this->world_info_bool);
					this->pub_world_loaded->Publish(any);
					// this->world_info_bool = false;//to be set to false from world_gov_experiment_control subscriber
				}
				if(!this->world_gov_experiment_control){
					//needed to prevent simulation from counting while world-gov-control is set to false
					this->world->Reset();
					// this->end_experiment = this->world_gov_experiment_control;
				}
				if(/*this->litter_in_nest >= this->litter_tot  */
					this->nest_distance_travelled >= this->max_nest_travel
					or _info.simTime.sec >= this->exp_dur)// or st.sec >= 30)//(true and this->no_litter) or  false and (st.sec >= 300 and st.nsec==0))
				{
					// this->litter_in_nest = 0;
					// // this->param_set =  true;
					// msgs::Any s_sim;
					// s_sim.set_type(msgs::Any::BOOLEAN);
					// this->start_sim = false;//to be double sure that simulation does not start
					// s_sim.set_bool_value(this->start_sim);
					// this->pub_start_sim->Publish(s_sim);
					// this->world_gov_experiment_control = false;//set to false since a simulation iteration is ended

					//All litter collected. Alert Governor that you have ended current simulation
					msgs::Any exp_control;
					exp_control.set_type(msgs::Any::STRING);
					std::string end_sim_str = "end"/* + to_string(this->nest_distance_travelled) +
												"," + to_string(this->max_nest_travel)*/;
					exp_control.set_string_value(end_sim_str);
					this->pub_experiment_control->Publish(exp_control);
					// std::cout<<"End Published"<<std::endl;
					// return;
					// this->world->Reset();
					// exit(0);
				}

				if(this->world_gov_experiment_control and !(this->param_set)) {
						//world governor controls when simulation starts by setting the value of world_gov_experiment_control to true
						//Also, if simulation parameters have been set
					//std::cout<<_info.simTime.Double()<<std::endl;
					if(this->start_sim2)
					{
						msgs::Any s_sim;
						s_sim.set_type(msgs::Any::BOOLEAN);
						this->start_sim = true;
						s_sim.set_bool_value(this->start_sim);
						this->pub_start_sim->Publish(s_sim);

						this->start_sim2 = false;
						this->log_timer = 0;
						// msgs::Any set_prefix;
						// set_prefix.set_type(msgs::Any::STRING);
						// set_prefix.set_string_value("new_file");
						// this->pub_model_log_control->Publish(set_prefix);
					}
					this->log_timer += this->max_step_size;

					// if(this->param_set)
					// {

					// 	this->my_Init();
					// 	this->param_set = false;
					// }
					//std::cout<<this->world->GetModelCount()<<endl;
					gazebo::common::Time st = _info.simTime;
				/*
					if(st.nsec==0)
					{
						ofstream myfile("robot_name.txt",std::ios::app|std::ios::ate);
						myfile << st.nsec<<std::endl;
						myfile.close();
					}*/

					/*/Start::Testing Seen litter messages
					std::string s="";
					for(auto it = this->seen_litter_map.begin(); it != this->seen_litter_map.end();it++)
					{
						s = s + it->first + ":" + to_string(it->second) + ",";
					}
					std::cout<<s<<std::endl;
					/End :: Test seen litter messages*/
					if(/*this->litter_in_nest >= this->litter_tot or */
						this->nest_distance_travelled >= this->max_nest_travel
						or st.sec >= this->exp_dur)//(true and this->no_litter) or  false and (st.sec >= 300 and st.nsec==0))
					{
						this->litter_in_nest = 0;
						// this->param_set = true;
						msgs::Any s_sim;
						s_sim.set_type(msgs::Any::BOOLEAN);
						this->start_sim = false;//to be double sure that simulation does not start
						s_sim.set_bool_value(this->start_sim);
						this->pub_start_sim->Publish(s_sim);
						this->world_gov_experiment_control = false;//set to false since a simulation iteration is ended

						//All litter collected. Alert Governor that you have ended current simulation
						// msgs::Any exp_control;
						// exp_control.set_string_value("end");
						// this->pub_experiment_control->Publish(exp_control);
						// this->world->Reset();

					}
					else
					{
						// for(auto m : this->robot_ptr)
						// {//Get current robot.
							// std::string r_name = m->GetName();
							math::Vector3 nest_pos = this->m_nest->GetWorldPose().pos;
							// int rep_neighbours = 0;
							// std::string rep_data = "";
							std::string att_data = "";

							double rslt_x=0.0,rslt_y=0.0;

							for(auto n : this->robot_ptr)
							{//Loop through neighbours
								std::string n_name = n->GetName();
								math::Vector3 n_pos = n->GetWorldPose().pos;
								int att_neighbours = 0;//initialize attraction neighbours to 0
								// double rep_signal = 0;
								double att_signal = 0; //initialize attraction signal to 0

								// std::string n_status = this->robot_status_map[n_name];
								// int n_seen_litter = this->seen_litter_map[n_name];

								//std::cout<<n_name<<" "<<n_status<<std::endl;

								// if(n_name.compare(r_name) !=0)
								// {//If not the current robot do this section
									nest_pos.z = 0;
									n_pos.z = 0;
									// if(abs(r_pos.x - n_pos.x) < this->nei_sensing and
									// 		abs(r_pos.y - n_pos.y) < this->nei_sensing)
									// {
										double dist = nest_pos.Distance(n_pos);
										// if(dist <= this->nei_sensing)//neighbour sensing distance... Consider setting parameter at startup
										// {
											// if(n_status.compare("searching")==0 || false)
											// {
											// 	rep_neighbours += 1;
											// 	double repulsion_intensity = 0;
											// 	if(this->com_model.compare("linear") == 0){
											// 		repulsion_intensity = (this->nei_sensing - dist)/(this->nei_sensing);
											// 	}
											// 	else if(this->com_model.compare("sound") == 0) {
											// 		repulsion_intensity = this->A0 * exp(-dist*(this->alpha)) + this->Ae;
											// 		//add noise
											// 		repulsion_intensity += this->noise_distro(this->generator);

											// 	}
											// 	else if(this->com_model.compare("soundv2") == 0){
											// 		//noise_mean is average of fitError/signalStrength and noise_std is the deviation across experiments
											// 		std::normal_distribution<double> noise_distro_std(this->noise_mean,this->noise_std);

											// 		//intensity of pure signal
											// 		repulsion_intensity = this->A0 * exp(-dist*(this->alpha)) + this->Ae;

											// 		//noise is proportional to intensity
											// 		double signal_std = repulsion_intensity * noise_distro_std(this->generator);

											// 		//use proportional value computed as standard deviation
											// 		std::normal_distribution<double> noise_distro_signal(repulsion_intensity,signal_std);

											// 		//noisy repulsion intenisity of communicated signal
											// 		repulsion_intensity = noise_distro_signal(this->generator);

											// 	}
											// 	else if(this->com_model.compare("vector") == 0) {
											// 		//TO DO
											// 	}
											// 	//double repulsion_intensity = (this->nei_sensing - dist)/(this->nei_sensing);
											// 	/*******************************************************/
											// 	// double repulsion_intensity = this->A0 * exp(-dist*(this->alpha)) + this->Ae;
											// 	if(dist > this->nei_sensing and this->cap_com == 1)
											// 	{//If outside comm range and cap_com is set
											// 		repulsion_intensity = 0;
											// 	}

											// 	if(repulsion_intensity < 0)
											// 		repulsion_intensity = 0;
											// 	/*******************************************************/

											// 	rep_signal += repulsion_intensity;
											// 	rep_data = rep_data + "," + to_string(dist);
											// }

											// if(n_seen_litter >= this->lit_threshold)//If seen litter is greater than 5, send an attraction signal
											// {
												//cout<<"n_seen_litter:"<<n_seen_litter<<" this->lit_threshold:"<<this->lit_threshold<<endl;
												att_neighbours += 1;
												double attraction_intensity = 0;
												if(this->com_model.compare("linear") == 0){
													attraction_intensity = (this->nei_sensing - dist)/(this->nei_sensing);
												}
												else if(this->com_model.compare("sound") == 0) {
													attraction_intensity = this->A0 * exp(-dist*(this->alpha)) + this->Ae;
													//add noise
													attraction_intensity += this->noise_distro(this->generator);
												}
												else if(this->com_model.compare("soundv2") == 0){
													//noise_mean is average of fitError/signalStrength and noise_std is the deviation across experiments
													std::normal_distribution<double> noise_distro_std1(this->noise_mean,this->noise_std);

													//intensity of pure signal
													attraction_intensity = this->A0 * exp(-dist*(this->alpha)) + this->Ae;

													//noise is proportional to intensity
													double signal_std1 = attraction_intensity * noise_distro_std1(this->generator);

													//use proportional value computed as standard deviation
													std::normal_distribution<double> noise_distro_signal1(attraction_intensity,signal_std1);

													//noisy repulsion intenisity of communicated signal
													attraction_intensity = noise_distro_signal1(this->generator);

												}
												else if(this->com_model.compare("vector") == 0) {
													//TO DO
												}
												// double attraction_intensity = (this->nei_sensing - dist)/(this->nei_sensing);
												/*******************************************************/
												// double attraction_intensity = this->A0 * exp(-dist*(this->alpha)) + this->Ae;

												if(dist > this->nei_sensing and this->cap_com == 1)
												{//If outside comm range and cap_com is set
													attraction_intensity = 0;
												}

												if(attraction_intensity < 0)
													attraction_intensity = 0;
												/*******************************************************/
												att_signal += attraction_intensity;
												att_data = att_data + "," + to_string(dist);
											// }
											//Computing Resultant Vector
											double vec_x,vec_y;
											vec_x = (nest_pos.x - n_pos.x)/dist;
											vec_y = (nest_pos.y - n_pos.y)/dist;
											rslt_x += vec_x;
											rslt_y += vec_y;
									// 	}
									// }
								// }
											double rslt_theta = atan2(rslt_y,rslt_x);

											msgs::Any any;
											any.set_type(msgs::Any::STRING);
											any.set_string_value(to_string(0) + ":" + to_string(0)
																	+ ":" + to_string(rslt_theta) + ":" +
																	to_string(att_neighbours) + ":" + to_string(att_signal));
											this->pub_commSignal[n_name]->Publish(any);//publish attraction info to the specific robot

							}

						// 	//any.set_string_value(to_string(att_neighbours) + ":" + to_string(att_signal));
						// 	//this->pub_attraction[r_name]->Publish(any);

						// 	if(st.nsec==0){//record repulsion once per second.
						// 		rep_data = r_name + "," + to_string(rep_signal) + "," + to_string(rep_neighbours) + rep_data;
						// 		msgs::Any any2;
						// 		any2.set_type(msgs::Any::STRING);
						// 		any2.set_string_value(rep_data);
						// 		this->pub_repel_signal->Publish(any2);

						// 		att_data = r_name + "," + to_string(att_signal) + "," + to_string(att_neighbours) + att_data;
						// 		any2.set_string_value(att_data);
						// 		this->pub_attract_signal->Publish(any2);
						// 	}
						// // }

						//std::cout <<this->log_timer << this->log_rate << this->start_sim <<std::endl;
						if( (st.nsec==0 or (this->log_timer >= this->log_rate and this->start_sim)))//rate of 100Hz
						{
							this->log_timer = 0;
							this->no_litter = true;//Assume there is no litter within world
							//std::cout<<this->psec<<endl;
							std::string all_litter_pos = "";

							double picked_litter = 0;

							for(auto m: this->litter_ptr)
							{//Get current pose of all litter in world
								gazebo::math::Vector3 lit_loc = m->GetWorldPose().pos;

								if(abs(lit_loc.x) < 500 && abs(lit_loc.y) < 500)
								{
									this->no_litter = false;
								}
								else
								{
									picked_litter += 1;
								}
								all_litter_pos = all_litter_pos + to_string(lit_loc.x) + ","
																+ to_string(lit_loc.y) + ":";

							}
							msgs::Any all_litter_pos_msg;
							all_litter_pos_msg.set_type(msgs::Any::STRING);

							all_litter_pos = to_string(_info.simTime.Double())
											+ ":" + all_litter_pos;
							all_litter_pos_msg.set_string_value(all_litter_pos);
							this->pub_litter_pose->Publish(all_litter_pos_msg);//publish current pos of all litter

						}
						//this->psec = st.nsec;
					}
					if(this->log_timer > this->log_rate)
					{
						this->log_timer = 0;
					}

					/*if(this->param_set)
					{
						//msgs::Any any;
						//any.set_type(msgs::Any::INT32);
						//any.set_int_value(this->robot_ptr.size());
						//any.set_type(msgs::Any::BOOLEAN);
						//any.set_bool_value(true);
						if(st.sec >= 30)
						{
							msgs::Any any;
							//any.set_type(msgs::Any::STRING);
							//any.set_string_value("end");
							any.set_type(msgs::Any::BOOLEAN);
							any.set_bool_value(this->world_info_bool);
							this->pub_world_loaded->Publish(any);
							this->world->Reset();
							this->world->Stop();//or this->world->Stop()(or Fini();
							//this->Load(this->world,sdf::ElementPtr sdf);
							//this->world_info_bool = true;
							//this->world->Load
							//this->world->Init();
						}
					}
					if(this->world_info_bool){
						msgs::Any any;
						//any.set_type(msgs::Any::STRING);
						//any.set_string_value("start");
						any.set_type(msgs::Any::BOOLEAN);
						any.set_bool_value(this->world_info_bool);
						this->pub_world_loaded->Publish(any);
						this->world_info_bool = false;
					}
					//this->pub_world_loaded->Publish(any);*/
				}

			}
	};

	//Register this plugin with the simulator
	GZ_REGISTER_WORLD_PLUGIN(WP_Swarm1)
}


/*    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.
 */

#include <boost/lambda/lambda.hpp>

#include "tudat/simulation/environment_setup/createAerodynamicCoefficientInterface.h"

namespace tudat
{

namespace simulation_setup
{

//! Function to create aerodynamic coefficient settings from coefficients stored in data files
std::shared_ptr< AerodynamicCoefficientSettings > readTabulatedAerodynamicCoefficientsFromFiles(
        const std::map< int, std::string > forceCoefficientFiles,
        const std::map< int, std::string > momentCoefficientFiles,
        const double referenceLength,
        const double referenceArea,
        const double lateralReferenceLength,
        const Eigen::Vector3d& momentReferencePoint,
        const std::vector< aerodynamics::AerodynamicCoefficientsIndependentVariables > independentVariableNames,
        const bool areCoefficientsInAerodynamicFrame,
        const bool areCoefficientsInNegativeAxisDirection,
        const std::shared_ptr< interpolators::InterpolatorSettings > interpolatorSettings )
{
    // Retrieve number of independent variables from file.
    int numberOfIndependentVariables =
            input_output::getNumberOfIndependentVariablesInCoefficientFile( forceCoefficientFiles.begin( )->second );

    // Call approriate file reading function for N independent variables
    std::shared_ptr< AerodynamicCoefficientSettings > coefficientSettings;
    if( numberOfIndependentVariables == 1 )
    {
        coefficientSettings = readGivenSizeTabulatedAerodynamicCoefficientsFromFiles< 1 >(
                    forceCoefficientFiles, momentCoefficientFiles, referenceLength, referenceArea, lateralReferenceLength,
                    momentReferencePoint, independentVariableNames, areCoefficientsInAerodynamicFrame,
                    areCoefficientsInNegativeAxisDirection, interpolatorSettings );
    }
    else if( numberOfIndependentVariables == 2 )
    {
        coefficientSettings = readGivenSizeTabulatedAerodynamicCoefficientsFromFiles< 2 >(
                    forceCoefficientFiles, momentCoefficientFiles, referenceLength, referenceArea, lateralReferenceLength,
                    momentReferencePoint, independentVariableNames, areCoefficientsInAerodynamicFrame,
                    areCoefficientsInNegativeAxisDirection, interpolatorSettings );
    }
    else if( numberOfIndependentVariables == 3 )
    {
        coefficientSettings = readGivenSizeTabulatedAerodynamicCoefficientsFromFiles< 3 >(
                    forceCoefficientFiles, momentCoefficientFiles, referenceLength, referenceArea, lateralReferenceLength,
                    momentReferencePoint, independentVariableNames, areCoefficientsInAerodynamicFrame,
                    areCoefficientsInNegativeAxisDirection, interpolatorSettings );
    }
    else
    {
        throw std::runtime_error( "Error when reading aerodynamic coefficient settings from file, found " +
                                  std::to_string( numberOfIndependentVariables ) +
                                  " independent variables, up to 3 currently supported" );
    }
    return coefficientSettings;
}

//! Function to create aerodynamic coefficient settings from coefficients stored in data files
std::shared_ptr< AerodynamicCoefficientSettings >
readTabulatedAerodynamicCoefficientsFromFiles(
        const std::map< int, std::string > forceCoefficientFiles,
        const double referenceArea,
        const std::vector< aerodynamics::AerodynamicCoefficientsIndependentVariables > independentVariableNames,
        const bool areCoefficientsInAerodynamicFrame,
        const bool areCoefficientsInNegativeAxisDirection,
        const std::shared_ptr< interpolators::InterpolatorSettings > interpolatorSettings )
{
    // Retrieve number of independent variables from file.
    int numberOfIndependentVariables =
            input_output::getNumberOfIndependentVariablesInCoefficientFile( forceCoefficientFiles.begin( )->second );

    // Call approriate file reading function for N independent variables
    std::shared_ptr< AerodynamicCoefficientSettings > coefficientSettings;
    if( numberOfIndependentVariables == 1 )
    {
        coefficientSettings = readGivenSizeTabulatedAerodynamicCoefficientsFromFiles< 1 >(
                    forceCoefficientFiles, referenceArea, independentVariableNames,
                    areCoefficientsInAerodynamicFrame, areCoefficientsInNegativeAxisDirection, interpolatorSettings );
    }
    else if( numberOfIndependentVariables == 2 )
    {
        coefficientSettings = readGivenSizeTabulatedAerodynamicCoefficientsFromFiles< 2 >(
                    forceCoefficientFiles, referenceArea, independentVariableNames,
                    areCoefficientsInAerodynamicFrame, areCoefficientsInNegativeAxisDirection, interpolatorSettings );
    }
    else if( numberOfIndependentVariables == 3 )
    {
        coefficientSettings = readGivenSizeTabulatedAerodynamicCoefficientsFromFiles< 3 >(
                    forceCoefficientFiles, referenceArea, independentVariableNames,
                    areCoefficientsInAerodynamicFrame, areCoefficientsInNegativeAxisDirection, interpolatorSettings );
    }
    else
    {
        throw std::runtime_error( "Error when reading aerodynamic coefficient settings from file, found " +
                                  std::to_string( numberOfIndependentVariables ) +
                                  " independent variables, up to 3 currently supported" );
    }
    return coefficientSettings;
}

//! Function to create an aerodynamic coefficient interface containing constant coefficients.
std::shared_ptr< aerodynamics::AerodynamicCoefficientInterface >
createConstantCoefficientAerodynamicCoefficientInterface(
        const Eigen::Vector3d constantForceCoefficient,
        const Eigen::Vector3d constantMomentCoefficient,
        const double referenceLength,
        const double referenceArea,
        const double lateralReferenceLength,
        const Eigen::Vector3d& momentReferencePoint,
        const bool areCoefficientsInAerodynamicFrame,
        const bool areCoefficientsInNegativeAxisDirection )
{
    // Create coefficient interface
    std::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > coefficientInterface =
            std::make_shared< aerodynamics::CustomAerodynamicCoefficientInterface >(
                [ = ]( const std::vector< double >& ){ return constantForceCoefficient; },
                [ = ]( const std::vector< double >& ){ return constantMomentCoefficient; },
                referenceLength, referenceArea, lateralReferenceLength, momentReferencePoint,
                std::vector< aerodynamics::AerodynamicCoefficientsIndependentVariables >( ),
                areCoefficientsInAerodynamicFrame, areCoefficientsInNegativeAxisDirection );
    coefficientInterface->updateFullCurrentCoefficients( std::vector< double >( ) );
    return coefficientInterface;
}

//! Function to create an aerodynamic coefficient interface containing constant coefficients.
std::shared_ptr< aerodynamics::AerodynamicCoefficientInterface >
createZeroParameterAerodynamicCoefficientInterface(
        const std::function< Eigen::Vector3d( ) > constantForceCoefficientFunction,
        const std::function< Eigen::Vector3d( ) > constantMomentCoefficientFunction,
        const double referenceLength,
        const double referenceArea,
        const double lateralReferenceLength,
        const Eigen::Vector3d& momentReferencePoint,
        const bool areCoefficientsInAerodynamicFrame,
        const bool areCoefficientsInNegativeAxisDirection )
{
    // Create coefficient interface
    std::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > coefficientInterface =
            std::make_shared< aerodynamics::CustomAerodynamicCoefficientInterface >(
                [ = ]( const std::vector< double >& ){ return constantForceCoefficientFunction( ); },
                [ = ]( const std::vector< double >& ){ return constantMomentCoefficientFunction( ); },
                referenceLength, referenceArea, lateralReferenceLength, momentReferencePoint,
                std::vector< aerodynamics::AerodynamicCoefficientsIndependentVariables >( ),
                areCoefficientsInAerodynamicFrame, areCoefficientsInNegativeAxisDirection );
    coefficientInterface->updateFullCurrentCoefficients( std::vector< double >( ) );
    return coefficientInterface;
}

//! Factory function for tabulated (1-D independent variables) aerodynamic coefficient interface from coefficient settings.
std::shared_ptr< aerodynamics::AerodynamicCoefficientInterface >
createUnivariateTabulatedCoefficientAerodynamicCoefficientInterface(
        const std::shared_ptr< AerodynamicCoefficientSettings > coefficientSettings,
        const std::string& body )
{
    using namespace tudat::interpolators;

    // Check consistency of type.
    std::shared_ptr< TabulatedAerodynamicCoefficientSettings< 1 > > tabulatedCoefficientSettings =
            std::dynamic_pointer_cast< TabulatedAerodynamicCoefficientSettings< 1 > >( coefficientSettings );
    if( tabulatedCoefficientSettings == nullptr )
    {
        throw std::runtime_error(
                    "Error, expected tabulated aerodynamic coefficients of size " +
                    std::to_string( 1 ) + "for body " + body );
    }
    else
    {

        // Retrieve or generate interpolation settings
        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::Vector3d > > forceInterpolator;
        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::Vector3d > > momentInterpolator;
        if ( tabulatedCoefficientSettings->getInterpolatorSettings( ) == nullptr )
        {
            forceInterpolator = createOneDimensionalInterpolator( tabulatedCoefficientSettings->getForceCoefficients( ),
                                                                  std::make_shared< InterpolatorSettings >( linear_interpolator ) );
            momentInterpolator = createOneDimensionalInterpolator( tabulatedCoefficientSettings->getForceCoefficients( ),
                                                                   std::make_shared< InterpolatorSettings >( linear_interpolator ) );
        }
        else
        {
            forceInterpolator = createOneDimensionalInterpolator( tabulatedCoefficientSettings->getForceCoefficients( ),
                                                                  std::dynamic_pointer_cast< InterpolatorSettings >(
                                                                      tabulatedCoefficientSettings->getInterpolatorSettings( ) ) );
            momentInterpolator = createOneDimensionalInterpolator( tabulatedCoefficientSettings->getForceCoefficients( ),
                                                                   std::dynamic_pointer_cast< InterpolatorSettings >(
                                                                       tabulatedCoefficientSettings->getInterpolatorSettings( ) ) );
        }

        // Create aerodynamic coefficient interface.
        return  std::make_shared< aerodynamics::CustomAerodynamicCoefficientInterface >(
                    std::bind( &Interpolator< double, Eigen::Vector3d >::interpolate, forceInterpolator, std::placeholders::_1 ),
                    std::bind( &Interpolator< double, Eigen::Vector3d >::interpolate, momentInterpolator, std::placeholders::_1 ),
                    tabulatedCoefficientSettings->getReferenceLength( ),
                    tabulatedCoefficientSettings->getReferenceArea( ),
                    tabulatedCoefficientSettings->getReferenceLength( ),
                    tabulatedCoefficientSettings->getMomentReferencePoint( ),
                    tabulatedCoefficientSettings->getIndependentVariableNames( ),
                    tabulatedCoefficientSettings->getAreCoefficientsInAerodynamicFrame( ),
                    tabulatedCoefficientSettings->getAreCoefficientsInNegativeAxisDirection( ) );
    }
}

//! Function to create and aerodynamic coefficient interface.
std::shared_ptr< aerodynamics::AerodynamicCoefficientInterface >
createAerodynamicCoefficientInterface(
        const std::shared_ptr< AerodynamicCoefficientSettings > coefficientSettings,
        const std::string& body )
{
    using namespace tudat::aerodynamics;

    std::shared_ptr< AerodynamicCoefficientInterface > coefficientInterface;

    // Check type of interface that is to be created.
    switch( coefficientSettings->getAerodynamicCoefficientType( ) )
    {
    case constant_aerodynamic_coefficients:
    {
        // Check consistency of type.
        std::shared_ptr< ConstantAerodynamicCoefficientSettings > constantCoefficientSettings =
                std::dynamic_pointer_cast< ConstantAerodynamicCoefficientSettings >(
                    coefficientSettings );
        if( constantCoefficientSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected constant aerodynamic coefficients for body " + body );
        }
        else
        {
            // create constant interface.
            coefficientInterface = createConstantCoefficientAerodynamicCoefficientInterface(
                        constantCoefficientSettings->getConstantForceCoefficient( ),
                        constantCoefficientSettings->getConstantMomentCoefficient( ),
                        constantCoefficientSettings->getReferenceLength( ),
                        constantCoefficientSettings->getReferenceArea( ),
                        constantCoefficientSettings->getReferenceLength( ),
                        constantCoefficientSettings->getMomentReferencePoint( ),
                        constantCoefficientSettings->getAreCoefficientsInAerodynamicFrame( ),
                        constantCoefficientSettings->getAreCoefficientsInNegativeAxisDirection( ) );
        }
        break;
    }
    case custom_aerodynamic_coefficients:
    {
        // Check consistency of type.
        std::shared_ptr< CustomAerodynamicCoefficientSettings > customCoefficientSettings =
                std::dynamic_pointer_cast< CustomAerodynamicCoefficientSettings >(
                    coefficientSettings );
        if( customCoefficientSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected custom aerodynamic coefficients for body " + body );
        }
        else
        {
            // create constant interface.
            coefficientInterface = std::make_shared< CustomAerodynamicCoefficientInterface >(
                        customCoefficientSettings->getForceCoefficientFunction( ),
                        customCoefficientSettings->getMomentCoefficientFunction( ),
                        customCoefficientSettings->getReferenceLength( ),
                        customCoefficientSettings->getReferenceArea( ),
                        customCoefficientSettings->getReferenceLength( ),
                        customCoefficientSettings->getMomentReferencePoint( ),
                        customCoefficientSettings->getIndependentVariableNames( ),
                        customCoefficientSettings->getAreCoefficientsInAerodynamicFrame( ),
                        customCoefficientSettings->getAreCoefficientsInNegativeAxisDirection( ) );
        }
        break;
    }
    case tabulated_coefficients:
    {
        // Check number of dimensions of tabulated coefficients.
        int numberOfDimensions = coefficientSettings->getIndependentVariableNames( ).size( );
        switch( numberOfDimensions )
        {
        case 1:
        {
            coefficientInterface = createUnivariateTabulatedCoefficientAerodynamicCoefficientInterface(
                        coefficientSettings, body );
            break;
        }
        case 2:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 2 >(
                        coefficientSettings, body );
            break;
        }
        case 3:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 3 >(
                        coefficientSettings, body );
            break;
        }
        case 4:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 4 >(
                        coefficientSettings, body );
            break;
        }
        case 5:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 5 >(
                        coefficientSettings, body );
            break;
        }
        case 6:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 6 >(
                        coefficientSettings, body );
            break;
        }
        default:
            throw std::runtime_error( "Error when making tabulated aerodynamic coefficient interface, " +
                                      std::to_string( numberOfDimensions ) + " dimensions not yet implemented" );
        }
        break;
    }
    case scaled_coefficients:
    {
        // Check consistency of type and class.
        std::shared_ptr< ScaledAerodynamicCoefficientInterfaceSettings > scaledCoefficientSettings =
                std::dynamic_pointer_cast< ScaledAerodynamicCoefficientInterfaceSettings >(
                    coefficientSettings );
        if( scaledCoefficientSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected scaled aerodynamic coefficient settings for body " + body );
        }
        else
        {
            std::shared_ptr< AerodynamicCoefficientInterface > baseInterface = createAerodynamicCoefficientInterface(
                        scaledCoefficientSettings->getBaseSettings( ), body );
            coefficientInterface = std::make_shared< ScaledAerodynamicCoefficientInterface >(
                        baseInterface, scaledCoefficientSettings->getForceScaling( ),
                        scaledCoefficientSettings->getMomentScaling( ), scaledCoefficientSettings->getIsScalingAbsolute( ) );
        }
        break;
    }
    default:
        throw std::runtime_error( "Error, do not recognize aerodynamic coefficient settings for " + body );
    }

    // Create and set control surfaces
    if( coefficientSettings->getControlSurfaceSettings( ).size( ) != 0 )
    {
        std::map< std::string, std::shared_ptr< ControlSurfaceIncrementAerodynamicInterface > >
                controlSurfaceIncrementInterfaces;
        std::map< std::string, std::shared_ptr< ControlSurfaceIncrementAerodynamicCoefficientSettings > >
                controlSurfaceSettings = coefficientSettings->getControlSurfaceSettings( );
        for( std::map< std::string, std::shared_ptr< ControlSurfaceIncrementAerodynamicCoefficientSettings > >::iterator
             settingIterator = controlSurfaceSettings.begin( ); settingIterator != controlSurfaceSettings.end( );
             settingIterator++ )
        {
            controlSurfaceIncrementInterfaces[ settingIterator->first ] =
                    createControlSurfaceIncrementAerodynamicCoefficientInterface(
                        settingIterator->second, body );
        }
        coefficientInterface->setControlSurfaceIncrements( controlSurfaceIncrementInterfaces );

    }

    return coefficientInterface;
}

} // simulation_setup

} // tudat


#ifndef VISCOELASTICTRANSFORMER_HPP
#define VISCOELASTICTRANSFORMER_HPP

#include <Eigen/Dense>
#include "NeighbourFinder.hpp"
#include <iostream>
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include "helper_functions.hpp"

typedef Eigen::Vector3f Vec3Float;
typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, 3> Vec3Mat; //matrix Mx3 of type float
typedef Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic> MatDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::Matrix< int, Eigen::Dynamic, 3> FacesMat;
typedef OpenMesh::DefaultTraits MyTraits;
typedef OpenMesh::TriMesh_ArrayKernelT<MyTraits>  TriMesh;

namespace registration{

class ViscoElasticTransformer
{
    public:

        void set_input(const FeatureMat * const inCorrespondingFeatures,
                       const VecDynFloat * const inWeights,
                       const VecDynFloat * const inFlags,
                       const FacesMat * const inFloatingFaces);
        void set_output(FeatureMat * const ioFloatingFeatures);
        void set_parameters(size_t numNeighbours = 10, float sigma = 3.0,
                            size_t viscousIterations = 10, size_t elasticIterations = 10);
        Vec3Mat get_transformation() const {return _displacementField;}
        void update();

    protected:

    private:
        //# Inputs
        //##_ioFeatures is used as both an input (to compute the transformation) and output
        const FeatureMat * _inCorrespondingFeatures = NULL;
        const VecDynFloat * _inWeights = NULL;
        const VecDynFloat * _inFlags = NULL;
        const FacesMat * _inFloatingFaces = NULL;

        //# Outputs
        FeatureMat * _ioFloatingFeatures = NULL;

        //# User Parameters
        size_t _numNeighbours = 10;
        float _sigma = 3.0;
        size_t _viscousIterations = 0;
        size_t _elasticIterations = 0;
        size_t _outlierDiffusionIterations = 15;

        //# Internal Data structures
        Vec3Mat _displacementField;
        Vec3Mat _oldDisplacementField;
        NeighbourFinder<Vec3Mat> _neighbourFinder;
        MatDynFloat _smoothingWeights;
        TriMesh _floatingMesh;

        //# Internal Parameters
        size_t _numElements = 0;
        bool _neighboursOutdated = true;
        bool _flagsOutdated = true;
        const float _minWeight = 0.00001f;

        //# Internal functions
        //## Update the neighbour finder
        void _update_neighbours();
        //## Update the weights used for smoothing
        void _update_smoothing_weights();
        //## Update the displacement field in a viscous manner
        void _update_viscously();
        //## Update the displacement field in an elastic manner
        void _update_elastically();
        //## Update the transformation for any outliers (via a diffusion process)
        void _update_outlier_transformation();
        //## Function to update the transformation
        void _update_transformation();
        //## Function to apply the transformation
        void _apply_transformation();
};

}//namespace registration

#endif // VISCOELASTICTRANSFORMER_HPP


#include <iostream>
#include <vector>
#include <set>
#include <tuple>
#include <Eigen/Dense>
#include <math.h>
#include <cosan/io/utils.h>
int main(int argc, char *argv[]){
    CosanMatrix X;
    std::string SummaryMessageX;
    std::vector<std::vector<uint>> IdxpinfX,IdxminfX,IdxmissingX;
    std::set<uint> colCatX;
    uint rowsX = 0,colsX = 0;
    std::vector<std::string> svaluesX;
//    std::cout<<isinf(stod(std::string("inf")))<<std::endl;
//    std::tie(rowsX,colsX,SummaryMessageX) = _load_csv<Eigen::MatrixXd>(argv[1],X,IdxpinfX,IdxminfX,IdxmissingX,svaluesX,colCatX);
    std::cout<<rowsX<<" "<<colsX<<" "<<std::endl;
    for (auto each : IdxpinfX) {std::cout<<each[0]<<" "<<each[1]<<std::endl;}
    for (auto each : IdxminfX) {std::cout<<each[0]<<" "<<each[1]<<std::endl;}
    for (auto each : IdxmissingX) {std::cout<<each[0]<<" "<<each[1]<<std::endl;}
    for (auto each : colCatX) {std::cout<<each<<std::endl;}
    for (auto each : svaluesX) {std::cout<<each<<std::endl;}
    std::cout<<X<<std::endl;
    return 0;
}


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

  Defines the abstraction for SIMD registers

  @copyright 2015 NumScale SAS

  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_DETAIL_PACK_OPERATORS_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_PACK_OPERATORS_HPP_INCLUDED

#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/modulo.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/is_less_equal.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_greater_equal.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/complement.hpp>
#include <boost/simd/function/logical_not.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/logical_not.hpp>
#include <boost/simd/function/unary_plus.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/detail/brigand.hpp>

#include <boost/simd/meta/is_pack.hpp>

namespace boost { namespace simd
{

#define BOOST_SIMD_PACK_DEFINE_OP(type_, op, f)                                                    \
  template <typename T, std::size_t N, typename U> BOOST_FORCEINLINE                               \
  typename std::enable_if<is_not_pack_t<U>::value, pack<type_, N>>::type                           \
  op(pack<T, N> const& p0, U const& s1) BOOST_NOEXCEPT { return f(p0, s1); }                       \
                                                                                \
  template <typename T, std::size_t N, typename U> BOOST_FORCEINLINE                               \
  typename std::enable_if<is_not_pack_t<U>::value, pack<type_, N>>::type                           \
  op(U const& s0, pack<T, N> const& p1) BOOST_NOEXCEPT { return f(s0, p1); }                       \
                                                                                \
  template <typename T, std::size_t N> BOOST_FORCEINLINE                                           \
  auto op(pack<T, N> const& p0, pack<T, N> const& p1) BOOST_NOEXCEPT                               \
  -> decltype(f(p0, p1)) { return f(p0, p1); }                                                     \
/**/

BOOST_SIMD_PACK_DEFINE_OP(T, operator+, plus)
BOOST_SIMD_PACK_DEFINE_OP(T, operator-, minus)
BOOST_SIMD_PACK_DEFINE_OP(T, operator%, modulo)
BOOST_SIMD_PACK_DEFINE_OP(T, operator/, divides)
BOOST_SIMD_PACK_DEFINE_OP(T, operator*, multiplies)

BOOST_SIMD_PACK_DEFINE_OP(T, operator&, bitwise_and)
BOOST_SIMD_PACK_DEFINE_OP(T, operator|, bitwise_or)
BOOST_SIMD_PACK_DEFINE_OP(T, operator^, bitwise_xor)
BOOST_SIMD_PACK_DEFINE_OP(logical<T>, operator&&, logical_and)
BOOST_SIMD_PACK_DEFINE_OP(logical<T>, operator||, logical_or)

BOOST_SIMD_PACK_DEFINE_OP(T, operator << , shift_left)
BOOST_SIMD_PACK_DEFINE_OP(T, operator >> , shift_right)

BOOST_SIMD_PACK_DEFINE_OP(logical<T>, operator<,  is_less)
BOOST_SIMD_PACK_DEFINE_OP(logical<T>, operator>,  is_greater)
BOOST_SIMD_PACK_DEFINE_OP(logical<T>, operator==, is_equal)
BOOST_SIMD_PACK_DEFINE_OP(logical<T>, operator<=, is_less_equal)
BOOST_SIMD_PACK_DEFINE_OP(logical<T>, operator>=, is_greater_equal)
BOOST_SIMD_PACK_DEFINE_OP(logical<T>, operator!=, is_not_equal)

#undef BOOST_SIMD_PACK_DEFINE_OP

  template <typename T, std::size_t N>
  BOOST_FORCEINLINE pack<as_logical_t<T>,N> operator!(pack<T,N> const& a) BOOST_NOEXCEPT
  {
    return logical_not(a);
  }

} }

#endif


// Copyright 2020 The Defold Foundation
// Licensed under the Defold License version 1.0 (the "License"); you may not use
// this file except in compliance with the License.
//
// You may obtain a copy of the License, together with FAQs at
// https://www.defold.com/license
//
// 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.

#include "res_convex_shape.h"

#include <dlib/log.h>

#include "gamesys.h"
#include <gamesys/physics_ddf.h>

/*
 * NOTE: Convex-shape is obsolete and the code below is not currently in use
 * We currently embed external convex shape resources into the collisionshape at compile-time
 */

namespace dmGameSystem
{
    bool AcquireResources(dmResource::HFactory factory,
                           PhysicsContext* context,
                           const void* buffer, uint32_t buffer_size,
                           ConvexShapeResource* resource,
                           const char* filename)
    {
        dmPhysicsDDF::ConvexShape* convex_shape;
        dmDDF::Result e = dmDDF::LoadMessage<dmPhysicsDDF::ConvexShape>(buffer, buffer_size, &convex_shape);
        if ( e != dmDDF::RESULT_OK )
        {
            return false;
        }

        bool result = true;
        switch (convex_shape->m_ShapeType)
        {
        case dmPhysicsDDF::ConvexShape::TYPE_SPHERE:
            if (convex_shape->m_Data.m_Count != 1)
            {
                dmLogError("Invalid sphere shape");
                result = false;
            }
            else
            {
                if (context->m_3D)
                    resource->m_Shape3D = dmPhysics::NewSphereShape3D(context->m_Context3D, convex_shape->m_Data[0]);
                else
                    resource->m_Shape2D = dmPhysics::NewCircleShape2D(context->m_Context2D, convex_shape->m_Data[0]);
            }
            break;
        case dmPhysicsDDF::ConvexShape::TYPE_BOX:
            if (convex_shape->m_Data.m_Count != 3)
            {
                dmLogError("Invalid box shape");
                result = false;
            }
            else
            {
                if (context->m_3D)
                    resource->m_Shape3D = dmPhysics::NewBoxShape3D(context->m_Context3D, dmVMath::Vector3(convex_shape->m_Data[0], convex_shape->m_Data[1], convex_shape->m_Data[2]));
                else
                    resource->m_Shape2D = dmPhysics::NewBoxShape2D(context->m_Context2D, dmVMath::Vector3(convex_shape->m_Data[0], convex_shape->m_Data[1], convex_shape->m_Data[2]));
            }
            break;
        case dmPhysicsDDF::ConvexShape::TYPE_CAPSULE:
            if (convex_shape->m_Data.m_Count != 2)
            {
                dmLogError("Invalid capsule shape");
                result = false;
            }
            else
            {
                if (context->m_3D)
                    resource->m_Shape3D = dmPhysics::NewCapsuleShape3D(context->m_Context3D, convex_shape->m_Data[0], convex_shape->m_Data[1]);
                else
                    // TODO: Add support
                    dmLogError("%s", "Capsules are not supported in 2D.");
            }
            break;
        case dmPhysicsDDF::ConvexShape::TYPE_HULL:
            if (convex_shape->m_Data.m_Count < 9)
            {
                dmLogError("Invalid hull shape");
                result = false;
            }
            else
            {
                if (context->m_3D)
                    resource->m_Shape3D = dmPhysics::NewConvexHullShape3D(context->m_Context3D, &convex_shape->m_Data[0], convex_shape->m_Data.m_Count / 3);
                else
                {
                    const uint32_t data_size = 2 * convex_shape->m_Data.m_Count / 3;
                    float* data_2d = new float[2 * convex_shape->m_Data.m_Count / 3];
                    for (uint32_t i = 0; i < data_size; ++i)
                    {
                        data_2d[i] = convex_shape->m_Data[i/2*3 + i%2];
                    }
                    resource->m_Shape2D = dmPhysics::NewPolygonShape2D(context->m_Context2D, data_2d, data_size/2);
                    delete [] data_2d;
                }
            }
            break;
        }

        dmDDF::FreeMessage(convex_shape);
        return result;
    }

    dmResource::Result ResConvexShapeCreate(const dmResource::ResourceCreateParams& params)
    {
        ConvexShapeResource* convex_shape = new ConvexShapeResource();
        convex_shape->m_3D = ((PhysicsContext*) params.m_Context)->m_3D;
        if (AcquireResources(params.m_Factory, (PhysicsContext*) params.m_Context, params.m_Buffer, params.m_BufferSize, convex_shape, params.m_Filename))
        {
            params.m_Resource->m_Resource = convex_shape;
            return dmResource::RESULT_OK;
        }
        else
        {
            delete convex_shape;
            return dmResource::RESULT_FORMAT_ERROR;
        }
    }

    void ReleaseResources(ConvexShapeResource* resource)
    {
        if (resource->m_Shape3D)
        {
            if (resource->m_3D)
                dmPhysics::DeleteCollisionShape3D(resource->m_Shape3D);
            else
                dmPhysics::DeleteCollisionShape2D(resource->m_Shape2D);
        }
    }

    dmResource::Result ResConvexShapeDestroy(const dmResource::ResourceDestroyParams& params)
    {
        ConvexShapeResource* convex_shape = (ConvexShapeResource*)params.m_Resource->m_Resource;
        ReleaseResources(convex_shape);
        delete convex_shape;
        return dmResource::RESULT_OK;
    }

    dmResource::Result ResConvexShapeRecreate(const dmResource::ResourceRecreateParams& params)
    {
        ConvexShapeResource* cs_resource = (ConvexShapeResource*)params.m_Resource->m_Resource;
        ConvexShapeResource tmp_convex_shape;
        PhysicsContext* physics_context = (PhysicsContext*) params.m_Context;
        tmp_convex_shape.m_3D = physics_context->m_3D;
        if (AcquireResources(params.m_Factory, (PhysicsContext*) params.m_Context, params.m_Buffer, params.m_BufferSize, &tmp_convex_shape, params.m_Filename))
        {
            if (physics_context->m_3D)
                dmPhysics::ReplaceShape3D(physics_context->m_Context3D, cs_resource->m_Shape3D, tmp_convex_shape.m_Shape3D);
            else
                dmPhysics::ReplaceShape2D(physics_context->m_Context2D, cs_resource->m_Shape2D, tmp_convex_shape.m_Shape2D);
            ReleaseResources(cs_resource);
            cs_resource->m_Shape3D = tmp_convex_shape.m_Shape3D;
            return dmResource::RESULT_OK;
        }
        else
        {
            return dmResource::RESULT_FORMAT_ERROR;
        }
    }
}


// Copyright 2021 Josh Pieper, jjp@pobox.com.
//
// 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.

#include "fw/stm32_i2c_timing.h"

#include <boost/test/auto_unit_test.hpp>

using namespace moteus;

namespace tt = boost::test_tools;

// Just some basic sanity checks

BOOST_AUTO_TEST_CASE(TimingTest1) {
  TimingInput input;
  input.peripheral_hz = 64000000;
  input.i2c_hz = 100000;
  input.i2c_mode = I2cMode::kStandard;
  input.analog_filter = AnalogFilter::kOff;

  const auto result = CalculateI2cTiming(input);
  BOOST_TEST(result.error == 0);
  BOOST_TEST(result.prescaler == 1);
  BOOST_TEST(result.scldel == 8);
  BOOST_TEST(result.sclh == 147);
  BOOST_TEST(result.scll == 172);
  BOOST_TEST(result.timingr == 0x108993ac);
}

BOOST_AUTO_TEST_CASE(TimingTest2) {
  TimingInput input;
  input.peripheral_hz = 64000000;
  input.i2c_hz = 400000;
  input.i2c_mode = I2cMode::kFast;
  input.analog_filter = AnalogFilter::kOff;

  const auto result = CalculateI2cTiming(input);
  BOOST_TEST(result.error == 0);
  BOOST_TEST(result.prescaler == 0);
  BOOST_TEST(result.scldel == 6);
  BOOST_TEST(result.sclh == 51);
  BOOST_TEST(result.scll == 108);
  BOOST_TEST(result.timingr == 0x0060336c);
}

BOOST_AUTO_TEST_CASE(TimingTest3) {
  TimingInput input;
  input.peripheral_hz = 64000000;
  input.i2c_hz = 1000000;
  input.i2c_mode = I2cMode::kFastPlus;
  input.analog_filter = AnalogFilter::kOff;

  const auto result = CalculateI2cTiming(input);
  BOOST_TEST(result.error == 0);
  BOOST_TEST(result.prescaler == 0);
  BOOST_TEST(result.scldel == 3);
  BOOST_TEST(result.sclh == 21);
  BOOST_TEST(result.scll == 42);
  BOOST_TEST(result.timingr == 0x0030152a);
}

BOOST_AUTO_TEST_CASE(TimingTest4) {
  TimingInput input;
  input.peripheral_hz = 128000000;
  input.i2c_hz = 1000000;
  input.i2c_mode = I2cMode::kFastPlus;
  input.analog_filter = AnalogFilter::kOff;

  const auto result = CalculateI2cTiming(input);
  BOOST_TEST(result.error == 0);
  BOOST_TEST(result.prescaler == 0);
  BOOST_TEST(result.scldel == 6);
  BOOST_TEST(result.sclh == 42);
  BOOST_TEST(result.scll == 85);
  BOOST_TEST(result.timingr == 0x00602a55);
}


#include <boost/python.hpp>

#include <scitbx/cubicle_neighbors.hpp>

namespace scitbx { namespace cubicle_neighbors_ext {

  struct cubicle_neighbors_wrappers
  {
    typedef cubicle_neighbors<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("cubicle_neighbors", no_init)
        .def(init<
          af::const_ref<vec3<double> > const&,
          double const&,
          double const&>((
            arg("main_sites_cart"),
            arg("cubicle_edge"),
            arg("epsilon")=1.e-6)))
        .def("neighbors_of", &w_t::neighbors_of, (
          arg("other_sites_cart"),
          arg("distance_cutoff_sq")))
      ;
    }
  };

  void
  init_module()
  {
    using namespace boost::python;
    cubicle_neighbors_wrappers::wrap();
    def("cubicles_max_memory_allocation_set",
      cubicles_max_memory_allocation_set, (arg("number_of_bytes")));
    def("cubicles_max_memory_allocation_get",
      cubicles_max_memory_allocation_get);
  }

}} // namespace scitbx::cubicle_neighbors_ext

BOOST_PYTHON_MODULE(scitbx_cubicle_neighbors_ext)
{
  scitbx::cubicle_neighbors_ext::init_module();
}


#include "Geometry/GEMGeometryBuilder/src/ME0GeometryBuilderFromDDD10EtaPart.h"
#include "Geometry/GEMGeometry/interface/ME0Geometry.h"
#include "Geometry/GEMGeometry/interface/ME0EtaPartitionSpecs.h"

#include <DetectorDescription/Core/interface/DDFilter.h>
#include <DetectorDescription/Core/interface/DDFilteredView.h>
#include <DetectorDescription/Core/interface/DDSolid.h>

#include "Geometry/MuonNumbering/interface/MuonDDDNumbering.h"
#include "Geometry/MuonNumbering/interface/MuonBaseNumber.h"
#include "Geometry/MuonNumbering/interface/ME0NumberingScheme.h"

#include "DataFormats/GeometrySurface/interface/TrapezoidalPlaneBounds.h"

#include "DataFormats/GeometryVector/interface/Basic3DVector.h"

#include "CLHEP/Units/GlobalSystemOfUnits.h"

#include <iostream>
#include <algorithm>
#include <boost/lexical_cast.hpp>

ME0GeometryBuilderFromDDD10EtaPart::ME0GeometryBuilderFromDDD10EtaPart()
{ }

ME0GeometryBuilderFromDDD10EtaPart::~ME0GeometryBuilderFromDDD10EtaPart()
{ }

ME0Geometry* ME0GeometryBuilderFromDDD10EtaPart::build(const DDCompactView* cview, const MuonDDDConstants& muonConstants)
{

  std::string attribute = "MuStructure";
  std::string value     = "MuonEndCapME0";

  // Asking only for the MuonME0's
  DDSpecificsMatchesValueFilter filter{DDValue(attribute, value, 0.0)};
  DDFilteredView fview(*cview,filter);

  return this->buildGeometry(fview, muonConstants);
}

ME0Geometry* ME0GeometryBuilderFromDDD10EtaPart::buildGeometry(DDFilteredView& fv, const MuonDDDConstants& muonConstants)
{

  ME0Geometry* geometry = new ME0Geometry();

  LogTrace("ME0GeometryBuilderFromDDD") <<"Building the geometry service";
  LogTrace("ME0GeometryBuilderFromDDD") <<"About to run through the ME0 structure\n"
					<<"Top level logical part: "
					<<fv.logicalPart().name().name();

  // ==========================================
  // ===  Test to understand the structure  ===
  // ==========================================
  #ifdef EDM_ML_DEBUG
  bool testChambers = fv.firstChild();
  LogTrace("ME0GeometryBuilderFromDDD") << "doChamber = fv.firstChild() = " << testChambers;
  // ----------------------------------------------------------------------------------------------------------------------------------------------
  while (testChambers) {
    // to etapartitions
    LogTrace("ME0GeometryBuilderFromDDD")<<"to layer "<<fv.firstChild(); // commented out in case only looping over sensitive volumes
    LogTrace("ME0GeometryBuilderFromDDD")<<"to etapt "<<fv.firstChild(); // commented out in case only looping over sensitive volumes
    MuonDDDNumbering mdddnum(muonConstants);
    ME0NumberingScheme me0Num(muonConstants);
    int rawId = me0Num.baseNumberToUnitNumber(mdddnum.geoHistoryToBaseNumber(fv.geoHistory()));
    ME0DetId detId = ME0DetId(rawId);
    ME0DetId detIdCh = detId.chamberId();
    // back to chambers
    LogTrace("ME0GeometryBuilderFromDDD")<<"back to layer "<<fv.parent(); // commented out in case only looping over sensitive volumes
    LogTrace("ME0GeometryBuilderFromDDD")<<"back to chamb "<<fv.parent(); // commented out in case only looping over sensitive volumes
    // ok lets get started ...
    LogTrace("ME0GeometryBuilderFromDDD") << "In DoChambers Loop :: ME0DetId "<<detId<<" = "<<detId.rawId()<<" (which belongs to ME0Chamber "<<detIdCh<<" = "<<detIdCh.rawId()<<")";
    LogTrace("ME0GeometryBuilderFromDDD") << "Second level logical part: " << fv.logicalPart().name().name();
    DDBooleanSolid solid2 = (DDBooleanSolid)(fv.logicalPart().solid());
    std::vector<double> dpar2  = solid2.parameters();
    std::stringstream parameters2;
    for(unsigned int i=0; i<dpar2.size(); ++i) {
      parameters2 << " dpar["<<i<<"]="<< dpar2[i]/10 << "cm ";
    }
    LogTrace("ME0GeometryBuilderFromDDD") << "Second level parameters: vector with size = "<<dpar2.size()<<" and elements "<<parameters2.str();
    // from GEM
    // DDBooleanSolid solid = (DDBooleanSolid)(fv.logicalPart().solid());
    // std::vector<double> dpar = solid.solidA().parameters();
    /*
    if(solid2.solidA()) {
      std::vector<double> dpar2a = solid2.solidA().parameters();
      std::stringstream parameters2a;
      for(unsigned int i=0; i<dpar2a.size(); ++i) {
	parameters2a << " dpara["<<i<<"]="<< dpar2a[i]/10 << "cm ";
      }
      LogTrace("ME0GeometryBuilderFromDDD") << "Second level parameters: vector with size = "<<dpar2a.size()<<" and elements "<<parameters2.str();
    }
    if(solid2.solidB()) {
      std::vector<double> dpar2b = solid2.solidB().parameters();
      std::stringstream parameters2b;
      for(unsigned int i=0; i<dpar2b.size(); ++i) {
	parameters2b << " dparb["<<i<<"]="<< dpar2b[i]/10 << "cm ";
      }
      LogTrace("ME0GeometryBuilderFromDDD") << "Second level parameters: vector with size = "<<dpar2b.size()<<" and elements "<<parameters2.str();
    }
    */
    bool doLayers = fv.firstChild();
    // --------------------------------------------------------------------------------------------------------------------------------------------
    LogTrace("ME0GeometryBuilderFromDDD") << "doLayer = fv.firstChild() = " << doLayers;
    while (doLayers) {
      // to etapartitions
      LogTrace("ME0GeometryBuilderFromDDD")<<"to etapt "<<fv.firstChild(); // commented out in case only looping over sensitive volumes
      MuonDDDNumbering mdddnum(muonConstants);
      ME0NumberingScheme me0Num(muonConstants);
      int rawId = me0Num.baseNumberToUnitNumber(mdddnum.geoHistoryToBaseNumber(fv.geoHistory()));
      ME0DetId detId = ME0DetId(rawId);
      ME0DetId detIdLa = detId.layerId();
      // back to layers
      LogTrace("ME0GeometryBuilderFromDDD")<<"back to layer "<<fv.parent(); // commented out in case only looping over sensitive volumes
      LogTrace("ME0GeometryBuilderFromDDD") << "In DoLayers Loop :: ME0DetId "<<detId<<" = "<<detId.rawId()<<" (which belongs to ME0Layer "<<detIdLa<<" = "<<detIdLa.rawId()<<")";
      LogTrace("ME0GeometryBuilderFromDDD") << "Third level logical part: " << fv.logicalPart().name().name();
      DDBooleanSolid solid3 = (DDBooleanSolid)(fv.logicalPart().solid());
      std::vector<double> dpar3 = solid3.parameters();
      std::stringstream parameters3;
      for(unsigned int i=0; i<dpar3.size(); ++i) {
	parameters3 << " dpar["<<i<<"]="<< dpar3[i]/10 << "cm ";
      }
      LogTrace("ME0GeometryBuilderFromDDD") << "Third level parameters: vector with size = "<<dpar3.size()<<" and elements "<<parameters3.str();
      bool doEtaParts = fv.firstChild();
      // --------------------------------------------------------------------------------------------------------------------------------------------
      LogTrace("ME0GeometryBuilderFromDDD") << "doEtaPart = fv.firstChild() = " << doEtaParts;
      while (doEtaParts) {
	LogTrace("ME0GeometryBuilderFromDDD") << "In DoEtaParts Loop :: ME0DetId "<<detId<<" = "<<detId.rawId();
	LogTrace("ME0GeometryBuilderFromDDD") << "Fourth level logical part: " << fv.logicalPart().name().name();
	DDBooleanSolid solid4 = (DDBooleanSolid)(fv.logicalPart().solid());
	std::vector<double> dpar4 = solid4.parameters();
	std::stringstream parameters4;
	for(unsigned int i=0; i<dpar4.size(); ++i) {
	  parameters4 << " dpar["<<i<<"]="<< dpar4[i]/10 << "cm ";
	}
	LogTrace("ME0GeometryBuilderFromDDD") << "Fourth level parameters: vector with size = "<<dpar4.size()<<" and elements "<<parameters4.str();
	// --------------------------------------------------------------------------------------------------------------------------------------------
	doEtaParts = fv.nextSibling();
	LogTrace("ME0GeometryBuilderFromDDD") << "doEtaPart = fv.nextSibling() = " << doEtaParts;
      }
      fv.parent(); // commented out in case only looping over sensitive volumes
      LogTrace("ME0GeometryBuilderFromDDD") << "went back to parent :: name = "<<fv.logicalPart().name().name()<<" will now ask for nextSibling";
      doLayers = fv.nextSibling();
      LogTrace("ME0GeometryBuilderFromDDD") << "doLayer = fv.nextSibling() = " << doLayers;
    }
    fv.parent(); // commented out in case only looping over sensitive volumes
    LogTrace("ME0GeometryBuilderFromDDD") << "went back to parent :: name = "<<fv.logicalPart().name().name()<<" will now ask for nextSibling";
    testChambers = fv.nextSibling();
    LogTrace("ME0GeometryBuilderFromDDD") << "doChamber = fv.nextSibling() = " << testChambers;
  }
  fv.parent();
  #endif

  // ==========================================
  // === Here the Real ME0 Geometry Builder ===
  // ==========================================
  bool doChambers = fv.firstChild();
  while (doChambers) {
    // to etapartitions and back again to pick up DetId
    fv.firstChild();
    fv.firstChild();
    MuonDDDNumbering mdddnum(muonConstants);
    ME0NumberingScheme me0Num(muonConstants);
    int rawId = me0Num.baseNumberToUnitNumber(mdddnum.geoHistoryToBaseNumber(fv.geoHistory()));
    ME0DetId detId = ME0DetId(rawId);
    ME0DetId detIdCh = detId.chamberId();
    fv.parent();
    fv.parent();

    // build chamber
    ME0Chamber *me0Chamber = buildChamber(fv, detIdCh);
    geometry->add(me0Chamber);

    // loop over layers of the chamber
    bool doLayers = fv.firstChild();
    while (doLayers) {
      // to etapartitions and back again to pick up DetId
      fv.firstChild();
      MuonDDDNumbering mdddnum(muonConstants);
      ME0NumberingScheme me0Num(muonConstants);
      int rawId = me0Num.baseNumberToUnitNumber(mdddnum.geoHistoryToBaseNumber(fv.geoHistory()));
      ME0DetId detId = ME0DetId(rawId);
      ME0DetId detIdLa = detId.layerId();
      fv.parent();

      // build layer
      ME0Layer *me0Layer = buildLayer(fv, detIdLa);
      me0Chamber->add(me0Layer);
      geometry->add(me0Layer);

      // loop over etapartitions of the layer
      bool doEtaParts = fv.firstChild();
      while (doEtaParts) {
	// pick up DetId
	MuonDDDNumbering mdddnum(muonConstants);
	ME0NumberingScheme me0Num(muonConstants);
	int rawId = me0Num.baseNumberToUnitNumber(mdddnum.geoHistoryToBaseNumber(fv.geoHistory()));
	ME0DetId detId = ME0DetId(rawId);

	// build etapartition
	ME0EtaPartition *etaPart = buildEtaPartition(fv, detId);
	me0Layer->add(etaPart);
	geometry->add(etaPart);

	doEtaParts = fv.nextSibling();
      }
      fv.parent();
      doLayers = fv.nextSibling();
    }
    fv.parent();
    doChambers = fv.nextSibling();
  }

  return geometry;
}

ME0Chamber* ME0GeometryBuilderFromDDD10EtaPart::buildChamber(DDFilteredView& fv, ME0DetId detId) const {
  LogTrace("ME0GeometryBuilderFromDDD") << "buildChamber "<<fv.logicalPart().name().name() <<" "<< detId <<std::endl;

  DDBooleanSolid solid = (DDBooleanSolid)(fv.logicalPart().solid());
  // std::vector<double> dpar = solid.solidA().parameters();
  std::vector<double> dpar = solid.parameters();
  double L  = dpar[0]/cm;// length is along local Y
  double T  = dpar[3]/cm;// thickness is long local Z
  double b  = dpar[4]/cm;// bottom width is along local X
  double B  = dpar[8]/cm;// top width is along local X
  // hardcoded :: double b = 21.9859, B = 52.7261, L = 87.1678, T = 12.9;

  #ifdef EDM_ML_DEBUG
  LogTrace("ME0GeometryBuilderFromDDD") << " name of logical part = "<<fv.logicalPart().name().name()<<std::endl;
  LogTrace("ME0GeometryBuilderFromDDD") << " dpar is vector with size = "<<dpar.size()<<std::endl;
  for(unsigned int i=0; i<dpar.size(); ++i) {
    LogTrace("ME0GeometryBuilderFromDDD") << " dpar ["<<i<<"] = "<< dpar[i]/10 << " cm "<<std::endl;
  }
  LogTrace("ME0GeometryBuilderFromDDD") << "size  b: "<< b << "cm, B: " << B << "cm,  L: " << L << "cm, T: " << T <<"cm "<<std::endl;
  #endif

  bool isOdd = false; // detId.chamber()%2;
  ME0BoundPlane surf(boundPlane(fv, new TrapezoidalPlaneBounds(b,B,L,T), isOdd ));
  ME0Chamber* chamber = new ME0Chamber(detId.chamberId(), surf);
  return chamber;
}

ME0Layer* ME0GeometryBuilderFromDDD10EtaPart::buildLayer(DDFilteredView& fv, ME0DetId detId) const {
  LogTrace("ME0GeometryBuilderFromDDD") << "buildLayer "<<fv.logicalPart().name().name() <<" "<< detId <<std::endl;

  DDBooleanSolid solid = (DDBooleanSolid)(fv.logicalPart().solid());
  // std::vector<double> dpar = solid.solidA().parameters();
  std::vector<double> dpar = solid.parameters();
  double L = dpar[0]/cm;// length is along local Y
  double t = dpar[3]/cm;// thickness is long local Z
  double b = dpar[4]/cm;// bottom width is along local X
  double B = dpar[8]/cm;// top width is along local X
  // dpar = solid.solidB().parameters();
  // dz += dpar[3]/cm;     // layer thickness --- to be checked !!! layer thickness should be same as eta part thickness
  // hardcoded :: double b = 21.9859, B = 52.7261, L = 87.1678, t = 0.4;

  #ifdef EDM_ML_DEBUG
  LogTrace("ME0GeometryBuilderFromDDD") << " name of logical part = "<<fv.logicalPart().name().name()<<std::endl;
  LogTrace("ME0GeometryBuilderFromDDD") << " dpar is vector with size = "<<dpar.size()<<std::endl;
  for(unsigned int i=0; i<dpar.size(); ++i) {
    LogTrace("ME0GeometryBuilderFromDDD") << " dpar ["<<i<<"] = "<< dpar[i]/10 << " cm "<<std::endl;
  }
  LogTrace("ME0GeometryBuilderFromDDD") << "size  b: "<< b << "cm, B: " << B << "cm,  L: " << L << "cm, t: " << t <<"cm "<<std::endl;
  #endif

  bool isOdd = false; // detId.chamber()%2;
  ME0BoundPlane surf(boundPlane(fv, new TrapezoidalPlaneBounds(b,B,L,t), isOdd ));
  ME0Layer* layer = new ME0Layer(detId.layerId(), surf);
  return layer;
}

ME0EtaPartition* ME0GeometryBuilderFromDDD10EtaPart::buildEtaPartition(DDFilteredView& fv, ME0DetId detId) const {
  LogTrace("ME0GeometryBuilderFromDDD") << "buildEtaPartition "<<fv.logicalPart().name().name() <<" "<< detId <<std::endl;

  // EtaPartition specific parameter (nstrips and npads)
  DDValue numbOfStrips("nStrips");
  DDValue numbOfPads("nPads");
  std::vector<const DDsvalues_type* > specs(fv.specifics());
  std::vector<const DDsvalues_type* >::iterator is = specs.begin();
  double nStrips = 0., nPads = 0.;
  for (;is != specs.end(); is++){
    if (DDfetch( *is, numbOfStrips)) nStrips = numbOfStrips.doubles()[0];
    if (DDfetch( *is, numbOfPads))   nPads = numbOfPads.doubles()[0];
  }
  LogTrace("ME0GeometryBuilderFromDDD")
    << ((nStrips == 0. ) ? ("No nStrips found!!") : ("Number of strips: " + boost::lexical_cast<std::string>(nStrips)));
  LogTrace("ME0GeometryBuilderFromDDD")
    << ((nPads == 0. ) ? ("No nPads found!!") : ("Number of pads: " + boost::lexical_cast<std::string>(nPads)));

  // EtaPartition specific parameter (size)
  std::vector<double> dpar = fv.logicalPart().solid().parameters();
  double b = dpar[4]/cm; // half bottom edge
  double B = dpar[8]/cm; // half top edge
  double L = dpar[0]/cm; // half apothem
  double t = dpar[3]/cm; // half thickness

  #ifdef EDM_ML_DEBUG
  LogTrace("ME0GeometryBuilderFromDDD") << " name of logical part = "<<fv.logicalPart().name().name()<<std::endl;
  LogTrace("ME0GeometryBuilderFromDDD") << " dpar is vector with size = "<<dpar.size()<<std::endl;
  for(unsigned int i=0; i<dpar.size(); ++i) {
    LogTrace("ME0GeometryBuilderFromDDD") << " dpar ["<<i<<"] = "<< dpar[i]/10 << " cm "<<std::endl;
  }
  LogTrace("ME0GeometryBuilderFromDDD") << "size  b: "<< b << "cm, B: " << B << "cm,  L: " << L << "cm, t: " << t <<"cm "<<std::endl;
  #endif

  std::vector<float> pars;
  pars.emplace_back(b);
  pars.emplace_back(B);
  pars.emplace_back(L);
  pars.emplace_back(nStrips);
  pars.emplace_back(nPads);

  bool isOdd = false; // detId.chamber()%2; // this gives the opportunity (in future) to change the face of the chamber (electronics facing IP or electronics away from IP)
  ME0BoundPlane surf(boundPlane(fv, new TrapezoidalPlaneBounds(b, B, L, t), isOdd ));
  std::string name = fv.logicalPart().name().name();
  ME0EtaPartitionSpecs* e_p_specs = new ME0EtaPartitionSpecs(GeomDetEnumerators::ME0, name, pars);

  ME0EtaPartition* etaPartition = new ME0EtaPartition(detId, surf, e_p_specs);
  return etaPartition;
}

ME0GeometryBuilderFromDDD10EtaPart::ME0BoundPlane
ME0GeometryBuilderFromDDD10EtaPart::boundPlane(const DDFilteredView& fv,
                                      Bounds* bounds, bool isOddChamber) const {
  // extract the position
  const DDTranslation & trans(fv.translation());
  const Surface::PositionType posResult(float(trans.x()/cm),
                                        float(trans.y()/cm),
                                        float(trans.z()/cm));

  // now the rotation
  //  DDRotationMatrix tmp = fv.rotation();
  // === DDD uses 'active' rotations - see CLHEP user guide ===
  //     ORCA uses 'passive' rotation.
  //     'active' and 'passive' rotations are inverse to each other
  //  DDRotationMatrix tmp = fv.rotation();
  DDRotationMatrix rotation = fv.rotation();//REMOVED .Inverse();
  DD3Vector x, y, z;
  rotation.GetComponents(x,y,z);
  // LogTrace("GEMGeometryBuilderFromDDD") << "translation: "<< fv.translation() << std::endl;
  // LogTrace("GEMGeometryBuilderFromDDD") << "rotation   : "<< fv.rotation() << std::endl;
  // LogTrace("GEMGeometryBuilderFromDDD") << "INVERSE rotation manually: \n"
  //        << x.X() << ", " << x.Y() << ", " << x.Z() << std::endl
  //        << y.X() << ", " << y.Y() << ", " << y.Z() << std::endl
  //        << z.X() << ", " << z.Y() << ", " << z.Z() << std::endl;

  Surface::RotationType rotResult(float(x.X()),float(x.Y()),float(x.Z()),
                                  float(y.X()),float(y.Y()),float(y.Z()),
                                  float(z.X()),float(z.Y()),float(z.Z()));

  //Change of axes for the forward
  Basic3DVector<float> newX(1.,0.,0.);
  Basic3DVector<float> newY(0.,0.,1.);
  Basic3DVector<float> newZ(0.,1.,0.);
  newY *= -1;

  rotResult.rotateAxes(newX, newY, newZ);

  return ME0BoundPlane( new BoundPlane( posResult, rotResult, bounds));
}