#ifndef HEADER_TYPES
#define HEADER_TYPES

#include <Eigen/Core>
#include <Eigen/StdVector>

typedef Eigen::Matrix3f Mat3f;
typedef Eigen::Matrix4f Mat4f;

typedef Eigen::Vector2f Vec2f;
typedef Eigen::Vector3f Vec3f;
typedef Eigen::Vector4f Vec4f;

typedef Eigen::Vector2i Vec2i;
typedef Eigen::Vector3i Vec3i;
typedef Eigen::Vector4i Vec4i;

typedef std::vector< int > Array1i;
typedef std::vector<float> Array1f;

typedef std::vector<Vec2i, Eigen::aligned_allocator<Vec2i> > Array2i;
typedef std::vector<Vec3i, Eigen::aligned_allocator<Vec3i> > Array3i;
typedef std::vector<Vec4i, Eigen::aligned_allocator<Vec4i> > Array4i;

typedef std::vector<Vec2f, Eigen::aligned_allocator<Vec2f> > Array2f;
typedef std::vector<Vec3f, Eigen::aligned_allocator<Vec3f> > Array3f;
typedef std::vector<Vec4f, Eigen::aligned_allocator<Vec4f> > Array4f;

#endif // HEADER_TYPES


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <map>
#include <string>
#include <iostream>
#include <random>
#include <Eigen/Geometry>
#include <time.h>
#include <chrono>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <sensor_msgs/JointState.h>
#include <baxter_core_msgs/JointCommand.h>
#include <control_msgs/JointTrajectoryControllerState.h>
#include <baxter_robot_controllers/baxter_robot_config.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <arc_utilities/eigen_helpers.hpp>

namespace baxter_robot_controllers
{
    class BaxterRobotPositionController
    {
    protected:

        std::vector<std::string> joint_names_;
        std::map<std::string, JointLimits> joint_limits_;
        std::map<std::string, PIDParams> joint_controller_params_;

        bool target_config_valid_;
        bool current_config_valid_;
        std::vector<double> target_config_;
        std::vector<double> current_config_;
        std::vector<double> current_velocities_;

        std::vector<double> config_error_integrals_;
        std::vector<double> last_config_errors_;

        ros::NodeHandle nh_;
        ros::Publisher status_pub_;
        ros::Publisher command_pub_;
        ros::Subscriber feedback_sub_;
        ros::Subscriber config_target_sub_;
        ros::ServiceServer abort_server_;

    public:

        BaxterRobotPositionController(ros::NodeHandle& nh,
                                      const std::string& position_command_topic,
                                      const std::string& config_feedback_topic,
                                      const std::string& velocity_command_topic,
                                      const std::string& status_topic,
                                      const std::string& abort_service,
                                      const std::map<std::string, JointLimits>& joint_limits,
                                      const std::map<std::string, PIDParams> joint_controller_params) : nh_(nh)
        {
            target_config_valid_ = false;
            current_config_valid_ = false;
            target_config_ = std::vector<double>();
            current_config_ = std::vector<double>();
            current_velocities_ = std::vector<double>();
            assert(SetsEqual(arc_helpers::GetKeys(joint_limits), arc_helpers::GetKeys(joint_controller_params)));
            joint_names_ = arc_helpers::GetKeys(joint_limits);
            config_error_integrals_ = std::vector<double>(joint_names_.size(), 0.0);
            last_config_errors_ = std::vector<double>(joint_names_.size(), 0.0);
            joint_limits_ = joint_limits;
            joint_controller_params_ = joint_controller_params;
            status_pub_ = nh_.advertise<control_msgs::JointTrajectoryControllerState>(status_topic, 1, false);
            command_pub_ = nh_.advertise<baxter_core_msgs::JointCommand>(velocity_command_topic, 1, false);
            feedback_sub_ = nh_.subscribe(config_feedback_topic, 1, &BaxterRobotPositionController::ConfigFeedbackCallback, this);
            config_target_sub_ = nh_.subscribe(position_command_topic, 1, &BaxterRobotPositionController::ConfigTargetCallback, this);
            abort_server_ = nh_.advertiseService(abort_service, &BaxterRobotPositionController::AbortCB, this);
        }

        void Loop(const double control_rate)
        {
            const double control_interval = 1.0 / control_rate;
            ros::Rate spin_rate(control_rate);
            uint8_t iteration_count = 0x00;
            while (ros::ok())
            {
                // Process callbacks
                ros::spinOnce();
                // Run controller
                if (iteration_count == 0x00)
                {
                    if (target_config_valid_ && current_config_valid_)
                    {
                        const std::vector<double> raw_config_correction = ComputeRawNextStep(current_config_, target_config_, control_interval);
                        const std::vector<double> real_config_correction = LimitCorrectionVelocities(raw_config_correction, joint_names_, joint_limits_);
                        CommandVelocities(real_config_correction);
                        PublishState(target_config_, real_config_correction, current_config_, current_velocities_);
                    }
                    else if (current_config_valid_)
                    {
                        //const std::vector<double> zero_velocity(joint_names_.size(), 0.0);
                        //CommandVelocities(zero_velocity);
                        //PublishState(current_config_, zero_velocity, current_config_, current_velocities_);
                        PublishState(current_config_, std::vector<double>(), current_config_, current_velocities_);
                    }
                    iteration_count = 0x00;
                }
                else
                {
                    iteration_count++;
                }
                // Spin
                spin_rate.sleep();
            }
        }

        inline void PublishState(const std::vector<double>& target_config, const std::vector<double>& target_velocities, const std::vector<double>& current_config, const std::vector<double>& current_velocities)
        {
            control_msgs::JointTrajectoryControllerState state_msg;
            state_msg.joint_names = joint_names_;
            state_msg.desired.positions = target_config;
            state_msg.desired.velocities = target_velocities;
            state_msg.actual.positions = current_config;
            state_msg.actual.velocities = current_velocities;
            state_msg.error.positions = EigenHelpers::Sub(target_config, current_config);
            state_msg.error.velocities = EigenHelpers::Sub(target_velocities, current_velocities);
            status_pub_.publish(state_msg);
        }

        inline bool AbortCB(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
        {
            UNUSED(req);
            UNUSED(res);
            ROS_INFO("Aborting config target");
            target_config_valid_ = false;
            config_error_integrals_ = std::vector<double>(joint_names_.size(), 0.0);
            last_config_errors_ = std::vector<double>(joint_names_.size(), 0.0);
            return true;
        }

        inline static double ClampValue(const double value, const double lower_limit, const double upper_limit)
        {
            return std::max(lower_limit, std::min(upper_limit, value));
        }

        inline std::vector<double> ComputeConfigError(const std::vector<double>& current_config, const std::vector<double>& target_config) const
        {
            const std::vector<double> config_error = EigenHelpers::Sub(target_config, current_config);
            return config_error;
        }

        inline std::vector<double> ComputeNewConfigErrorIntegral(const std::vector<double>& raw_config_error_integral, const std::vector<std::string>& joint_names, const std::map<std::string, PIDParams>& joint_params) const
        {
            assert(raw_config_error_integral.size() == joint_names.size());
            std::vector<double> limited_config_error_integral(raw_config_error_integral.size(), 0.0);
            for (size_t idx = 0; idx < limited_config_error_integral.size(); idx++)
            {
                const double joint_config_error_integral = raw_config_error_integral[idx];
                const std::string& joint_name = joint_names[idx];
                const auto found_itr = joint_params.find(joint_name);
                assert(found_itr != joint_params.end());
                const double i_clamp = found_itr->second.Iclamp();
                const double clamped_joint_config_error_integral = ClampValue(joint_config_error_integral, -i_clamp, i_clamp);
                limited_config_error_integral[idx] = clamped_joint_config_error_integral;
            }
            return limited_config_error_integral;
        }

        inline std::vector<double> ComputeRawNextStep(const std::vector<double>& current_config, const std::vector<double>& target_config, const double time_interval)
        {
            assert(joint_names_.size() == current_config.size());
            assert(joint_names_.size() == target_config.size());
            //std::cout << "Current config: " << PrettyPrint::PrettyPrint(current_config) << std::endl;
            //std::cout << "Target config: " << PrettyPrint::PrettyPrint(target_config) << std::endl;
            // Compute the pose error in our 'world frame'
            const std::vector<double> config_errors = ComputeConfigError(current_config, target_config);
            //std::cout << "Current errors: " << PrettyPrint::PrettyPrint(config_errors) << std::endl;
            // Compute the integral of pose error & update the stored value
            const std::vector<double> config_error_integrals_update = EigenHelpers::Add(EigenHelpers::Multiply(last_config_errors_, 0.5), EigenHelpers::Multiply(config_errors, 0.0));
            config_error_integrals_ = ComputeNewConfigErrorIntegral(EigenHelpers::Add(config_error_integrals_, config_error_integrals_update), joint_names_, joint_controller_params_);
            // Compute the derivative of pose error
            const std::vector<double> config_error_derivatives = EigenHelpers::Divide(EigenHelpers::Sub(config_errors, last_config_errors_), time_interval);
            // Update the stored pose error
            last_config_errors_ = config_errors;
            // Compute Feedback terms
            std::vector<double> feedback_terms(config_errors.size(), 0.0);
            for (size_t idx = 0; idx < feedback_terms.size(); idx++)
            {
                const double joint_config_error = config_errors[idx];
                const double joint_config_error_integral = config_error_integrals_[idx];
                const double joint_config_error_derivative = config_error_derivatives[idx];
                const std::string& joint_name = joint_names_[idx];
                const auto found_itr = joint_controller_params_.find(joint_name);
                assert(found_itr != joint_controller_params_.end());
                const PIDParams& params = found_itr->second;
                const double p_term = joint_config_error * params.Kp();
                const double i_term = joint_config_error_integral * params.Ki();
                const double d_term = joint_config_error_derivative * params.Kd();
                const double feedback_term = p_term + i_term + d_term;
                feedback_terms[idx] = feedback_term;
            }
            //std::cout << "Feedback command: " << PrettyPrint::PrettyPrint(feedback_terms) << std::endl;
            return feedback_terms;
        }

        inline std::vector<double> LimitCorrectionVelocities(const std::vector<double>& raw_velocities, const std::vector<std::string>& joint_names, const std::map<std::string, JointLimits>& limits) const
        {
            assert(raw_velocities.size() == joint_names.size());
            std::vector<double> limited_velocities(raw_velocities.size(), 0.0);
            for (size_t idx = 0; idx < limited_velocities.size(); idx++)
            {
                const double raw_velocity = raw_velocities[idx];
                const std::string& joint_name = joint_names[idx];
                const auto found_itr = limits.find(joint_name);
                assert(found_itr != limits.end());
                const double velocity_limit = found_itr->second.MaxVelocity();
                const double limited_velocity = ClampValue(raw_velocity, -velocity_limit, velocity_limit);
                limited_velocities[idx] = limited_velocity;
            }
            return limited_velocities;
        }

        inline void CommandVelocities(const std::vector<double>& velocities)
        {
            if (velocities.size() == joint_names_.size())
            {
                baxter_core_msgs::JointCommand command_msg;
                command_msg.mode = baxter_core_msgs::JointCommand::VELOCITY_MODE;
                command_msg.names = joint_names_;
                command_msg.command = velocities;
                command_pub_.publish(command_msg);
            }
        }

        inline bool IsSubset(const std::vector<std::string>& candidate_set, const std::vector<std::string>& candidate_subset) const
        {
            if (candidate_set.size() < candidate_subset.size())
            {
                return false;
            }
            std::map<std::string, uint8_t> names_map;
            for (size_t idx = 0; idx < candidate_set.size(); idx++)
            {
                const std::string& name = candidate_set[idx];
                names_map[name] = 0x01;
            }
            for (size_t idx = 0; idx < candidate_subset.size(); idx++)
            {
                const std::string& name = candidate_subset[idx];
                const auto found_itr = names_map.find(name);
                if (found_itr == names_map.end())
                {
                    return false;
                }
            }
            return true;
        }

        inline bool SetsEqual(const std::vector<std::string>& set1, const std::vector<std::string>& set2) const
        {
            if (IsSubset(set1, set2) && IsSubset(set2, set1))
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        inline void ConfigTargetCallback(baxter_core_msgs::JointCommand config_target)
        {
            if (config_target.mode == baxter_core_msgs::JointCommand::POSITION_MODE || config_target.mode == baxter_core_msgs::JointCommand::RAW_POSITION_MODE)
            {
                if (config_target.names.size() == config_target.command.size())
                {
                    // Push the command into a map
                    std::map<std::string, double> command_map;
                    for (size_t idx = 0; idx < config_target.names.size(); idx++)
                    {
                        const std::string& name = config_target.names[idx];
                        const double command = config_target.command[idx];
                        command_map[name] = command;
                    }
                    // Extract the joint commands in order
                    std::vector<double> target_config(joint_names_.size(), 0.0);
                    bool command_valid = true;
                    for (size_t idx = 0; idx < joint_names_.size(); idx++)
                    {
                        // Get the name of the joint
                        const std::string& joint_name = joint_names_[idx];
                        // Get the commanded value
                        const auto found_itr = command_map.find(joint_name);
                        if (found_itr != command_map.end())
                        {
                            const double position = found_itr->second;
                            // Get the limits for the joint
                            const auto limits_found_itr = joint_limits_.find(joint_name);
                            // If we have limits saved, limit the joint command
                            if (limits_found_itr != joint_limits_.end())
                            {
                                const std::pair<double, double> limits = limits_found_itr->second.PositionLimits();
                                const double lower_limit = limits.first;
                                const double upper_limit = limits.second;
                                const double limited_position = ClampValue(position, lower_limit, upper_limit);
                                target_config[idx] = limited_position;
                            }
                            // If we don't have limits saved, then we don't need to limit the joint value
                            else
                            {
                                target_config[idx] = position;
                            }
                        }
                        else
                        {
                            ROS_WARN("Invalid JointCommand: joint %s missing", joint_name.c_str());
                            command_valid = false;
                        }
                    }
                    if (command_valid == true)
                    {
                        target_config_ = target_config;
                        target_config_valid_ = true;
                    }
                }
                else
                {
                    ROS_WARN("Invalid JointCommand: %zu names, %zu commands", config_target.names.size(), config_target.command.size());
                    target_config_valid_ = false;
                }
            }
            else
            {
                ROS_WARN("JointCommand mode is not POSITION_MODE or RAW_POSITION_MODE");
                target_config_valid_ = false;
            }
        }

        inline void ConfigFeedbackCallback(sensor_msgs::JointState config_feedback)
        {
            if ((config_feedback.name.size() == config_feedback.position.size()) && (config_feedback.name.size() == config_feedback.velocity.size()) && IsSubset(config_feedback.name, joint_names_))
            {
                // Push the joint state into a map
                std::map<std::string, std::pair<double, double>> joint_state_map;
                for (size_t idx = 0; idx < config_feedback.name.size(); idx++)
                {
                    const std::string& name = config_feedback.name[idx];
                    const double position = config_feedback.position[idx];
                    const double velocity = config_feedback.velocity[idx];
                    joint_state_map[name] = std::make_pair(position, velocity);
                }
                // Extract the joint state in order
                std::vector<double> current_config(joint_names_.size(), 0.0);
                std::vector<double> current_velocities(joint_names_.size(), 0.0);
                bool config_valid = true;
                for (size_t idx = 0; idx < joint_names_.size(); idx++)
                {
                    const std::string& joint_name = joint_names_[idx];
                    const auto found_itr = joint_state_map.find(joint_name);
                    if (found_itr != joint_state_map.end())
                    {
                        const double position = found_itr->second.first;
                        const double velocity = found_itr->second.second;
                        current_config[idx] = position;
                        current_velocities[idx] = velocity;
                    }
                    else
                    {
                        ROS_WARN("Invalid JointState feedback: joint %s missing", joint_name.c_str());
                        config_valid = false;
                    }
                }
                if (config_valid == true)
                {
                    current_config_ = current_config;
                    current_velocities_ = current_velocities;
                    current_config_valid_ = true;
                }
            }
            else if (config_feedback.name.size() == 1 && config_feedback.position.size() == 1)
            {
                ;
            }
            else
            {
                ROS_WARN("Invalid JointState feedback: %zu names, %zu positions", config_feedback.name.size(), config_feedback.position.size());
                current_config_valid_ = false;
            }
        }
    };
}


#include <G4LogicalVolume.hh>
#include <G4PVPlacement.hh>
#include <G4Material.hh>
#include <G4Tubs.hh>
#include <G4Sphere.hh>
#include <G4Box.hh>
#include <G4SDManager.hh>
#include <G4ThreeVector.hh>

#include <G4VisAttributes.hh>

#include <G4UserLimits.hh>

/* #include <boost/foreach.hpp> */

#include "G4BeamTestTank.h"
#include "G4BeamTestSiSD.h"
/* #include "G4TankIceSD.h" */

//prevent gcc to make something stupid with pretended unused variables
#ifdef __GNUC__
#define ATTRIBUTE_UNUSED __attribute__((unused))
#else
#define ATTRIBUTE_UNUSED
#endif

G4BeamTestTank::G4BeamTestTank()
{
  // Get tank dimensions
  // tankThickness_ = 0.0*CLHEP::cm;
  tankThickness_ = 0.44 * 2.54 *CLHEP::cm;
  tankHeight_  = 76.83 * 2.54 * CLHEP::cm;
  innerRadius_ = 32 * 2.54 * CLHEP::cm;
  outerRadius_ = innerRadius_ + tankThickness_;

  // Get fill height
  fillHeight_ = 55.1 * 2.54 * CLHEP::cm;
  airHeight_ = tankHeight_ - fillHeight_;

  // Set DOM dimensions
  glassOuterRadius_ = 6.5 * 2.54 * CLHEP::cm; // 6.5" outer glass sphere radius
  glassThickness_   = 0.5 * 2.54 * CLHEP::cm; // 0.5" glass sphere thickness
}

G4BeamTestTank::~G4BeamTestTank()
{

}

G4VPhysicalVolume* G4BeamTestTank::InstallTank(G4VPhysicalVolume* mother, const G4ThreeVector& origin)
{
  // User limits (energy cutoffs)
  // Do not create photons or electrons below cherenkov threshold
  // See also corresponding UserSpecialCuts in Physicslist !!!!
  G4UserLimits* energyLimit = new G4UserLimits();
  energyLimit->SetUserMinEkine(2.26 * CLHEP::eV);  // Lower threshold of PMT - 550nm

  G4Material* plastic = G4Material::GetMaterial("Plastic");
  G4Tubs* solidTank = new G4Tubs(
          "solid_tank_",
          0.0 * CLHEP::m, outerRadius_, 0.5 * tankHeight_,
          0.0 * CLHEP::deg, 360.0 * CLHEP::deg
          );
  tankLog_ = new G4LogicalVolume(
          solidTank, plastic, "log_tank_", 0, 0, 0
          );

  // Define water volume
  G4Material* water = G4Material::GetMaterial("Water");
  G4Tubs* solidWater = new G4Tubs(
          "solid_water_",
          0.0 * CLHEP::m, innerRadius_, 0.5 * fillHeight_,
          0.0 * CLHEP::deg, 360.0 * CLHEP::deg
          );
  G4LogicalVolume* logWater =
      new G4LogicalVolume(solidWater, water, "log_water_", 0, 0, 0);
  G4ThreeVector physWaterPosition(
          0, 0, -0.5*tankHeight_ + tankThickness_ + 0.5*fillHeight_
          );
  G4VPhysicalVolume* physWater ATTRIBUTE_UNUSED =
      new G4PVPlacement(
              0, physWaterPosition, logWater, "water_", tankLog_, false, 0
              );

  // Define air volume
  G4Material* air = G4Material::GetMaterial("Air");
  G4Tubs* solidAir = new G4Tubs(
          "solid_air_",
          0.0 * CLHEP::m, innerRadius_, 0.5 * airHeight_,
          0.0 * CLHEP::deg, 360.0 * CLHEP::deg
          );
  G4LogicalVolume* logAir =
      new G4LogicalVolume(solidAir, air, "log_air_", 0, 0, 0);
  G4ThreeVector physAirPosition(0, 0,
          -0.5 * tankHeight_ + 0.5 * CLHEP::cm + fillHeight_ + 0.5 * airHeight_
          );
  G4VPhysicalVolume* physAir_UNUSED =
      new G4PVPlacement(
              0, physAirPosition, logAir, "air_", tankLog_, false, 0
              );

  // Define glass sphere & effective DOM material splitted in upper and lower part
  G4Material* glass        = G4Material::GetMaterial("Glass");
  G4Material* effectiveDOM = G4Material::GetMaterial("effectiveDOM");

  G4ThreeVector upperDOMpos(0, 0, -0.5 * airHeight_);
  G4ThreeVector lowerDOMpos(0, 0,  0.5 * fillHeight_);

  G4Sphere *upperglasssphere = new G4Sphere ("solid_dom_up_",
                                             0.0 * CLHEP::m, glassOuterRadius_,
                                             0.0 * CLHEP::deg, 360.0 * CLHEP::deg,
                                             0.0 * CLHEP::deg,  90.0 * CLHEP::deg);
  G4Sphere *lowerglasssphere = new G4Sphere ("solid_dom_lo_",
                                             0.0  * CLHEP::m, glassOuterRadius_,
                                             0.0  * CLHEP::deg, 360.0 * CLHEP::deg,
                                             90.0 * CLHEP::deg, 180.0 * CLHEP::deg);

  G4double domInnerRadius = glassOuterRadius_ - glassThickness_;
  G4Sphere *upperdomsphere = new G4Sphere ("solid_inside_dom_up_",
                                           0.0 * CLHEP::m, domInnerRadius,
                                           0.0 * CLHEP::deg, 360.0 * CLHEP::deg,
                                           0.0 * CLHEP::deg,  90.0 * CLHEP::deg);
  G4Sphere *lowerdomsphere = new G4Sphere ("solid_inside_dom_lo_",
                                           0.0  * CLHEP::m, domInnerRadius,
                                           0.0  * CLHEP::deg, 360.0 * CLHEP::deg,
                                           90.0 * CLHEP::deg, 180.0 * CLHEP::deg);

  G4LogicalVolume* logUpperGlass =
      new G4LogicalVolume(upperglasssphere, glass,
                          "log_dom_up_", 0, 0, 0);
  G4LogicalVolume* logLowerGlass =
      new G4LogicalVolume(lowerglasssphere, glass,
                          "log_dom_lo_", 0, 0, 0);
  G4LogicalVolume* logUpperDOM =
      new G4LogicalVolume(upperdomsphere, effectiveDOM,
                          "log_inside_dom_up_", 0, 0, 0);
  G4LogicalVolume* logLowerDOM =
      new G4LogicalVolume(lowerdomsphere, effectiveDOM,
                          "log_inside_dom_lo_", 0, 0, 0);
  G4VPhysicalVolume* physUpperGlass ATTRIBUTE_UNUSED =
      new G4PVPlacement(0, upperDOMpos, logUpperGlass,
                        "dom_up_", logAir, false, 0);
  G4VPhysicalVolume* physLowerGlass ATTRIBUTE_UNUSED =
      new G4PVPlacement(0, lowerDOMpos, logLowerGlass,
                        "dom_lo_", logWater, false, 0);
  G4VPhysicalVolume* physUpperDOM ATTRIBUTE_UNUSED =
      new G4PVPlacement(0, G4ThreeVector(0,0,0), logUpperDOM,
                        "inside_dom_up_", logUpperGlass, false, 0);
  G4VPhysicalVolume* physLowerDOM ATTRIBUTE_UNUSED =
      new G4PVPlacement(0, G4ThreeVector(0,0,0), logLowerDOM,
                        "inside_dom_lo_", logLowerGlass, false, 0);

  // apply energy limits
  logUpperGlass->SetUserLimits(energyLimit);
  logLowerGlass->SetUserLimits(energyLimit);
  logUpperDOM->SetUserLimits(energyLimit);
  logLowerDOM->SetUserLimits(energyLimit);
  // }

  // Define sensitive detector
  G4SDManager* sdManager = G4SDManager::GetSDMpointer();
  iceSD_ = new G4BeamTestSiSD("ice_SD_", "HitsCollection");
  sdManager->AddNewDetector(iceSD_);
  // logLowerDOM->SetSensitiveDetector(iceSD_);
  logLowerGlass->SetSensitiveDetector(iceSD_);

  // Instantiation of a set of visualization attributes with red colour
  G4VisAttributes * tankVisAtt = new G4VisAttributes(G4Colour(1,0,0));
  // Assignment of the visualization attributes to the logical volume
  tankLog_->SetVisAttributes(tankVisAtt);

  // G4ThreeVector tankPos = position_ - origin - mother->GetTranslation();
  G4ThreeVector tankPos = origin;
  G4VPhysicalVolume* tankPhys = new G4PVPlacement(
          0, tankPos, tankLog_, "tank_", mother->GetLogicalVolume(), false, 0
          );

  // apply energy limits
  tankLog_->SetUserLimits(energyLimit);
  logAir->SetUserLimits(energyLimit);
  logWater->SetUserLimits(energyLimit);

  return tankPhys;
}


/*Copyright (c) 2020 James Gayvert

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/*
 * AOCAP.cpp
 */
#include "BasisSet.h"
#include <numgrid.h>
#include "utils.h"
#include "gto_ordering.h"
#include "AOCAP.h"
#include <chrono>
#include <ctime>
#include <thread>
#include <math.h>
#include <vector>
#include <iostream>
#include "opencap_exception.h"
#include <Eigen/Dense>

AOCAP::AOCAP(std::vector<Atom> geometry,std::map<std::string, std::string> params)
{
	verify_cap_parameters(params);
	double capx,capy,capz,rcut,radial,angpts;
	std::stringstream capxss(params["cap_x"]);
	std::stringstream capyss(params["cap_y"]);
	std::stringstream capzss(params["cap_z"]);
	std::stringstream rcutss(params["r_cut"]);
	std::stringstream radialss(params["radial_precision"]);
	std::stringstream angularss(params["angular_points"]);
	capxss >> capx;
	capyss >> capy;
	capzss >> capz;
	rcutss >> rcut;
	radialss >> radial;
	angularss >> angpts;
	//now fill class members
	cap_type = params["cap_type"];
	cap_x = capx;
	cap_y= capy;
	cap_z = capz;
	r_cut = rcut;
	radial_precision = pow(10,-1.0*radial);
	angular_points = angpts;
	atoms = geometry;
}

double AOCAP::eval_voronoi_cap(double x, double y, double z)
{
    double atom_distances[atoms.size()];
    double r_closest=1000.0;
    //find r closest and fill up our distances array
    for(size_t j=0;j<atoms.size();j++)
    {
        if(atoms[j].Z!=0)
        {
			  double dist_x= (x-atoms[j].coords[0]) * (x-atoms[j].coords[0]);
			  double dist_y= (y-atoms[j].coords[1]) * (y-atoms[j].coords[1]);
			  double dist_z= (z-atoms[j].coords[2]) * (z-atoms[j].coords[2]);
			  double dist=sqrt(dist_x+dist_y+dist_z);
			  if(dist<r_closest)
				 r_closest=dist;
			  atom_distances[j]=dist;
        }
    }
    double weights[atoms.size()];
    for(size_t j=0;j<atoms.size();j++)
    {
  	  if(atoms[j].Z!=0)
  		  weights[j]=1/pow((pow(atom_distances[j],2.0)-pow(r_closest,2.0) +1),2.0);
  	  else
  		  weights[j]=0;
    }
    double numerator=0.0;
    double denominator=0.0;
    for(size_t j=0;j<atoms.size();j++)
    {
  	  numerator+=atom_distances[j]*atom_distances[j]*weights[j];
  	  denominator+=weights[j];
    }
    double r=sqrt(numerator/denominator);
    if(r<r_cut)
  	  return 0;
    else
  	  return (r-r_cut)*(r-r_cut);
}

double AOCAP::eval_box_cap(double x, double y, double z)
{
    double result = 0;
    if(abs(x)>cap_x)
	 result += (abs(x)-cap_x) * (abs(x)-cap_x);
    if(abs(y)>cap_y)
   	 result += (abs(y)-cap_y) * (abs(y)-cap_y);
    if(abs(z)>cap_z)
   	 result += (abs(z)-cap_z) * (abs(z)-cap_z);
    return result;
}

double AOCAP::eval_pot(double x, double y, double z)
{
	if(compare_strings(cap_type, "box"))
		return eval_box_cap(x,y,z);
	else if (compare_strings(cap_type,"voronoi"))
		return eval_voronoi_cap(x,y,z);
	return 0;
}

void AOCAP::compute_ao_cap_mat(Eigen::MatrixXd &cap_mat, BasisSet bs)
{
	double x_coords_bohr[atoms.size()];
	double y_coords_bohr[atoms.size()];
	double z_coords_bohr[atoms.size()];
	int nuc_charges[atoms.size()];
	for(size_t i=0;i<atoms.size();i++)
	{
		x_coords_bohr[i]=atoms[i].coords[0];
		y_coords_bohr[i]=atoms[i].coords[1];
		z_coords_bohr[i]=atoms[i].coords[2];
		nuc_charges[i]=atoms[i].Z;
		if (atoms[i].Z==0)
			nuc_charges[i]=1; //choose bragg radius for H for ghost atoms
	}
    //double radial_precision = radial_precision;
    int min_num_angular_points = angular_points;
    int max_num_angular_points = angular_points;
	for(size_t i=0;i<atoms.size();i++)
	{
		//allocate and create grid
		context_t *context = numgrid_new_atom_grid(radial_precision,
		                                 min_num_angular_points,
		                                 max_num_angular_points,
		                                 nuc_charges[i],
		                                 bs.alpha_max(atoms[i]),
		                                 bs.max_L(),
		                                 &bs.alpha_min(atoms[i])[0]);
		int num_points = numgrid_get_num_grid_points(context);
        double *grid_x_bohr = new double[num_points];
        double *grid_y_bohr = new double[num_points];
        double *grid_z_bohr = new double[num_points];
        double *grid_w = new double[num_points];
        numgrid_get_grid(  context,
                           atoms.size(),
                           i,
                           x_coords_bohr,
                           y_coords_bohr,
                           z_coords_bohr,
                           nuc_charges,
                           grid_x_bohr,
                           grid_y_bohr,
                           grid_z_bohr,
                           grid_w);
		evaluate_grid_on_atom(cap_mat,bs,grid_x_bohr,grid_y_bohr,grid_z_bohr,grid_w,num_points);
	}
}

void AOCAP::evaluate_grid_on_atom(Eigen::MatrixXd &cap_mat,BasisSet bs,double* grid_x_bohr,
		double *grid_y_bohr,double *grid_z_bohr,double *grid_w,int num_points)
{
	//pre-calculate cap matrix on grid
	std::vector<float> cap_values (num_points);
	#pragma omp parallel for
	for (int i=0;i<num_points;i++)
		cap_values[i]= eval_pot(grid_x_bohr[i],grid_y_bohr[i],grid_z_bohr[i]);
	std::vector<std::vector<float>> bf_values;
	//pre-calculate basis functions on grid
	for(size_t i=0;i<bs.basis.size();i++)
	{
		Shell my_shell = bs.basis[i];
		std::vector<std::array<size_t,3>> order = opencap_carts_ordering(my_shell.l);
		for(size_t j=0;j<my_shell.num_carts();j++)
		{
			std::vector<float> vec(num_points);
			std::array<size_t,3> cart = order[j];
			#pragma omp parallel for
			for (int k=0;k<num_points;k++)
				vec[k]= my_shell.evaluate(grid_x_bohr[k],grid_y_bohr[k],grid_z_bohr[k],cart[0],cart[1],cart[2]);
			bf_values.push_back(vec);
		}
	}
    //now lets evaluate
	for (size_t i=0;i<bs.num_carts();i++)
	{
		for(size_t j=i;j<bs.num_carts();j++)
		{
			for(int k=0;k<num_points;k++)
			{
				cap_mat(i,j)+=grid_w[k]*cap_values[k]*bf_values[i][k]*bf_values[j][k];
				if (j!=i)
					cap_mat(j,i)+=grid_w[k]*cap_values[k]*bf_values[i][k]*bf_values[j][k];
			}
		}
	}
}

void AOCAP::verify_cap_parameters(std::map<std::string,std::string> &parameters)
{
	std::vector<std::string> missing_keys;
	if(parameters.find("cap_type")==parameters.end())
		opencap_throw("Error: Missing cap_type keyword.");
	if(compare_strings(parameters["cap_type"],"box"))
	{
		if(parameters.find("cap_x")==parameters.end())
			missing_keys.push_back("cap_x");
		if(parameters.find("cap_y")==parameters.end())
			missing_keys.push_back("cap_y");
		if (parameters.find("cap_z")==parameters.end())
			missing_keys.push_back("cap_z");
	}
	else if (compare_strings(parameters["cap_type"],"voronoi"))
	{
		if(parameters.find("r_cut")==parameters.end())
			missing_keys.push_back("r_cut");
	}
	else
		opencap_throw("Error: only box and voronoi CAPs supported.");
	if(missing_keys.size()!=0)
	{
		std::string error_str = "Missing CAP keywords: ";
		for (auto key: missing_keys)
			error_str+=key+" ";
		opencap_throw(error_str);
	}
	std::map<std::string, std::string> defaults = {{"radial_precision", "14"}, {"angular_points", "590"}};
	for (const auto &pair:defaults)
	{
		if(parameters.find(pair.first)==parameters.end())
			parameters[pair.first]=pair.second;
	}
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief test_scaled_conversion.cpp

\details
Test unit scaling

Output:
@verbatim
@endverbatim
**/

#include <boost/units/quantity.hpp>
#include <boost/units/conversion.hpp>
#include <boost/units/unit.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/scaled_base_unit.hpp>
#include <boost/units/scale.hpp>
#include <boost/units/base_dimension.hpp>

#include <boost/test/minimal.hpp>

#define BOOST_UNITS_CHECK_CLOSE(a, b) (BOOST_CHECK((std::abs((a) - (b)) < .0000001)))

namespace bu = boost::units;

struct base_dimension1 : bu::base_dimension<base_dimension1, 1> {};

struct base_unit1 : bu::base_unit<base_unit1, base_dimension1::dimension_type, 1> {};
struct base_unit2 : bu::base_unit<base_unit2, base_dimension1::dimension_type, 2> {};
struct base_unit3 : bu::base_unit<base_unit3, base_dimension1::dimension_type, 3> {};

typedef bu::scaled_base_unit<base_unit2, bu::scale<10, bu::static_rational<3> > > scaled_base_unit2;

BOOST_UNITS_DEFINE_CONVERSION_FACTOR(base_unit1, scaled_base_unit2, double, 5);
BOOST_UNITS_DEFINE_CONVERSION_FACTOR(scaled_base_unit2, base_unit3, double, 3);

int test_main(int,char *[])
{
    BOOST_UNITS_CHECK_CLOSE(bu::conversion_factor(base_unit1::unit_type(), base_unit2::unit_type()), 5000);
    BOOST_UNITS_CHECK_CLOSE(bu::conversion_factor(base_unit2::unit_type(), base_unit3::unit_type()), 0.003);
    BOOST_UNITS_CHECK_CLOSE(bu::conversion_factor(scaled_base_unit2::unit_type(), base_unit2::unit_type()), 1000);

    return(0);
}


/* boost random/uniform_smallint.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_smallint.hpp,v 1.20.2.1 2003/03/17 01:39:15 beman_dawes Exp $
 *
 * Revision history
 *  2001-04-08  added min<max assertion (N. Becker)
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_SMALLINT_HPP
#define BOOST_RANDOM_UNIFORM_SMALLINT_HPP

#include <cassert>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/detail/workaround.hpp>
#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#include <boost/type_traits/is_float.hpp>
#endif

namespace boost {

// uniform integer distribution on a small range [min, max]

namespace detail {

template <class InputStream, class UniformInt, class Impl>
InputStream& extract_uniform_int(InputStream& is, UniformInt& ud, Impl& impl)
{
    typename UniformInt::result_type min, max;
    is >> std::ws >> min >> std::ws >> max;
    impl.set(min, max);
    return is;
}

template<class UniformRandomNumberGenerator, class IntType>
struct uniform_smallint_integer
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;

  uniform_smallint_integer(base_type & rng, IntType min, IntType max)
    : _rng(&rng)
  { set(min, max); }

  void set(result_type min, result_type max);

  result_type min() const { return _min; }
  result_type max() const { return _max; }
  base_type& base() const { return *_rng; }

  result_type operator()()
  {
    // we must not use the low bits here, because LCGs get very bad then
    return (((*_rng)() - _rng->min()) / _factor) % _range + _min;
  }

private:
  typedef typename base_type::result_type base_result;
  base_type * _rng;
  IntType _min, _max;
  base_result _range;
  int _factor;
};

template<class UniformRandomNumberGenerator, class IntType>
void uniform_smallint_integer<UniformRandomNumberGenerator, IntType>::
set(result_type min, result_type max)
{
  _min = min;
  _max = max;
  assert(min < max);

  _range = static_cast<base_result>(_max-_min)+1;
  _factor = 1;

  // LCGs get bad when only taking the low bits.
  // (probably put this logic into a partial template specialization)
  // Check how many low bits we can ignore before we get too much
  // quantization error.
  base_result r_base = _rng->max() - _rng->min();
  if(r_base == std::numeric_limits<base_result>::max()) {
    _factor = 2;
    r_base /= 2;
  }
  r_base += 1;
  if(r_base % _range == 0) {
    // No quantization effects, good
    _factor = r_base / _range;
  } else {
    // carefully avoid overflow; pessimizing heree
    for( ; r_base/_range/32 >= _range; _factor *= 2)
      r_base /= 2;
  }
}

template<class UniformRandomNumberGenerator, class IntType>
class uniform_smallint_float
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;

  uniform_smallint_float(base_type & rng, IntType min, IntType max)
    : _rng(rng)
  {
    // MSVC fails BOOST_STATIC_ASSERT with std::numeric_limits at class scope
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(std::numeric_limits<IntType>::is_integer);
    BOOST_STATIC_ASSERT(!std::numeric_limits<typename base_type::result_type>::is_integer);
#endif

    assert(min < max);
  }

  void set(result_type min, result_type max)
  {
    _min = min;
    _max = max;
    _range = static_cast<base_result>(_max-_min)+1;
  }

  result_type min() const { return _min; }
  result_type max() const { return _max; }
  base_type& base() const { return _rng.base(); }

  result_type operator()()
  {
    return static_cast<IntType>(_rng() * _range) + _min;
  }

private:
  typedef typename base_type::result_type base_result;
  uniform_01<base_type> _rng;
  IntType _min, _max;
  base_result _range;
};

// simulate partial specialization
template<bool is_integer>
struct uniform_smallint;

template<>
struct uniform_smallint<true>
{
  template<class UniformRandomNumberGenerator, class IntType>
  struct impl
  {
    typedef uniform_smallint_integer<UniformRandomNumberGenerator, IntType> type;
  };
};

template<>
struct uniform_smallint<false>
{
  template<class UniformRandomNumberGenerator, class IntType>
  struct impl
  {
    typedef uniform_smallint_float<UniformRandomNumberGenerator, IntType> type;
  };
};

} // namespace detail

template<class UniformRandomNumberGenerator, class IntType = int>
class uniform_smallint
{
private:
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
  typedef typename detail::uniform_smallint<std::numeric_limits<typename UniformRandomNumberGenerator::result_type>::is_integer>::BOOST_NESTED_TEMPLATE impl<UniformRandomNumberGenerator, IntType>::type impl_type;
#elif BOOST_WORKAROUND( __BORLANDC__, BOOST_TESTED_AT(0x0570) )
  typedef typename detail::uniform_smallint< boost::is_float<typename UniformRandomNumberGenerator::result_type>::value == false >::BOOST_NESTED_TEMPLATE impl<UniformRandomNumberGenerator, IntType>::type impl_type;
#else
  BOOST_STATIC_CONSTANT(bool, base_float = (boost::is_float<typename UniformRandomNumberGenerator::result_type>::value == false));
  typedef typename detail::uniform_smallint<base_float>::BOOST_NESTED_TEMPLATE impl<UniformRandomNumberGenerator, IntType>::type impl_type;
#endif

public:
  typedef uniform_smallint<UniformRandomNumberGenerator, IntType> adaptor_type;
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

  explicit uniform_smallint(base_type & rng, IntType min = 0, IntType max = 9)
    : _impl(rng, min, max)
  {
    // MSVC fails BOOST_STATIC_ASSERT with std::numeric_limits at class scope
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(std::numeric_limits<IntType>::is_integer);
#endif
  }

  result_type min() const { return _impl.min(); }
  result_type max() const { return _impl.max(); }
  adaptor_type& adaptor() { return *this; }
  base_type& base() const { return _impl.base(); }
  void reset() { }

  result_type operator()() { return _impl(); }

  // deprecated
#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const uniform_smallint& x, const uniform_smallint& y)
  { return x.min() == y.min() && x.max() == y.max() && x.base() == y.base(); }

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const uniform_smallint& ud)
  {
    os << ud.min() << " " << ud.max();
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, uniform_smallint& ud)
  {
# if BOOST_WORKAROUND(_MSC_FULL_VER, BOOST_TESTED_AT(13102292)) && BOOST_MSVC > 1300
      return detail::extract_uniform_int(is, ud, ud._impl);
# else
    IntType min, max;
    is >> std::ws >> min >> std::ws >> max;
    ud._impl.set(min, max);
    return is;
# endif
  }
#endif

#else
  // Use a member function
  bool operator==(const uniform_smallint& rhs) const
  { return min() == rhs.min() && max() == rhs.max() && base() == rhs.base();  }
#endif

private:
  impl_type _impl;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UniformRandomNumberGenerator, class IntType>
const bool uniform_smallint<UniformRandomNumberGenerator, IntType>::has_fixed_range;
#endif

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_SMALLINT_HPP


/* Copyright © 2019 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at
 * https://opensource.org/licenses/BSD-3-Clause
 */

#define BOOST_TEST_MODULE test_od_serialization

#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#include <cstdio>
#include <ml/neural_net/model_spec.hpp>
#include <toolkits/coreml_export/mlmodel_include.hpp>
#include <toolkits/object_detection/od_serialization.hpp>

namespace turi {
namespace object_detection {
namespace {

const std::vector<std::pair<float, float>> anchor_boxes = {
    {1.f, 2.f}, {1.f, 1.f},  {2.f, 1.f},   {2.f, 4.f},   {2.f, 2.f},
    {4.f, 2.f}, {4.f, 8.f},  {4.f, 4.f},   {8.f, 4.f},   {8.f, 16.f},
    {8.f, 8.f}, {16.f, 8.f}, {16.f, 32.f}, {16.f, 16.f}, {32.f, 16.f},
};

BOOST_AUTO_TEST_CASE(test_init_darknet_yolo) {
  neural_net::model_spec nn_spec;
  const size_t num_classes = 10;
  init_darknet_yolo(nn_spec, num_classes, anchor_boxes);

  const CoreML::Specification::NeuralNetwork& nn = nn_spec.get_coreml_spec();
  TS_ASSERT_EQUALS(nn.layers_size(), 25);

  int layer_num = 0;
  int num_features = 3;
  const std::map<int, int> layer_num_to_channels = {
      {0, 16},  {1, 32},  {2, 64},   {3, 128},
      {4, 256}, {5, 512}, {6, 1024}, {7, 1024}};
  for (auto const& x : layer_num_to_channels) {
    const auto& convlayer_ = nn.layers(layer_num);
    TS_ASSERT(convlayer_.has_convolution());
    TS_ASSERT_EQUALS(convlayer_.name(),
                     "conv" + std::to_string(x.first) + "_fwd");
    TS_ASSERT_EQUALS(convlayer_.convolution().outputchannels(), x.second);
    TS_ASSERT_EQUALS(convlayer_.convolution().kernelchannels(), num_features);
    TS_ASSERT_EQUALS(convlayer_.convolution().stride(0), 1);
    TS_ASSERT_EQUALS(convlayer_.convolution().stride(1), 1);
    TS_ASSERT_EQUALS(convlayer_.convolution().kernelsize(0), 3);
    TS_ASSERT_EQUALS(convlayer_.convolution().kernelsize(1), 3);

    const auto& batchnormlayer_ = nn.layers(layer_num + 1);
    TS_ASSERT(batchnormlayer_.has_batchnorm());
    TS_ASSERT_EQUALS(batchnormlayer_.name(),
                     "batchnorm" + std::to_string(x.first) + "_fwd");
    TS_ASSERT_EQUALS(batchnormlayer_.batchnorm().channels(), x.second);
    TS_ASSERT_EQUALS(batchnormlayer_.batchnorm().epsilon(), 0.00001f);

    const auto& relulayer_ = nn.layers(layer_num + 2);
    TS_ASSERT(relulayer_.has_activation());
    TS_ASSERT_EQUALS(relulayer_.name(),
                     "leakyrelu" + std::to_string(x.first) + "_fwd");
    TS_ASSERT_EQUALS(relulayer_.activation().leakyrelu().alpha(), 0.1f);

    layer_num = layer_num + 3;
    num_features = x.second;
  }
}

BOOST_AUTO_TEST_CASE(test_save_load) {
  // Create Test Model
  neural_net::model_spec nn_spec_1;
  std::map<std::string, variant_type> state1 = {
      {"num_classes", 10}, {"model", "darknet_yolo"}, {"max_iterations", 5}};
  init_darknet_yolo(nn_spec_1,
                    variant_get_value<size_t>(state1.at("num_classes")),
                    anchor_boxes);

  // Save it
  dir_archive archive_write;
  archive_write.open_directory_for_write("serialized_save_load_tests");
  turi::oarchive oarc(archive_write);
  _save_impl(oarc, nn_spec_1, state1);
  archive_write.close();

  // Load it
  dir_archive archive_read;
  archive_read.open_directory_for_read("serialized_save_load_tests");
  turi::iarchive iarc(archive_read);
  size_t version = 1;
  neural_net::model_spec nn_spec_2;
  std::map<std::string, variant_type> state2 = {
      {"num_classes", 10}, {"model", "darknet_yolo"}, {"max_iterations", 5}};
  _load_version(iarc, version, nn_spec_2, state2, anchor_boxes);
  archive_read.close();

  // Compare saved and loaded models
  const CoreML::Specification::NeuralNetwork& nn_saved =
      nn_spec_1.get_coreml_spec();
  const CoreML::Specification::NeuralNetwork& nn_loaded =
      nn_spec_2.get_coreml_spec();
  TS_ASSERT(nn_saved.SerializeAsString() == nn_loaded.SerializeAsString());
}

}  // namespace
}  // namespace object_detection
}  // namespace turi


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2013 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2013 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <iomanip>
#include <string>

// Instead of having a separate (and nearly similar) unit test to test multipolygons,
// we now include them here and compile them by default. Only undefining the next line
// will avoid testing multi-geometries
#define BOOST_GEOMETRY_UNIT_TEST_MULTI

#include <boost/variant/variant.hpp>

#include <geometry_test_common.hpp>

// The include to test
#include <boost/geometry/algorithms/remove_spikes.hpp>

// Helper includes
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/perimeter.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/strategies/strategies.hpp>

#if defined(BOOST_GEOMETRY_UNIT_TEST_MULTI)

#  include <boost/geometry/multi/algorithms/remove_spikes.hpp>

#  include <boost/geometry/multi/algorithms/area.hpp>
#  include <boost/geometry/multi/algorithms/correct.hpp>
#  include <boost/geometry/multi/algorithms/perimeter.hpp>
#  include <boost/geometry/multi/geometries/multi_polygon.hpp>
#  include <boost/geometry/multi/io/wkt/wkt.hpp>
#endif

#if defined(TEST_WITH_SVG)
#  include <boost/geometry/io/svg/svg_mapper.hpp>
#endif

template <typename Geometry>
inline void test_remove_spikes(std::string const& /*id*/,
            Geometry& geometry,
            double expected_area, double expected_perimeter)
{
    bg::remove_spikes(geometry);

    double detected_area = bg::area(geometry);
    double detected_perimeter = bg::perimeter(geometry);

    BOOST_CHECK_CLOSE(detected_area, expected_area, 0.01);
    BOOST_CHECK_CLOSE(detected_perimeter, expected_perimeter, 0.01);
}

template <typename Geometry>
void test_geometry(std::string const& id, std::string const& wkt,
            double expected_area, double expected_perimeter)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    bg::correct(geometry);
    boost::variant<Geometry> v(geometry);

#if defined(TEST_WITH_SVG)
    std::ostringstream filename;
    filename << "remove_spikes_" << id;
    if (! bg::closure<Geometry>::value)
    {
        filename << "_open";
    }
    filename << ".svg";
    std::ofstream svg(filename.str().c_str());

    bg::svg_mapper<typename bg::point_type<Geometry>::type> mapper(svg, 500, 500);
    mapper.add(geometry);
    mapper.map(geometry, "fill-opacity:0.3;opacity:0.6;fill:rgb(51,51,153);stroke:rgb(0,0,255);stroke-width:2");
#endif

    test_remove_spikes(id, geometry, expected_area, expected_perimeter);
    test_remove_spikes(id, v, expected_area, expected_perimeter);

#if defined(TEST_WITH_SVG)
    mapper.map(geometry, "opacity:0.6;fill:none;stroke:rgb(255,0,0);stroke-width:3");
#endif
}

template <typename P, bool Clockwise, bool Closed>
void test_polygons()
{
    typedef bg::model::ring<P, Clockwise, Closed> ring;
    typedef bg::model::polygon<P, Clockwise, Closed> polygon;

    test_geometry<ring>("box",
            "POLYGON((0 0,0 4,4 4,4 0,0 0))",
            16, 16);
    test_geometry<polygon>("box",
            "POLYGON((0 0,0 4,4 4,4 0,0 0))",
            16, 16);
    test_geometry<polygon>("box2",
            "POLYGON((0 0,0 2,0 4,2 4,4 4,4 2,4 0,2 0,0 0))",
            16, 16);
    test_geometry<polygon>("spike_right",
            "POLYGON((0 0,0 4,4 4,4 2,6 2,4 2,4 0,0 0))",
            16, 16);
    test_geometry<polygon>("spike_at_corner",
            "POLYGON((0 0,0 4,6 4,4 4,4 0,0 0))",
            16, 16);
    test_geometry<polygon>("spike_at_first",
            "POLYGON((0 0,-1 3,0 0,0 4,4 4,4 0,0 0))",
            16, 16);
    test_geometry<polygon>("spike_at_last",
            "POLYGON((0 0,0 4,4 4,4 0,6 0,0 0))",
            16, 16);
    test_geometry<polygon>("spike_at_closing",
            "POLYGON((-1 0,0 0,0 4,4 4,4 0,0 0,-1 0))",
            16, 16);
    test_geometry<polygon>("double_spike",
            "POLYGON((0 0,0 4,4 4,4 2,6 2,5 2,4 2,4 0,0 0))",
            16, 16);
    test_geometry<polygon>("three_double_spike",
            "POLYGON((0 0,0 4,4 4,4 2,6 2,5 2,4.5 2,4 2,4 0,0 0))",
            16, 16);
    test_geometry<polygon>("spike_with_corner",
            "POLYGON((0 0,0 4,4 4,4 2,6 2,6 4,6 2,4 2,4 0,0 0))",
            16, 16);

    test_geometry<polygon>("triangle0",
            "POLYGON((0 0,0 4,2 0,4 0,0 0))",
            4, 6 + sqrt(20.0));
    test_geometry<polygon>("triangle1",
            "POLYGON((0 4,2 0,4 0,0 0,0 4))",
            4, 6 + sqrt(20.0));
    test_geometry<polygon>("triangle2",
            "POLYGON((2 0,4 0,0 0,0 4,2 0))",
            4, 6 + sqrt(20.0));
    test_geometry<polygon>("triangle3",
            "POLYGON((4 0,0 0,0 4,2 0,4 0))",
            4, 6 + sqrt(20.0));

    test_geometry<polygon>("only_spike1",
            "POLYGON((0 0,2 2,0 0))",
            0, 0);
    test_geometry<polygon>("only_spike2",
            "POLYGON((0 0,2 2,4 4,2 2,0 0))",
            0, 0);
    test_geometry<polygon>("only_spike3",
            "POLYGON((0 0,2 2,4 4,0 0))",
            0, 0);
    test_geometry<polygon>("only_spike4",
            "POLYGON((0 0,4 4,2 2,0 0))",
            0, 0);
}

template <typename P, bool Clockwise, bool Closed>
void test_multi_polygons()
{
    typedef bg::model::polygon<P, Clockwise, Closed> polygon;
    typedef bg::model::multi_polygon<polygon> multi_polygon;

    test_geometry<multi_polygon>("multi_spike_with_corner",
            "MULTIPOLYGON(((0 0,0 4,4 4,4 2,6 2,6 4,6 2,4 2,4 0,0 0)))",
            16, 16);
}

template <typename P, bool Clockwise, bool Closed>
void test_all()
{
    test_polygons<P, Clockwise, Closed>();
    test_multi_polygons<P, Clockwise, Closed>();
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<double>, true, true>();
    test_all<bg::model::d2::point_xy<double>, true, false>();
    return 0;
}


#include <iostream>
#include <cstdlib>
#include <cmath>
#include <boost/pending/disjoint_sets.hpp>
#include <vector>
#include <queue>
#include <map>
using namespace std;

template <class T>
class AffinityGraphCompare{
	private:
	    const T * mEdgeWeightArray;
	public:
		AffinityGraphCompare(const T * EdgeWeightArray){
			mEdgeWeightArray = EdgeWeightArray;
		}
		bool operator() (const int& ind1, const int& ind2) const {
			return (mEdgeWeightArray[ind1] > mEdgeWeightArray[ind2]);
		}
};

/*
 * Compute the MALIS loss function and its derivative wrt the affinity graph
 * MAXIMUM spanning tree
 * Author: Srini Turaga (sturaga@mit.edu)
 * All rights reserved
 */
void malis_loss_weights_cpp(const int nVert, const int* seg,
               const int nEdge, const int* node1, const int* node2, const float* edgeWeight,
               const int pos,
               int* nPairPerEdge){

    /* Disjoint sets and sparse overlap vectors */
    vector<map<int,int> > overlap(nVert);
    vector<int> rank(nVert);
    vector<int> parent(nVert);
    boost::disjoint_sets<int*, int*> dsets(&rank[0],&parent[0]);
    for (int i=0; i<nVert; ++i){
        dsets.make_set(i);
        if (0!=seg[i]) {
            overlap[i].insert(pair<int,int>(seg[i],1));
        }
    }

    /* Sort all the edges in increasing order of weight */
    std::vector< int > pqueue( nEdge );
    int j = 0;
    for ( int i = 0; i < nEdge; i++ ){
        if ((node1[i]>=0) && (node1[i]<nVert) && (node2[i]>=0) && (node2[i]<nVert))
	        pqueue[ j++ ] = i;
    }
    unsigned long nValidEdge = j;
    pqueue.resize(nValidEdge);
    sort( pqueue.begin(), pqueue.end(), AffinityGraphCompare<float>( edgeWeight ) );

    /* Start MST */
    int e;
    int set1, set2;
    int nPair = 0;
    map<int,int>::iterator it1, it2;

    /* Start Kruskal's */
    for (unsigned int i = 0; i < pqueue.size(); ++i ) {
        e = pqueue[i];

        set1 = dsets.find_set(node1[e]);
        set2 = dsets.find_set(node2[e]);

        if (set1!=set2){
            dsets.link(set1, set2);

            /* compute the number of pairs merged by this MST edge */
            for (it1 = overlap[set1].begin();
                    it1 != overlap[set1].end(); ++it1) {
                for (it2 = overlap[set2].begin();
                        it2 != overlap[set2].end(); ++it2) {

                    nPair = it1->second * it2->second;

                    if (pos && (it1->first == it2->first)) {
                        nPairPerEdge[e] += nPair;
                    } else if ((!pos) && (it1->first != it2->first)) {
                        nPairPerEdge[e] += nPair;
                    }
                }
            }

            /* move the pixel bags of the non-representative to the representative */
            if (dsets.find_set(set1) == set2) // make set1 the rep to keep and set2 the rep to empty
                swap(set1,set2);

            it2 = overlap[set2].begin();
            while (it2 != overlap[set2].end()) {
                it1 = overlap[set1].find(it2->first);
                if (it1 == overlap[set1].end()) {
                    overlap[set1].insert(pair<int,int>(it2->first,it2->second));
                } else {
                    it1->second += it2->second;
                }
                overlap[set2].erase(it2++);
            }
        } // end link

    } // end while
}

void connected_components_cpp(const int nVert,
               const int nEdge, const int* node1, const int* node2, const int* edgeWeight,
               int* seg){

    /* Make disjoint sets */
    vector<int> rank(nVert);
    vector<int> parent(nVert);
    boost::disjoint_sets<int*, int*> dsets(&rank[0],&parent[0]);
    for (int i=0; i<nVert; ++i)
        dsets.make_set(i);

    /* union */
    for (int i = 0; i < nEdge; ++i )
         // check bounds to make sure the nodes are valid
        if ((edgeWeight[i]!=0) && (node1[i]>=0) && (node1[i]<nVert) && (node2[i]>=0) && (node2[i]<nVert))
            dsets.union_set(node1[i],node2[i]);

    /* find */
    for (int i = 0; i < nVert; ++i)
        seg[i] = dsets.find_set(i);
}

void marker_watershed_cpp(const int nVert, const int* marker,
               const int nEdge, const int* node1, const int* node2, const float* edgeWeight,
               int* seg){

    /* Make disjoint sets */
    vector<int> rank(nVert);
    vector<int> parent(nVert);
    boost::disjoint_sets<int*, int*> dsets(&rank[0],&parent[0]);
    for (int i=0; i<nVert; ++i)
        dsets.make_set(i);

    /* initialize output array and find representatives of each class */
    std::map<int,int> components;
    for (int i=0; i<nVert; ++i){
        seg[i] = marker[i];
        if (seg[i] > 0)
            components[seg[i]] = i;
    }

    // merge vertices labeled with the same marker
    for (int i=0; i<nVert; ++i)
        if (seg[i] > 0)
            dsets.union_set(components[seg[i]],i);

    /* Sort all the edges in decreasing order of weight */
    std::vector<int> pqueue( nEdge );
    int j = 0;
    for (int i = 0; i < nEdge; ++i)
        if ((edgeWeight[i]!=0) &&
            (node1[i]>=0) && (node1[i]<nVert) &&
            (node2[i]>=0) && (node2[i]<nVert) &&
            (marker[node1[i]]>=0) && (marker[node2[i]]>=0))
                pqueue[ j++ ] = i;
    unsigned long nValidEdge = j;
    pqueue.resize(nValidEdge);
    sort( pqueue.begin(), pqueue.end(), AffinityGraphCompare<float>( edgeWeight ) );

    /* Start MST */
    int set1, set2, label_of_set1, label_of_set2;
    for (unsigned int i = 0; i < pqueue.size(); ++i ) {

        set1=dsets.find_set(node1[i]);
        set2=dsets.find_set(node2[i]);
        label_of_set1 = seg[set1];
        label_of_set2 = seg[set2];

        if ((set1!=set2) &&
            ( ((label_of_set1==0) && (marker[set1]==0)) ||
             ((label_of_set2==0) && (marker[set1]==0))) ){

            dsets.link(set1, set2);
            // either label_of_set1 is 0 or label_of_set2 is 0.
            seg[dsets.find_set(set1)] = std::max(label_of_set1,label_of_set2);

        }

    }

    // write out the final coloring
    for (int i=0; i<nVert; i++)
        seg[i] = seg[dsets.find_set(i)];

}


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

#ifndef HERMITE_PATH_PLANNER__HERMITE_PATH_GENERATOR_HPP_
#define HERMITE_PATH_PLANNER__HERMITE_PATH_GENERATOR_HPP_

#include <quaternion_operation/quaternion_operation.h>

#include <boost/optional.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <hermite_path_msgs/msg/hermite_path_stamped.hpp>
#include <iostream>
#include <map>
#include <vector>
#include <visualization_msgs/msg/marker_array.hpp>

namespace hermite_path_planner
{
class HermitePathGenerator
{
public:
  explicit HermitePathGenerator(double robot_width);
  hermite_path_msgs::msg::HermitePath generateHermitePath(
    geometry_msgs::msg::Pose start, geometry_msgs::msg::Pose goal);
  geometry_msgs::msg::Point getPointOnHermitePath(
    hermite_path_msgs::msg::HermitePath path, double t);
  std::vector<geometry_msgs::msg::Point> getPointsOnHermitePath(
    hermite_path_msgs::msg::HermitePath path, int resolution, double max_t = 1.0);
  boost::optional<double> checkFirstCollisionWithCircle(
    hermite_path_msgs::msg::HermitePath path, geometry_msgs::msg::Point center, double radius);
  double getLength(hermite_path_msgs::msg::HermitePath path, int resolution);
  double getCurvature(hermite_path_msgs::msg::HermitePath path, double t);
  boost::optional<double> getNormalizedLongitudinalDistanceInFrenetCoordinate(
    hermite_path_msgs::msg::HermitePath path, geometry_msgs::msg::Point p);
  boost::optional<double> getLongitudinalDistanceInFrenetCoordinate(
    hermite_path_msgs::msg::HermitePath path, geometry_msgs::msg::Point p, int resolution);
  boost::optional<double> getLateralDistanceInFrenetCoordinate(
    hermite_path_msgs::msg::HermitePath path, geometry_msgs::msg::Point p);
  geometry_msgs::msg::Vector3 getTangentVector(hermite_path_msgs::msg::HermitePath path, double t);
  geometry_msgs::msg::Vector3 getNormalVector(hermite_path_msgs::msg::HermitePath path, double t);
  double getReferenceVelocity(hermite_path_msgs::msg::HermitePathStamped path, double t);
  visualization_msgs::msg::MarkerArray generateDeleteMarker();
  visualization_msgs::msg::MarkerArray generateMarker(
    hermite_path_msgs::msg::HermitePathStamped path, int resolution, bool with_polygon = true);
  visualization_msgs::msg::MarkerArray generateMarker(
    std::vector<hermite_path_msgs::msg::HermitePathStamped> path, int resolution);
  double getMaximumCurvature(hermite_path_msgs::msg::HermitePath path, int resolution);

private:
  hermite_path_msgs::msg::HermitePath generateHermitePath(
    geometry_msgs::msg::Pose start, geometry_msgs::msg::Pose goal, double start_vector_magnitude,
    double end_vector_magnitude);
  visualization_msgs::msg::Marker getBoundsPolygon(
    hermite_path_msgs::msg::HermitePathStamped path, int resolution, double z_offset);
  geometry_msgs::msg::Vector3 getVectorFromPose(geometry_msgs::msg::Pose pose, double magnitude);
  std::vector<geometry_msgs::msg::Point> getLeftBounds(
    hermite_path_msgs::msg::HermitePath path, int resolution);
  std::vector<geometry_msgs::msg::Point> getRightBounds(
    hermite_path_msgs::msg::HermitePath path, int resolution);
  double robot_width_;
  double calculateNewtonMethodStepSize(
    hermite_path_msgs::msg::HermitePath path, geometry_msgs::msg::Point center, double radius,
    double t);
  inline double sigmoid(double gain, double x, double offset = 0.0)
  {
    return 1.0 / (1.0 + exp(-gain * (x - offset)));
  }
};
}  // namespace hermite_path_planner

#endif  // HERMITE_PATH_PLANNER__HERMITE_PATH_GENERATOR_HPP_


#include "camplex/FiducialVisualizer.h"

#include "geometry_msgs/Point.h"

#include "argus_utils/utils/ParamUtils.h"
#include "argus_utils/geometry/GeometryUtils.h"

#include <boost/foreach.hpp>

namespace argus
{

geometry_msgs::Point trans_to_point( const Translation3Type& trans )
{
    geometry_msgs::Point p;
    p.x = trans.x();
    p.y = trans.y();
    p.z = trans.z();
    return p;
}

FiducialVisualizer::FiducialVisualizer()
{
    SetPointSize( 0.1 );
    SetShowAxes( true );
}

void FiducialVisualizer::ReadParams( const ros::NodeHandle& nh )
{
    PoseVisualizer::ReadParams( nh );
    double a;
    bool b;
    if( GetParam( nh, "point_size", a ) ) { SetPointSize( a ); }
    if( GetParam( nh, "show_axes", b ) ) { SetShowAxes( b ); }
}

void FiducialVisualizer::SetPointSize( double s )
{
    if( s < 0 )
    {
        throw std::invalid_argument( "Point size must be positive." );
    }
    _pointSize = s;
}

void FiducialVisualizer::SetShowAxes( bool b )
{
    _showAxes = b;
}

std::vector<MarkerMsg> FiducialVisualizer::ToMarkers( const PoseSE3& pose,
                                                      const Fiducial& fid,
                                                      const std::string& name ) const
{
    MarkerMsg marker = InitMarker();
    marker.id = VisualizerID::FID_ID;
    marker.scale.x = _pointSize;
    marker.scale.y = _pointSize;
    marker.type = MarkerMsg::POINTS;

    AddPoints( fid.Transform( pose ), marker.points );
    std::vector<MarkerMsg> markers;
    markers.push_back( marker );

    if( _showAxes )
    {
        std::vector<MarkerMsg> axesMarkers = PoseVisualizer::ToMarkers( pose, name );
        markers.insert( markers.end(), axesMarkers.begin(), axesMarkers.end() );
    }

    return markers;
}

std::vector<MarkerMsg> FiducialVisualizer::ToMarkers( const std::vector<PoseSE3>& poses,
                                                      const std::vector<Fiducial>& fids,
                                                      const std::vector<std::string>& names ) const
{
    if( poses.size() != fids.size() || (!names.empty() && names.size() != poses.size()) )
    {
        throw std::invalid_argument( "Must specify equal number of names, poses, and fiducials." );
    }

    std::vector<MarkerMsg> markers;
    if( poses.empty() )
    {
        return markers;
    }

    MarkerMsg marker = InitMarker();
    marker.id = VisualizerID::FID_ID;
    marker.scale.x = _pointSize;
    marker.scale.y = _pointSize;
    marker.type = MarkerMsg::POINTS;

    for( unsigned int i = 0; i < poses.size(); ++i )
    {
        const PoseSE3& pose = poses[i];
        const Fiducial& fid = fids[i];
        AddPoints( fid.Transform( pose ), marker.points );
        if( !names.empty() )
        {
            const std::string& name = names[i];
            AddNameMarker( name, pose, markers );
        }

    }
    markers.push_back( marker );

    if( _showAxes )
    {
        std::vector<MarkerMsg> axesMarkers = PoseVisualizer::ToMarkers( poses, names );
        markers.insert( markers.end(), axesMarkers.begin(), axesMarkers.end() );
    }

    return markers;
}

void FiducialVisualizer::AddPoints( const Fiducial& fid,
                                    std::vector<geometry_msgs::Point>& points ) const
{
    BOOST_FOREACH( const Translation3Type& trans, fid.points )
    {
        points.push_back( trans_to_point( trans ) );
    }
}

}


/*
 * Copyright (c) 2021. Kun Huang.
 * This Source Code Form is subject to the terms of the Apache License, v. 2.0.
 */

//
// Created by huangkun on 2021/7/21.
//

#ifndef OPENGV2_OBSERVATIONBASE_HPP
#define OPENGV2_OBSERVATIONBASE_HPP

#include <memory>
#include <Eigen/Eigen>
#include <unsupported/Eigen/MatrixFunctions>

namespace opengv2 {
    class ObservationBase {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef std::shared_ptr<ObservationBase> Ptr;

        ObservationBase(const Eigen::Ref<const Eigen::MatrixXd> &measurement,
                        const Eigen::Ref<const Eigen::MatrixXd> &invCovariance) :
                measurement_(measurement), invSqrtCovariance_(invCovariance.sqrt()) {}

        virtual ~ObservationBase() = default;

        inline const Eigen::MatrixXd &measurement() const noexcept {
            return measurement_;
        }

        /**
         * @return The inverse square root of the covariance matrix
         * @note should be multiplied inside the cost function
         * The covariance matrix is a positive semi-definite matrix.
         */
        inline const Eigen::MatrixXd &invSqrtCovariance() const noexcept {
            return invSqrtCovariance_;
        }

        /**
         * @note Warning: Not thread safe. Just for Debug and simulation.
         */
        virtual inline void setMeasurement(const Eigen::Ref<const Eigen::MatrixXd> &measurement) noexcept {
            measurement_ = measurement;
        }

    protected:
        Eigen::MatrixXd measurement_;
        Eigen::MatrixXd invSqrtCovariance_;
    };
}

#endif //OPENGV2_OBSERVATIONBASE_HPP


// mbf9bin.cpp : Defines the entry point for the console application.
//

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <ostream>
#include <string>

extern "C" {
	#include "SFMT.h"
}

#include "time_ms.h"
#include "mbf16random.h"

#include <emmintrin.h>

#include <boost/thread.hpp>
#include <boost/atomic.hpp>

using namespace std;
using namespace mbf16;

boost::atomic<uint64_t> seed;
boost::atomic<int> loop_count;
boost::mutex mtx_;

const int ARRAY_SIZE = 10000;

void thread()
{
	sfmt_t mt;
	uint64_t working_seed;
	{
		mtx_.lock();
		working_seed = seed++;
		mtx_.unlock();
	}
	rand_init(working_seed, mt);

	int i=0;
	int j=0;

	int cnt;
	mbf8* f8l = (mbf8*)calloc(ARRAY_SIZE, sizeof(mbf8));
	mbf8* f8r = (mbf8*)calloc(ARRAY_SIZE, sizeof(mbf8));

	for (cnt=0;cnt<loop_count;cnt++)
	{
		double t1 = TimeMillis();
		for (i=0;i<ARRAY_SIZE;i++)
		{
			f8l[i] = makef8(mt);
			f8r[i] = makef8(mt);
		}

		unsigned int sumcnt = 0;
		double t2 = TimeMillis();

		for (i=0;i<ARRAY_SIZE;i++)
		{
			for (j=0;j<ARRAY_SIZE;j++)
			{
				if (cmp8(f8l[i],f8r[j]))
				{
					sumcnt++;
				}
			}
		}
		mtx_.lock();
		cout << working_seed << "\t" << cnt << "\t" << sumcnt << "\t" << t2-t1 << "\t" << TimeMillis()-t2 << endl;
		mtx_.unlock();

	}
}

int main(int argc, char* argv[])
{
	if (argc < 4)
	{
		std::cout << "too few parameters";
		return -1;
	}

	std::string param1(argv[1]);
	seed = std::stoull(param1);

	std::string param2(argv[2]);
	int threadcount = std::stoi(param2);

	std::string param3(argv[3]);
	loop_count = std::stoi(param3);

	if (seed==0)
	{
		cout << "seed should not be 0";
		return -1;
	}
	init();

	boost::thread** threads = new boost::thread*[threadcount];
	for (int i = 0; i < threadcount; i++)
	{
		threads[i] = new boost::thread( thread );
	}

	for (int i = 0; i < threadcount; i++)
	{
		threads[i]->join();
	}

    return EXIT_SUCCESS;
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id: random.hpp 634 2010-08-15 16:39:44Z mistevens $
 */

/*
 * Good random numbers from Boost
 *  Provides a common class  for all random number requirements to test Bayes++
 */

#include <boost/version.hpp>
#include <boost/random.hpp>

namespace Bayesian_filter
{

namespace
{
	// ISSUE variate_generator cannot be used without Partial template specialistion
	template<class Engine, class Distribution>
	class simple_generator_01
	{
	public:
		typedef typename Distribution::result_type result_type;
		simple_generator_01(Engine& e, Distribution& d)
			: urng01(e), _dist(d)
		{}
		result_type operator()()
		{	return _dist(urng01);
		 }
	private:
		class uniform_01
		{	// Simple uniform [0..1) distribution
			typedef typename Distribution::input_type result_type;
			Engine& _eng;
		public:
			uniform_01(Engine& e)
				: _eng(e)
			{}
			result_type operator()()
			{
				result_type factor = result_type(1) / result_type(_eng.max()-_eng.min() +  result_type(1));
              	return result_type(_eng() - _eng.min()) * factor;
			}
		};
		uniform_01 urng01;
		Distribution& _dist;
	};
	template<class Engine, class Distribution>
	class simple_generator
	{
	public:
		typedef typename Distribution::result_type result_type;
		simple_generator(Engine& e, Distribution& d)
			: _eng(e), _dist(d)
		{}
		result_type operator()()
		{	return _dist(_eng);
		 }
	private:
		Engine& _eng;
		Distribution& _dist;
	};
}//namespace

class Boost_random
{
public:
	typedef Bayesian_filter_matrix::Float Float;
	typedef boost::mt19937 URng;
	Boost_random() : dist_uniform_01(), dist_normal_01()
	{}
	Bayesian_filter_matrix::Float normal(const Float mean, const Float sigma)
	{
		boost::normal_distribution<Float> dist(mean, sigma);
		simple_generator_01<URng, boost::normal_distribution<Float> > gen(rng, dist);
		return gen();
	}
	void normal(Bayesian_filter_matrix::DenseVec& v, const Float mean, const Float sigma)
	{
		boost::normal_distribution<Float> dist(mean, sigma);
		simple_generator_01<URng, boost::normal_distribution<Float> > gen(rng, dist);
		std::generate (v.begin(), v.end(), gen);
	}
	void normal(Bayesian_filter_matrix::DenseVec& v)
	{
		simple_generator_01<URng, boost::normal_distribution<Float> > gen(rng, dist_normal_01);
		std::generate (v.begin(), v.end(), gen);
	}
	void uniform_01(Bayesian_filter_matrix::DenseVec& v)
	{
		simple_generator<URng, boost::uniform_real<Float> > gen(rng, dist_uniform_01);
		std::generate (v.begin(), v.end(), gen);
	}
#ifdef BAYES_FILTER_GAPPY
	void normal(Bayesian_filter_matrix::Vec& v, const Float mean, const Float sigma)
	{
		boost::normal_distribution<Float> dist(mean, sigma);
		simple_generator_01<URng, boost::normal_distribution<Float> > gen(rng, dist);
		for (std::size_t i = 0, iend=v.size(); i < iend; ++i)
			v[i] = gen();
	}
	void normal(Bayesian_filter_matrix::Vec& v)
	{
		simple_generator_01<URng, boost::normal_distribution<Float> > gen(rng, dist_normal_01);
		for (std::size_t i = 0, iend=v.size(); i < iend; ++i)
			v[i] = gen();
	}
	void uniform_01(Bayesian_filter_matrix::Vec& v)
	{
		simple_generator_01<URng, boost::uniform_real<Float> > gen(rng, dist_uniform_01);
		for (std::size_t i = 0, iend=v.size(); i < iend; ++i)
			v[i] = gen();
	}
#endif
	void seed()
	{
		rng.seed();
	}
private:
	URng rng;
	boost::uniform_real<Float> dist_uniform_01;
	boost::normal_distribution<Float> dist_normal_01;
};

}//namespace