text:
// ROS includes
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>

// We will use OpenCV to bootstrap solution
#include <opencv2/calib3d/calib3d.hpp>

// Third-party includes
#include <geometry_msgs/TransformStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/Path.h>
#include <std_srvs/Trigger.h>

// Non-standard datra messages
#include <deepdive_ros/Light.h>
#include <deepdive_ros/Lighthouses.h>
#include <deepdive_ros/Trackers.h>

// Ceres and logging
#include <Eigen/Core>
#include <Eigen/Geometry>

// C++ libraries
#include <map>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>

// Shared local code
#include "deepdive.hh"

// GLOBAL PARAMETERS

// List of lighthouses
TrackerMap trackers_;
LighthouseMap lighthouses_;
MeasurementMap measurements_;
CorrectionMap corrections_;

// Global strings
std::string calfile_ = "deepdive.tf2";
std::string frame_world_ = "world";     // World frame
std::string frame_vive_ = "vive";       // Vive frame
std::string frame_body_ = "body";       // EKF / external solution
std::string frame_truth_ = "truth";     // Vive solution

// Rejection thresholds
int thresh_count_ = 4;
double thresh_angle_ = 60.0;
double thresh_duration_ = 1.0;

// Are we running in "offline" mode
bool offline_ = false;

// Should we publish rviz markers
bool visualize_ = true;

// Are we recording right now?
bool recording_ = false;

// Should we correct lighthouse measurements
bool correct_ = false;

// Graph resolution
double res_ = 0.1;

// Tracker  centroid from body frame
std::vector<double> offset_;

// World -> vive registatration
double wTv_[6];

// Sensor visualization publisher
ros::Publisher pub_truth_;
std::map<std::string, ros::Publisher> pub_sensors_;
std::map<std::string, std::map<std::string, ros::Publisher>> pub_path_;

// Timer for managing offline
ros::Timer timer_;

// Jointly solve
bool Solve() {
  // Check that we have enough measurements
  if (measurements_.empty()) {
    ROS_WARN("Insufficient measurements received, so cannot solve problem.");
    return false;
  } else {
    double t = (measurements_.rbegin()->first
      - measurements_.begin()->first).toSec();
    ROS_INFO_STREAM("Processing " << measurements_.size()
      << " measurements running for " << t << " seconds from "
      << measurements_.begin()->first << " to "
      << measurements_.rbegin()->first);
  }

  // Check corrections
  if (corrections_.empty()) {
    ROS_INFO("No corrections in dataset. Assuming first body pose at origin.");
  } else {
    double t = (corrections_.rbegin()->first - corrections_.begin()->first).toSec();
    ROS_INFO_STREAM("Processing " << corrections_.size()
      << " corrections running for " << t << " seconds from "
      << corrections_.begin()->first << " to "
      << corrections_.rbegin()->first);
  }

  // Data storage for the upcoming steps

  typedef std::map<ros::Time,             // Time
            std::map<uint8_t,             // Sensor
              std::map<uint8_t,           // Axis
                std::vector<double>       // Sample
              >
            >
          > Bundle;
  std::map<std::string,                   // Tracker
    std::map<std::string,                 // Lighthouse
      Bundle
    >
  > bundle;

  std::map<ros::Time, double[6]> cor;     // Corrections

  double height = 0.0;                    // Average height

  // We bundle measurements into into bins of width "resolution". This allows
  // us to take the average of the measurements to improve accuracy
  {
    ROS_INFO("Bundling measurements into larger discrete time units.");
    MeasurementMap::iterator mt;
    for (mt = measurements_.begin(); mt != measurements_.end(); mt++) {
      std::string const& tserial = mt->second.light.header.frame_id;
      std::string const& lserial = mt->second.light.lighthouse;
      size_t const& a = mt->second.light.axis;
      ros::Time t = ros::Time(round(mt->first.toSec() / res_) * res_);
      std::vector<deepdive_ros::Pulse>::iterator pt;
      for (pt = mt->second.light.pulses.begin(); pt != mt->second.light.pulses.end(); pt++)
        bundle[tserial][lserial][t][pt->sensor][a].push_back(pt->angle);
    }
    ROS_INFO("Bundling corrections into larger discrete time units.");
    CorrectionMap::iterator ct;
    for (ct = corrections_.begin(); ct != corrections_.end(); ct++) {
      ros::Time t = ros::Time(round(ct->first.toSec() / res_) * res_);
      Eigen::Quaterniond q(
        ct->second.transform.rotation.w,
        ct->second.transform.rotation.x,
        ct->second.transform.rotation.y,
        ct->second.transform.rotation.z);
      Eigen::AngleAxisd aa(q.toRotationMatrix());
      cor[t][0] = ct->second.transform.translation.x;
      cor[t][1] = ct->second.transform.translation.y;
      cor[t][2] = ct->second.transform.translation.z;
      cor[t][3] = aa.angle() * aa.axis()[0];
      cor[t][4] = aa.angle() * aa.axis()[1];
      cor[t][5] = aa.angle() * aa.axis()[2];
      height += ct->second.transform.translation.z;
    }
    if (!corrections_.empty())
      height /= corrections_.size();
    ROS_INFO_STREAM("Average height is " << height << " meters");
  }

  // Data storage for next step

  typedef std::map<std::string,               // Tracker
    std::map<ros::Time,               // Time
      std::map<std::string,           // Lighthouse
        double[6]                     // Transform
      >
    >
  > PoseMap;

  PoseMap poses;

  // We are going to estimate the pose of each slave lighthouse in the frame
  // of the master lighthouse (vive frame) using PNP. We can think of the
  // two lighthouses as a stereo pair that are looking at a set of corresp-
  // ondences (photosensors). We want to calibrate this stereo pair.
  {
    ROS_INFO("Using P3P to estimate pose sequence in every lighthouse frame.");
    double fov = 2.0944;                          // 120deg FOV
    double w = 1.0;                               // 1m synthetic image plane
    double z = w / (2.0 * std::tan(fov / 2.0));   // Principle distance
    uint32_t count = 0;                           // Track num transforms
    // Iterate over lighthouses
    LighthouseMap::iterator lt;
    for (lt = lighthouses_.begin(); lt != lighthouses_.end(); lt++) {
      // Iterate over trackers
      TrackerMap::iterator tt;
      for (tt = trackers_.begin(); tt != trackers_.end(); tt++) {
        ROS_INFO_STREAM("- Slave " << lt->first << " and tracker " << tt->first);
        // Iterate over time epochs
        Bundle::iterator bt;
        for (bt = bundle[tt->first][lt->first].begin();
          bt != bundle[tt->first][lt->first].end(); bt++) {
          // One for each time instance
          std::vector<cv::Point3f> obj;
          std::vector<cv::Point2f> img;
          // Try and find correspondences for every possible sensor
          for (uint8_t s = 0; s < NUM_SENSORS; s++) {
            // Mean angles for the <lighthouse, axis>
            double angles[2];
            // Check that we have azimuth/elevation for both lighthouses
            if (!Mean(bundle[tt->first][lt->first][bt->first][s][0], angles[0]) ||
                !Mean(bundle[tt->first][lt->first][bt->first][s][1], angles[1]))
              continue;
            // Correct the angles using the lighthouse parameters
            Correct(lt->second.params, angles, correct_);
            // Push on the correct world sensor position
            obj.push_back(cv::Point3f(
              trackers_[tt->first].sensors[s * 6 + 0],
              trackers_[tt->first].sensors[s * 6 + 1],
              trackers_[tt->first].sensors[s * 6 + 2]));
            // Push on the coordinate in the slave image plane
            img.push_back(cv::Point2f(z * tan(angles[0]), z * tan(angles[1])));
          }
          // In the case that we have 4 or more measurements, then we can try
          // and estimate the trackers location in the lighthouse frame.
          if (obj.size() > 6) {
            cv::Mat cam = cv::Mat::eye(3, 3, cv::DataType<double>::type);
            cv::Mat dist;
            cam.at<double>(0, 0) = z;
            cam.at<double>(1, 1) = z;
            cv::Mat R(3, 1, cv::DataType<double>::type);
            cv::Mat T(3, 1, cv::DataType<double>::type);
            cv::Mat C(3, 3, cv::DataType<double>::type);
            if (cv::solvePnPRansac(obj, img, cam, dist, R, T,  false,
              100, 8.0, 0.99, cv::noArray(), cv::SOLVEPNP_UPNP)) {
              cv::Rodrigues(R, C);
              Eigen::Matrix3d rot;
              for (size_t r = 0; r < 3; r++)
                for (size_t c = 0; c < 3; c++)
                  rot(r, c) = C.at<double>(r, c);
              Eigen::AngleAxisd aa(rot);
              poses[tt->first][bt->first][lt->first][0] = T.at<double>(0, 0);
              poses[tt->first][bt->first][lt->first][1] = T.at<double>(1, 0);
              poses[tt->first][bt->first][lt->first][2] = T.at<double>(2, 0);
              poses[tt->first][bt->first][lt->first][3] = aa.angle() * aa.axis()[0];
              poses[tt->first][bt->first][lt->first][4] = aa.angle() * aa.axis()[1];
              poses[tt->first][bt->first][lt->first][5] = aa.angle() * aa.axis()[2];
              count++;
            }
          }
        }
      }
    }
    ROS_INFO_STREAM("Using " << count << " PNP solutions");
  }
  // We now have a separate pose sequence for each tracker in each lighthouse
  // frame. We now need to find the transform from the slave to master light-
  // house in a way that projects one pose sequence into the other.
  {
    ROS_INFO("Estimating master -> slave lighthouse transforms.");
    // Add residual blocks to the problem
    LighthouseMap::iterator lm = lighthouses_.begin();  // First is master
    LighthouseMap::iterator lt;                         //
    for (lt = lighthouses_.begin(); lt != lighthouses_.end(); lt++) {
      // Master lighthouse
      if (lt == lighthouses_.begin()) {
        for (size_t i = 0; i < 6; i++)
          lt->second.vTl[0] = 0;
        continue;
      }
      // Correspondences
      std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> corresp;
      // Slave lighthouse
      if (lt != lighthouses_.begin()) {
        TrackerMap::iterator tt;
        for (tt = trackers_.begin(); tt != trackers_.end(); tt++) {
          std::map<ros::Time, std::map<std::string, double[6]>>::iterator pt;
          for (pt = poses[tt->first].begin(); pt != poses[tt->first].end(); pt++) {
            // We must have a pose for both the master AND the slave
            if (pt->second.find(lt->first) == pt->second.end() ||
                pt->second.find(lm->first) == pt->second.end()) continue;
            // If we get here, then we have a correspondence
            corresp.push_back(std::pair<Eigen::Vector3d, Eigen::Vector3d>(
              Eigen::Vector3d(
                pt->second[lt->first][0],
                pt->second[lt->first][1],
                pt->second[lt->first][2]
              ),
              Eigen::Vector3d(
                pt->second[lm->first][0],
                pt->second[lm->first][1],
                pt->second[lm->first][2])
              )
            );
          }
        }
      }
      // Run kabsch to determine the projection from the slave to master
      Eigen::Matrix<double, 3, Eigen::Dynamic> pti(3, corresp.size());
      Eigen::Matrix<double, 3, Eigen::Dynamic> ptj(3, corresp.size());
      for (size_t i = 0; i < corresp.size(); i++) {
        pti.block<3, 1>(0, i) = corresp[i].first;
        ptj.block<3, 1>(0, i) = corresp[i].second;
      }
      // Perform a KABSCH transform on the two matrices
      ROS_INFO_STREAM("- Using " << corresp.size() << " correspondences");
      Eigen::Affine3d A;
      if (Kabsch<double>(pti, ptj, A, false))
        ROS_INFO_STREAM("- Solution " << A.translation().norm());
      else
        ROS_INFO("- Solution not found");
      // Write the solution
      lt->second.vTl[0] = A.translation()[0];
      lt->second.vTl[1] = A.translation()[1];
      lt->second.vTl[2] = A.translation()[2];
      Eigen::AngleAxisd aa(A.linear());
      lt->second.vTl[3] =  aa.angle() * aa.axis()[0];
      lt->second.vTl[4] =  aa.angle() * aa.axis()[1];
      lt->second.vTl[5] =  aa.angle() * aa.axis()[2];
    }
  }

  // REGISTRATION - for the granite lab we know that the trackers are mounted
  // symmetrically about the body frame. Consequently, the average X-Y pose
  // of the trackers is a good approximation of the body X-Y pose. The height
  // is fixed, so we can take the average from the corrections
  {
    ROS_INFO("Using corrections to register vive to world frame.");
    // The vive frame is the same as the maste rlighthouse
    std::string lm = lighthouses_.begin()->first;
    // This will store the correspondences
    std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> corresp;
    // Iterate over all corrections
    std::map<ros::Time, double[6]>::iterator ct;
    for (ct = cor.begin(); ct != cor.end(); ct++) {
      double x = 0.0, y = 0.0, z = 0.0;
      size_t n = 0;
      TrackerMap::iterator tt;
      for (tt = trackers_.begin(); tt != trackers_.end(); tt++) {
        if (poses.find(tt->first) != poses.end() &&
          poses[tt->first].find(ct->first) != poses[tt->first].end() &&
          poses[tt->first][ct->first].find(lm) != poses[tt->first][ct->first].end()) {
          x += poses[tt->first][ct->first][lm][0];
          y += poses[tt->first][ct->first][lm][1];
          z += poses[tt->first][ct->first][lm][2];
          n++;
        }
      }
      // Only if we have data from all trackers
      if (n == trackers_.size()) {
        corresp.push_back(
          std::pair<Eigen::Vector3d, Eigen::Vector3d>(
            Eigen::Vector3d(x / n, y / n, z / n),
            Eigen::Vector3d(
              ct->second[0] + offset_[0],
              ct->second[1] + offset_[1],
              ct->second[2] + offset_[2]
            )
          )
        );
      }
    }
    // Run kabsch to determine the projection from the slave to master
    Eigen::Matrix<double, 3, Eigen::Dynamic> pti(3, corresp.size());
    Eigen::Matrix<double, 3, Eigen::Dynamic> ptj(3, corresp.size());
    for (size_t i = 0; i < corresp.size(); i++) {
      pti.block<3, 1>(0, i) = corresp[i].first;
      ptj.block<3, 1>(0, i) = corresp[i].second;
    }
    // Perform a KABSCH transform on the two matrices
    ROS_INFO_STREAM("- Using " << corresp.size() << " correspondences");
    Eigen::Affine3d A;
    if (Kabsch<double>(pti, ptj, A, false))
      ROS_INFO_STREAM("- Solution " << A.translation().norm());
    else
      ROS_INFO("- No correspondences so vive -> world frame is identity");
    // Write the solution
    wTv_[0] = A.translation()[0];
    wTv_[1] = A.translation()[1];
    wTv_[2] = A.translation()[2];
    Eigen::AngleAxisd aa(A.linear());
    wTv_[3] = aa.angle() * aa.axis()[0];
    wTv_[4] = aa.angle() * aa.axis()[1];
    wTv_[5] = aa.angle() * aa.axis()[2];
  }

  // We now have a great estimate of the slave -> master lighthous transforms,
  // sensor trajectories, and global registration
  {
    SendTransforms(frame_world_, frame_vive_, frame_body_,
      wTv_, lighthouses_, trackers_);
    // Write the solution to a config file
    if (WriteConfig(calfile_, frame_world_, frame_vive_, frame_body_,
      wTv_, lighthouses_, trackers_))
      ROS_INFO_STREAM("Calibration written to " << calfile_);
    else
      ROS_INFO_STREAM("Could not write calibration to" << calfile_);
    // Print the trajectory of the body-frame in the world-frame
    if (visualize_) {
      // Estimates
      LighthouseMap::iterator lt;
      for (lt = lighthouses_.begin(); lt != lighthouses_.end(); lt++) {
        TrackerMap::iterator tt;
        for (tt = trackers_.begin(); tt != trackers_.end(); tt++) {
          // Create a path
          nav_msgs::Path msg;
          msg.header.stamp = ros::Time::now();
          msg.header.frame_id = lt->first;
          // Iterate over timestamp
          std::map<ros::Time, std::map<std::string, double[6]>>::iterator pt;
          for (pt = poses[tt->first].begin(); pt != poses[tt->first].end(); pt++) {
            // Don't plot if the data doesn't exist
            if (pt->second.find(lt->first) == pt->second.end())
              continue;
            // Convert axis angle to quaternion
            Eigen::Vector3d v(
              pt->second[lt->first][3],
              pt->second[lt->first][4],
              pt->second[lt->first][5]);
            Eigen::AngleAxisd aa = Eigen::AngleAxisd::Identity();
            if (v.norm() > 0) {
              aa.angle() = v.norm();
              aa.axis() = v.normalized();
            }
            Eigen::Quaterniond q(aa);
            // PUblish pose
            geometry_msgs::PoseStamped ps;
            ps.header.stamp = pt->first;
            ps.header.frame_id = lt->first;
            ps.pose.position.x = pt->second[lt->first][0];
            ps.pose.position.y = pt->second[lt->first][1];
            ps.pose.position.z = pt->second[lt->first][2];
            ps.pose.orientation.w = q.w();
            ps.pose.orientation.x = q.x();
            ps.pose.orientation.y = q.y();
            ps.pose.orientation.z = q.z();
            msg.poses.push_back(ps);
          }
          pub_path_[lt->first][tt->first].publish(msg);
        }
      }
      // Create a path
      nav_msgs::Path msg;
      msg.header.stamp = ros::Time::now();
      msg.header.frame_id = frame_world_;
      std::map<ros::Time, double[6]>::iterator ct;
      for (ct = cor.begin(); ct != cor.end(); ct++) {
        Eigen::Vector3d v(ct->second[3], ct->second[4], ct->second[5]);
        Eigen::AngleAxisd aa = Eigen::AngleAxisd::Identity();
        if (v.norm() > 0) {
          aa.angle() = v.norm();
          aa.axis() = v.normalized();
        }
        Eigen::Quaterniond q(aa);
        geometry_msgs::PoseStamped ps;
        ps.header.stamp = ct->first;
        ps.header.frame_id = frame_world_;
        ps.pose.position.x = ct->second[0];
        ps.pose.position.y = ct->second[1];
        ps.pose.position.z = ct->second[2];
        ps.pose.orientation.w = q.w();
        ps.pose.orientation.x = q.x();
        ps.pose.orientation.y = q.y();
        ps.pose.orientation.z = q.z();
        msg.poses.push_back(ps);
      }
      pub_truth_.publish(msg);
    }
  }
  return true;
}

// MESSAGE CALLBACKS

void LightCallback(deepdive_ros::Light::ConstPtr const& msg) {
  // Reset the timer use din offline mode to determine the end of experiment
  timer_.stop();
  timer_.start();
  // Check that we are recording and that the tracker/lighthouse is ready
  if (!recording_ ||
    trackers_.find(msg->header.frame_id) == trackers_.end() ||
    lighthouses_.find(msg->lighthouse) == lighthouses_.end() ||
    !trackers_[msg->header.frame_id].ready ||
    !lighthouses_[msg->lighthouse].ready) return;
  // Copy over the data
  size_t deleted = 0;
  deepdive_ros::Light data = *msg;
  std::vector<deepdive_ros::Pulse>::iterator it = data.pulses.end();
  while (it-- > data.pulses.begin()) {
    // std::cout << it->duration << std::endl;
    if (it->angle > thresh_angle_ / 57.2958 ||     // Check angle
        it->duration < thresh_duration_ / 1e6) {  // Check duration
      it = data.pulses.erase(it);
      deleted++;
    }
  }
  if (data.pulses.size() < thresh_count_)
    return;
  // Add the data
  measurements_[ros::Time::now()].light = data;
}

bool TriggerCallback(std_srvs::Trigger::Request  &req,
                     std_srvs::Trigger::Response &res)
{
  if (!recording_) {
    res.success = true;
    res.message = "Recording started.";
  }
  if (recording_) {
    // Solve the problem
    res.success = Solve();
    if (res.success)
      res.message = "Recording stopped. Solution found.";
    else
      res.message = "Recording stopped. Solution not found.";
    // Clear all the data and corrections
    measurements_.clear();
  }
  // Toggle recording state
  recording_ = !recording_;
  // Success
  return true;
}

// When corrections arrive
void CorrectionCallback(tf2_msgs::TFMessage::ConstPtr const& msg) {
  // Check that we are recording and that the tracker/lighthouse is ready
  if (!recording_)
    return;
  std::vector<geometry_msgs::TransformStamped>::const_iterator it;
  for (it = msg->transforms.begin(); it != msg->transforms.end(); it++) {
    if (it->header.frame_id == frame_world_ &&
        it->child_frame_id == frame_body_) {
      corrections_[ros::Time::now()] = *it;
    }
  }
}

// Fake a trigger when the timer expires
void TimerCallback(ros::TimerEvent const& event) {
  std_srvs::Trigger::Request req;
  std_srvs::Trigger::Response res;
  TriggerCallback(req, res);
}

// Called when a new lighthouse appears
void NewLighthouseCallback(LighthouseMap::iterator lighthouse) {
  ROS_INFO_STREAM("Found lighthouse " << lighthouse->first);
}

// Called when a new tracker appears
void NewTrackerCallback(TrackerMap::iterator tracker) {
  ROS_INFO_STREAM("Found tracker " << tracker->first);
  if (visualize_) {
    visualization_msgs::MarkerArray msg;
    std::map<std::string, Tracker>::iterator jt;
    for (jt = trackers_.begin(); jt != trackers_.end(); jt++)  {
      for (uint16_t i = 0; i < NUM_SENSORS; i++) {
        // All this code just to convert a normal to a quaternion
        Eigen::Vector3d vfwd(jt->second.sensors[6*i+3],
          jt->second.sensors[6*i+4], jt->second.sensors[6*i+5]);
        if (vfwd.norm() > 0) {
          Eigen::Vector3d vdown(0.0, 0.0, 1.0);
          Eigen::Vector3d vright = vdown.cross(vfwd);
          vfwd = vfwd.normalized();
          vright = vright.normalized();
          vdown = vdown.normalized();
          Eigen::Matrix3d dcm;
          dcm << vfwd.x(), vright.x(), vdown.x(),
                 vfwd.y(), vright.y(), vdown.y(),
                 vfwd.z(), vright.z(), vdown.z();
          Eigen::Quaterniond q(dcm);
          // Now plot an arrow representing the normal
          visualization_msgs::Marker marker;
          marker.header.frame_id = jt->first + "/light";
          marker.header.stamp = ros::Time::now();
          marker.ns = jt->first;
          marker.id = i;
          marker.type = visualization_msgs::Marker::ARROW;
          marker.action = visualization_msgs::Marker::ADD;
          marker.pose.position.x = jt->second.sensors[6*i+0];
          marker.pose.position.y = jt->second.sensors[6*i+1];
          marker.pose.position.z = jt->second.sensors[6*i+2];
          marker.pose.orientation.w = q.w();
          marker.pose.orientation.x = q.x();
          marker.pose.orientation.y = q.y();
          marker.pose.orientation.z = q.z();
          marker.scale.x = 0.010;
          marker.scale.y = 0.001;
          marker.scale.z = 0.001;
          marker.color.a = 1.0;
          marker.color.r = 1.0;
          marker.color.g = 0.0;
          marker.color.b = 0.0;
          msg.markers.push_back(marker);
        }
      }
      pub_sensors_[tracker->first].publish(msg);
    }
  }
}

// MAIN ENTRY POINT

int main(int argc, char **argv) {
  // Initialize ROS and create node handle
  ros::init(argc, argv, "deepdive_calibrate");
  ros::NodeHandle nh("~");

  // If we are in offline mode when we will replay the data back at 10x the
  // speed, using it all to find a calibration solution for both the body
  // as a function of  time and the lighthouse positions.
  if (!nh.getParam("offline", offline_))
    ROS_FATAL("Failed to get if we are running in offline mode.");
  if (offline_) {
    ROS_INFO("We are in offline mode. Speed-up is possible.");
    recording_ = true;
  }

  // Reset the registration information
  for (size_t i = 0; i < 6; i++)
    wTv_[i] = 0;

  // Get the parent information
  if (!nh.getParam("calfile", calfile_))
    ROS_FATAL("Failed to get the calfile file.");

  // Get some global information
  if (!nh.getParam("frames/world", frame_world_))
    ROS_FATAL("Failed to get frames/world parameter.");
  if (!nh.getParam("frames/vive", frame_vive_))
    ROS_FATAL("Failed to get frames/vive parameter.");
  if (!nh.getParam("frames/body", frame_body_))
    ROS_FATAL("Failed to get frames/body parameter.");
  if (!nh.getParam("frames/truth", frame_truth_))
    ROS_FATAL("Failed to get frames/truth parameter.");

  // Get the thresholds
  if (!nh.getParam("thresholds/count", thresh_count_))
    ROS_FATAL("Failed to get threshods/count parameter.");
  if (!nh.getParam("thresholds/angle", thresh_angle_))
    ROS_FATAL("Failed to get thresholds/angle parameter.");
  if (!nh.getParam("thresholds/duration", thresh_duration_))
    ROS_FATAL("Failed to get thresholds/duration parameter.");

  // Tracking resolution
  if (!nh.getParam("resolution", res_))
    ROS_FATAL("Failed to get resolution parameter.");

  // Whether to apply light corrections
  if (!nh.getParam("correct", correct_))
    ROS_FATAL("Failed to get correct parameter.");

  // Visualization option
  if (!nh.getParam("visualize", visualize_))
    ROS_FATAL("Failed to get the visualize parameter.");

  // Get the offset of the tracker centroid and the body
  if (!nh.getParam("offset", offset_))
    ROS_FATAL("Failed to get the lighthouse transform.");
  if (offset_.size() != 3)
    ROS_FATAL("Failed to parse lighthouse transform.");

  // Get the parent information
  std::vector<std::string> lighthouses;
  if (!nh.getParam("lighthouses", lighthouses))
    ROS_FATAL("Failed to get the lighthouse list.");
  std::vector<std::string>::iterator it;
  for (it = lighthouses.begin(); it != lighthouses.end(); it++) {
    std::string serial;
    if (!nh.getParam(*it + "/serial", serial))
      ROS_FATAL("Failed to get the lighthouse serial.");
    std::vector<double> transform;
    if (!nh.getParam(*it + "/transform", transform))
      ROS_FATAL("Failed to get the lighthouse transform.");
    if (transform.size() != 7) {
      ROS_FATAL("Failed to parse lighthouse transform.");
      continue;
    }
    Eigen::Quaterniond q(transform[6], transform[3], transform[4], transform[5]);
    Eigen::AngleAxisd aa(q);
    lighthouses_[serial].vTl[0] = transform[0];
    lighthouses_[serial].vTl[1] = transform[1];
    lighthouses_[serial].vTl[2] = transform[2];
    lighthouses_[serial].vTl[3] = aa.angle() * aa.axis()[0];
    lighthouses_[serial].vTl[4] = aa.angle() * aa.axis()[1];
    lighthouses_[serial].vTl[5] = aa.angle() * aa.axis()[2];
    lighthouses_[serial].ready = false;
  }

  // Get the parent information
  std::vector<std::string> trackers;
  if (!nh.getParam("trackers", trackers))
    ROS_FATAL("Failed to get the tracker list.");
  std::vector<std::string>::iterator jt;
  for (jt = trackers.begin(); jt != trackers.end(); jt++) {
    std::string serial;
    if (!nh.getParam(*jt + "/serial", serial))
      ROS_FATAL("Failed to get the tracker serial.");
    std::vector<double> extrinsics;
    if (!nh.getParam(*jt + "/extrinsics", extrinsics))
      ROS_FATAL("Failed to get the tracker extrinsics.");
    if (extrinsics.size() != 7) {
      ROS_FATAL("Failed to parse tracker extrinsics.");
      continue;
    }
    Eigen::Quaterniond q( extrinsics[6], extrinsics[3], extrinsics[4], extrinsics[5]);
    Eigen::AngleAxisd aa(q);
    trackers_[serial].bTh[0] = extrinsics[0];
    trackers_[serial].bTh[1] = extrinsics[1];
    trackers_[serial].bTh[2] = extrinsics[2];
    trackers_[serial].bTh[3] = aa.angle() * aa.axis()[0];
    trackers_[serial].bTh[4] = aa.angle() * aa.axis()[1];
    trackers_[serial].bTh[5] = aa.angle() * aa.axis()[2];
    trackers_[serial].ready = false;
    // Publish sensor location and body trajectory
    pub_truth_ = nh.advertise<nav_msgs::Path>("/truth", 10, true);
    pub_sensors_[serial] = nh.advertise<visualization_msgs::MarkerArray>(
      "/sensors/" + *jt, 10, true);
    LighthouseMap::iterator lt;
    for (lt = lighthouses_.begin(); lt != lighthouses_.end(); lt++)
      pub_path_[lt->first][serial] = nh.advertise<nav_msgs::Path>(
        "/path/" + *jt + "/" + lt->first, 10, true);
  }

  // Send the transforms out
  SendTransforms(frame_world_, frame_vive_, frame_body_,
    wTv_, lighthouses_, trackers_);

  // Subscribe to tracker and lighthouse updates
  ros::Subscriber sub_tracker  =
    nh.subscribe<deepdive_ros::Trackers>("/trackers", 1000, std::bind(
      TrackerCallback, std::placeholders::_1, std::ref(trackers_),
        NewTrackerCallback));
  ros::Subscriber sub_lighthouse =
    nh.subscribe<deepdive_ros::Lighthouses>("/lighthouses", 1000, std::bind(
      LighthouseCallback, std::placeholders::_1, std::ref(lighthouses_),
        NewLighthouseCallback));
  ros::Subscriber sub_light =
    nh.subscribe("/light", 1000, LightCallback);
  ros::Subscriber sub_corrections =
    nh.subscribe("/tf", 1000, CorrectionCallback);
  ros::ServiceServer service =
    nh.advertiseService("/trigger", TriggerCallback);

  // Setup a timer to automatically trigger solution on end of experiment
  timer_ = nh.createTimer(ros::Duration(1.0), TimerCallback, true, false);

  // Block until safe shutdown
  ros::spin();

  // Success!
  return 0;
}

ext: cc

max_stars_repo_name: asymingt/deepdive

max_issues_repo_name: asymingt/deepdive

max_forks_repo_name: asymingt/deepdive


text:
/******************************************************************************
Copyright (c) 2019 Georgia Instititue of Technology
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.

Author : Hyoukjun Kwon (hyoukjun@gatech.edu)
*******************************************************************************/

#include <iostream>
#include <memory>
#include <vector>
#include <list>
#include <boost/program_options.hpp>

#include "BASE_constants.hpp"
#include "BASE_base-objects.hpp"
#include "option.hpp"

#include "DFA_tensor.hpp"

#include "AHW-noc-model.hpp"

#include "CA_cost-analysis-results.hpp"

#include "API_configuration.hpp"
#include "API_user-interface-v2.hpp"

#include "DSE_cost-database.hpp"
#include "DSE_design_point.hpp"
#include "DSE_hardware_modules.hpp"
#include "DSE_csv_writer.hpp"

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

  maestro::Options option;
  bool success = option.parse(argc, argv);

  if(!success) {
    std::cout << "[MAESTRO] Failed to parse program options" << std::endl;
  }

  maestro::InitializeBaseObjects(option.message_print_lv);

  int num_pes = option.np;

  /*
   * Hard coded part; will Fix it
   */

  if(option.bw_sweep && option.top_bw_only) {
    int min_bw = option.bw_tick;

    for(int bw = option.min_noc_bw; bw <= option.max_noc_bw; bw += option.bw_tick) {
      std::shared_ptr<std::vector<bool>> noc_multcast = std::make_shared<std::vector<bool>>();
      std::shared_ptr<std::vector<int>> noc_latency = std::make_shared<std::vector<int>>();
      std::shared_ptr<std::vector<int>> noc_bw = std::make_shared<std::vector<int>>();

      if(option.top_bw_only) {
        noc_bw->push_back(bw);
        noc_bw->push_back(70000);
        noc_bw->push_back(70000);
        noc_bw->push_back(70000);
        noc_bw->push_back(70000);
        noc_bw->push_back(70000);

        noc_latency->push_back(option.hop_latency * option.hops);
        noc_latency->push_back(1);
        noc_latency->push_back(1);
        noc_latency->push_back(1);
        noc_latency->push_back(1);
        noc_latency->push_back(1);

        noc_multcast->push_back(option.mc);
        noc_multcast->push_back(true);
        noc_multcast->push_back(true);
        noc_multcast->push_back(true);
        noc_multcast->push_back(true);
        noc_multcast->push_back(true);
      }

      auto config = std::make_shared<maestro::ConfigurationV2>(
          option.dfsl_file_name,
          noc_bw,
          noc_latency,
          noc_multcast,
          option.np,
          option.num_simd_lanes,
          option.bw,
          option.l1_size,
          option.l2_size
          );

      std::cout << "BW: " << bw << std::endl;
      auto api = std::make_shared<maestro::APIV2>(config);
      auto res = api->AnalyzeNeuralNetwork(option.print_res_to_screen, true);

    }
  }
  else {
    std::shared_ptr<std::vector<bool>> noc_multcast = std::make_shared<std::vector<bool>>();
    std::shared_ptr<std::vector<int>> noc_latency = std::make_shared<std::vector<int>>();
    std::shared_ptr<std::vector<int>> noc_bw = std::make_shared<std::vector<int>>();

    noc_bw->push_back(option.bw);
    noc_bw->push_back(option.bw);
    noc_bw->push_back(option.bw);
    noc_bw->push_back(option.bw);

    noc_latency->push_back(option.hop_latency * option.hops);
    noc_latency->push_back(option.hop_latency * option.hops);
    noc_latency->push_back(option.hop_latency * option.hops);
    noc_latency->push_back(option.hop_latency * option.hops);

    noc_multcast->push_back(true);
    noc_multcast->push_back(true);
    noc_multcast->push_back(true);
    noc_multcast->push_back(true);

    auto config = std::make_shared<maestro::ConfigurationV2>(
        option.dfsl_file_name,
        noc_bw,
        noc_latency,
        noc_multcast,
        option.np,
        option.num_simd_lanes,
        option.bw,
        option.l1_size,
        option.l2_size
        );

    auto api = std::make_shared<maestro::APIV2>(config);

    auto res = api->AnalyzeNeuralNetwork(option.print_res_to_screen, option.print_res_to_csv_file, option.print_log_file);
  }
  /////////////////////////////////////////////////////////////////

  return 0;
}

ext: cpp

max_stars_repo_name: YiRuitao/single-path-nas-pytorch

max_issues_repo_name: YiRuitao/Single-Path-NAS-with-MAESTRO

max_forks_repo_name: YiRuitao/Single-Path-NAS-with-MAESTRO


text:
/*
Copyright 2009-2021 Nicolas Colombe

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

#pragma once

#include "math.hpp"
#include "vector2.hpp"

#include <boost/optional.hpp>

namespace eXl
{
  template <class Real>
  class AABB2D
  {
  public:
    inline AABB2D(){}
    inline AABB2D(Real iMinX, Real iMinY, Real iMaxX, Real iMaxY)
    {
      m_Data[0] = Vector2<Real>(iMinX,iMinY);
      m_Data[1] = Vector2<Real>(iMaxX,iMaxY);
    }

    static inline AABB2D FromCenterAndSize(Vector2<Real> iCenter,Vector2<Real> iSize)
    {
      AABB2D ret;
      ret.m_Data[0] = iCenter - iSize / 2.0;
      ret.m_Data[1] = iCenter + iSize / 2.0;
      return ret;
    }

    inline AABB2D(Vector2<Real> iMin,Vector2<Real> iSize)
    {
      m_Data[0] = iMin;
      m_Data[1] = iMin + iSize;
    }

    template <class OtherReal>
    explicit inline AABB2D(AABB2D<OtherReal> const& iOther)
    {
      m_Data[0] = Vector2<Real>(iOther.m_Data[0]);
      m_Data[1] = Vector2<Real>(iOther.m_Data[1]);
    }

    inline bool Empty() const
    {
      return m_Data[0].X() == m_Data[1].X() || m_Data[0].Y() == m_Data[1].Y();
    }

    bool CircleTest(Vector2f const& iPos, Real iRadius)
    {
      if(Contains(iPos))
      {
        return true;
      }
      else
      {
        //Test segment X == X0
        Real dist = m_Data[0].X() - iPos.X();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real y = Math<Real>::Sqrt(dist) + iPos.Y();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].Y() - y && y - m_Data[0].Y() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }
        //Test segment X == X1
        dist = m_Data[1].X() - iPos.X();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real y = Math<Real>::Sqrt(dist) + iPos.Y();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].Y() - y && y - m_Data[0].Y() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }

        //TestSegment Y == Y0
        dist = m_Data[0].Y() - iPos.Y();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real x = Math<Real>::Sqrt(dist) + iPos.X();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].X() - x && x - m_Data[0].X() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }
        //Test segment Y == Y1
        dist = m_Data[1].Y() - iPos.Y();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real x = Math<Real>::Sqrt(dist) + iPos.X();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].X() - x && x - m_Data[0].X() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }
      }
      return false;
    }

    inline void Rotate(Real iAngle)
    {
      Vector2<Real> center = GetCenter();
      Vector2<Real> size = GetSize();
      Vector2<Real> points[4];
      points[0] = size * -0.5;
      points[1] = Vector2<Real>(size.X() * -0.5, size.Y() *  0.5) ;
      points[2] = Vector2<Real>(size.X() *  0.5, size.Y() * -0.5) ;
      points[3] = size * 0.5;
      points[0] = Vector2<Real>(points[0].X() * Math<Real>::Cos(iAngle) + points[0].Y() * Math<Real>::Sin(iAngle)
                         , points[0].Y() * Math<Real>::Cos(iAngle) - points[0].X() * Math<Real>::Sin(iAngle));
      points[1] = Vector2<Real>(points[1].X() * Math<Real>::Cos(iAngle) + points[1].Y() * Math<Real>::Sin(iAngle)
                         , points[1].Y() * Math<Real>::Cos(iAngle) - points[1].X() * Math<Real>::Sin(iAngle));
      points[2] = Vector2<Real>(points[2].X() * Math<Real>::Cos(iAngle) + points[2].Y() * Math<Real>::Sin(iAngle)
                         , points[2].Y() * Math<Real>::Cos(iAngle) - points[2].X() * Math<Real>::Sin(iAngle));
      points[3] = Vector2<Real>(points[3].X() * Math<Real>::Cos(iAngle) + points[3].Y() * Math<Real>::Sin(iAngle)
                         , points[3].Y() * Math<Real>::Cos(iAngle) - points[3].X() * Math<Real>::Sin(iAngle));

      m_Data[0] = m_Data[1] = points[0];
      for (unsigned int i = 1; i < 4; ++i)
      {
        m_Data[0].X() = Math<Real>::Min(m_Data[0].X(), points[i].X());
        m_Data[0].Y() = Math<Real>::Min(m_Data[0].Y(), points[i].Y());
        m_Data[1].X() = Math<Real>::Max(m_Data[1].X(), points[i].X());
        m_Data[1].Y() = Math<Real>::Max(m_Data[1].Y(), points[i].Y());
      }
      m_Data[0] += center;
      m_Data[1] += center;
    }

    inline Vector2<Real> GetCenter()const
    {
      return (m_Data[0] + m_Data[1])/2;
    }

    inline Vector2<Real> GetSize()const
    {
      return m_Data[1] - m_Data[0];
    }

    inline void Inflate(Real iCoeff)
    {
      Vector2<Real> center = (m_Data[1] + m_Data[0]) / 2;
      Vector2<Real> size = m_Data[1] - m_Data[0];
      size = size * iCoeff;
      m_Data[0] = center - size / 2.0;
      m_Data[1] = center + size / 2.0;
    }

    inline Real MinX()const{return m_Data[0].X();}
    inline Real MinY()const{return m_Data[0].Y();}
    inline Real MaxX()const{return m_Data[1].X();}
    inline Real MaxY()const{return m_Data[1].Y();}

    inline Real& MinX(){return m_Data[0].X();}
    inline Real& MinY(){return m_Data[0].Y();}
    inline Real& MaxX(){return m_Data[1].X();}
    inline Real& MaxY(){return m_Data[1].Y();}

    inline bool operator ==(AABB2D const& iOther) const
    {
      return m_Data[0]==iOther.m_Data[0] && m_Data[1] == iOther.m_Data[1];
    }

    inline bool operator !=(AABB2D const& iOther) const
    {
      return !((*this) == iOther);
    }

	  inline bool Empty()
	  {
		  return m_Data[0].X() == m_Data[1].X() || m_Data[0].Y() == m_Data[1].Y();
	  }

    inline void SetCommonBox(AABB2D<Real> const& iBox1,AABB2D<Real> const& iBox2)
    {
      m_Data[0].X() = Math<Real>::Max(iBox1.m_Data[0].X(),iBox2.m_Data[0].X());
      m_Data[0].Y() = Math<Real>::Max(iBox1.m_Data[0].Y(),iBox2.m_Data[0].Y());
      m_Data[1].X() = Math<Real>::Max(Math<Real>::Min(iBox1.m_Data[1].X(),iBox2.m_Data[1].X()),m_Data[0].X());
      m_Data[1].Y() = Math<Real>::Max(Math<Real>::Min(iBox1.m_Data[1].Y(),iBox2.m_Data[1].Y()),m_Data[0].Y());
    }

    inline void Absorb(Vector2<Real> const& iPoint)
    {
      m_Data[0].X() = Math<Real>::Min(m_Data[0].X(), iPoint.X());
      m_Data[0].Y() = Math<Real>::Min(m_Data[0].Y(), iPoint.Y());
      m_Data[1].X() = Math<Real>::Max(m_Data[1].X(), iPoint.X());
      m_Data[1].Y() = Math<Real>::Max(m_Data[1].Y(), iPoint.Y());
    }

    inline void Absorb(AABB2D<Real> const& iBox)
    {
      m_Data[0].X() = Math<Real>::Min(m_Data[0].X(),iBox.m_Data[0].X());
      m_Data[0].Y() = Math<Real>::Min(m_Data[0].Y(),iBox.m_Data[0].Y());
      m_Data[1].X() = Math<Real>::Max(m_Data[1].X(),iBox.m_Data[1].X());
      m_Data[1].Y() = Math<Real>::Max(m_Data[1].Y(),iBox.m_Data[1].Y());
    }

    inline bool IsInside(AABB2D const& iOther, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      return (m_Data[0].X() - iOther.m_Data[0].X() >= iEpsilon && m_Data[0].Y() - iOther.m_Data[0].Y() >= iEpsilon
           && iOther.m_Data[1].X() - m_Data[1].X() >= iEpsilon && iOther.m_Data[1].Y() - m_Data[1].Y() >= iEpsilon );
    }

    inline bool Intersect(AABB2D const& iOther, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      return iOther.m_Data[1].X() - m_Data[0].X() >= iEpsilon && m_Data[1].X() - iOther.m_Data[0].X() >= iEpsilon
          && iOther.m_Data[1].Y() - m_Data[0].Y() >= iEpsilon && m_Data[1].Y() - iOther.m_Data[0].Y() >= iEpsilon;
    }

    //0 No, 1 Left, 2 Right, 3 Down, 4 Up
    inline int Touch(AABB2D const& iOther, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      if(iOther.m_Data[1].Y() - m_Data[0].Y() >= iEpsilon && m_Data[1].Y() - iOther.m_Data[0].Y() >= iEpsilon)
      {
        Real val = m_Data[0].X() - iOther.m_Data[1].X();
        if(val <= iEpsilon && val >= -iEpsilon)
          return 1;

        val = m_Data[1].X() - iOther.m_Data[0].X();
        if(val <= iEpsilon && val >= -iEpsilon)
          return 2;
      }
      if(iOther.m_Data[1].X() - m_Data[0].X() >= iEpsilon && m_Data[1].X() - iOther.m_Data[0].X() >= iEpsilon)
      {
        Real val = m_Data[0].Y() - iOther.m_Data[1].Y();

        if(val <= iEpsilon && val >= -iEpsilon)
          return 3;

        val = m_Data[1].Y() - iOther.m_Data[0].Y();
        if(val <= iEpsilon && val >= -iEpsilon)
          return 4;
      }
      return 0;
    }

    boost::optional<Vector2<Real>> SegmentTest(Vector2<Real> const& iOrigin, Vector2<Real> const& iDir, Real iEpsilon = Math<Real>::ZERO_TOLERANCE)
    {
      Real const scaleX = 1.0 / iDir.X();
      Real const scaleY = 1.0 / iDir.Y();
      Real const signX = Math<Real>::Sign(scaleX);
      Real const signY = Math<Real>::Sign(scaleY);

      Real const nearTimeX = ((signX > 0 ? m_Data[0].X() : m_Data[1].X()) - iOrigin.X()) * scaleX;
      Real const nearTimeY = ((signY > 0 ? m_Data[0].Y() : m_Data[1].Y()) - iOrigin.Y()) * scaleY;
      Real const farTimeX = ((signX > 0 ? m_Data[1].X() : m_Data[0].X()) - iOrigin.X()) * scaleX;
      Real const farTimeY = ((signY > 0 ? m_Data[1].Y() : m_Data[0].Y()) - iOrigin.Y()) * scaleY;

      if (nearTimeX > farTimeY || nearTimeY > farTimeX)
      {
        return boost::none;
      }

      Real const nearTime = nearTimeX > nearTimeY ? nearTimeX : nearTimeY;
      Real const farTime = farTimeX < farTimeY ? farTimeX : farTimeY;

      if (nearTime > 1 - iEpsilon || farTime < -iEpsilon)
      {
        return boost::none;
      }

      return iOrigin + iDir * nearTime;
    }

    inline bool Contains(Vector2<Real>const& iPoint, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      return iPoint.X() - m_Data[0].X() >= iEpsilon && m_Data[1].X() - iPoint.X() >= iEpsilon
          && iPoint.Y() - m_Data[0].Y() >= iEpsilon && m_Data[1].Y() - iPoint.Y() >= iEpsilon;
    }

    // Neg value -> distance to min, Pos value -> distance to max, 0, inside
    Vector2f Classify(Vector2<Real>const& iPoint, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      Vector2f res(iPoint.X() - m_Data[0].X(), iPoint.Y() - m_Data[0].Y());
      for(uint32_t axis = 0; axis < 2; ++axis)
      {
        if(!(res.m_Data[axis] < -iEpsilon))
        {
          res.m_Data[axis] = m_Data[1].m_Data[axis] - iPoint.m_Data[axis];
          if(res.m_Data[axis] < -iEpsilon)
          {
            res.m_Data[axis] *= -1;
          }
          else
          {
            res.m_Data[axis] = 0;
          }
        }
      }

      return res;
    }

    inline Vector2<Real> GetDistX(AABB2D const& iOther) const
    {
      return Vector2<Real>(iOther.m_Data[0].X() - m_Data[0].X(),m_Data[1].X() - iOther.m_Data[1].X());
    }

    inline Vector2<Real> GetDistY(AABB2D const& iOther) const
    {
      return Vector2<Real>(iOther.m_Data[0].Y() - m_Data[0].Y(),m_Data[1].Y() - iOther.m_Data[1].Y());
    }

    Vector2<Real> m_Data[2];
  };

  typedef AABB2D<int> AABB2Di;
  typedef AABB2D<float> AABB2Df;
  typedef AABB2D<double> AABB2Dd;
}

ext: hpp

max_stars_repo_name: eXl-Nic/eXl

max_issues_repo_name: eXl-Nic/eXl

max_forks_repo_name: eXl-Nic/eXl


text:
//---------------------------------------------------------------------------
//
// Example of redistribution using the Round Redistribution component
//
// Matthieu Dreher
// Argonne National Laboratory
// 9700 S. Cass Ave.
// Argonne, IL 60439
// mdreher@anl.gov
//
//--------------------------------------------------------------------------

#include <boost/iostreams/device/back_inserter.hpp>
#include <boost/iostreams/stream.hpp>

#include <bredala/data_model/constructtype.h>
#include <bredala/data_model/simpleconstructdata.hpp>
#include <bredala/data_model/vectorconstructdata.hpp>
#include <bredala/data_model/boost_macros.h>

#include <boost/serialization/serialization.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/string.hpp>

#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>

#include <bredala/transport/mpi/redist_round_mpi.h>

#include <assert.h>
#include <math.h>
#include <stddef.h>
#include <stdint.h>
#include <mpi.h>
#include <sstream>
#include <fstream>

using namespace decaf;
using namespace std;

void printArray(const std::vector<int>& array)
{
    std::cout<<"[";
    for(unsigned int i = 0; i < array.size(); i++)
        std::cout<<array[i]<<",";
    std::cout<<"]"<<std::endl;
}

bool isBetween(int rank, int start, int nb)
{
    return rank >= start && rank < start + nb;
}

void runTestParallelRedistOverlap(int startSource, int nbSource, int startReceptors, int nbReceptors)
{
    int size_world, rank;
    MPI_Comm_size(MPI_COMM_WORLD, &size_world);
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);

    if(size_world < max(startSource + nbSource, startReceptors + nbReceptors))
    {
        std::cout<<"ERROR : not enough rank for "<<nbSource<<" sources and "<<nbReceptors<<" receivers"<<std::endl;
        return;
    }

    if(!isBetween(rank, startSource, nbSource) && !isBetween(rank, startReceptors, nbReceptors))
        return;

    std::cout<<"Construction of the redistribution component."<<std::endl;
    std::cout<<"Rank : "<<rank<<std::endl;
    std::cout<<"Configuration : "<<startSource<<", "<<nbSource<<", "<<startReceptors<<", "<<nbReceptors<<std::endl;
    RedistRoundMPI *component = new RedistRoundMPI(startSource, nbSource,
                                                     startReceptors, nbReceptors,
                                                     MPI_COMM_WORLD, DECAF_REDIST_P2P, DECAF_REDIST_MERGE_ONCE);

    std::cout<<"-------------------------------------"<<std::endl;
    std::cout<<"Test with Redistribution component with overlapping..."<<std::endl;
    std::cout<<"-------------------------------------"<<std::endl;

    if(isBetween(rank, startSource, nbSource)){
        std::cout<<"Running Redistributed test between "<<nbSource<<" producers"
                   "and "<<nbReceptors<<" consummers"<<std::endl;

        unsigned int nbParticules = 11;
        std::vector<int> index = std::vector<int>(nbParticules);
        for(int i = 0; i< nbParticules; i++)
            index[i] = rank*nbParticules + i;

        printArray(index);

        std::shared_ptr<VectorConstructData<int> > array = std::make_shared<VectorConstructData<int> >( index, 1 );

        pConstructData container;

        container->appendData(std::string("index"), array,
                             DECAF_POS, DECAF_PRIVATE,
                             DECAF_SPLIT_DEFAULT, DECAF_MERGE_APPEND_VALUES);

        component->process(container, decaf::DECAF_REDIST_SOURCE);

    }
    if(isBetween(rank, startReceptors, nbReceptors))
    {
        pConstructData result;
        component->process(result, decaf::DECAF_REDIST_DEST);

        std::cout<<"==========================="<<std::endl;
        std::cout<<"Final Merged map has "<<result->getNbItems()<<" items."<<std::endl;
        std::cout<<"Final Merged map has "<<result->getMap()->size()<<" fields."<<std::endl;
        std::shared_ptr<VectorConstructData<int> > index = result->getTypedData<VectorConstructData<int> >("index");
        printArray(index->getVector());
        std::cout<<"==========================="<<std::endl;
        std::cout<<"Simple test between "<<nbSource<<" producers and "<<nbReceptors<<" consummer completed"<<std::endl;
    }

    component->flush();

    delete component;

    std::cout<<"-------------------------------------"<<std::endl;
    std::cout<<"Test with Redistribution component with overlapping completed"<<std::endl;
    std::cout<<"-------------------------------------"<<std::endl;
}

int main(int argc,
         char** argv)
{
    MPI_Init(NULL, NULL);

    char processorName[MPI_MAX_PROCESSOR_NAME];
    int size_world, rank, nameLen;

    MPI_Comm_size(MPI_COMM_WORLD, &size_world);
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    MPI_Get_processor_name(processorName,&nameLen);

    srand(time(NULL) + rank * size_world + nameLen);

    //runTestParallelRedistOverlap(0, 4, 4, 4);
    //runTestParallelRedistOverlap(0, 4, 4, 3);
    //runTestParallelRedistOverlap(0, 4, 2, 2);
    //runTestParallelRedistOverlap(0, 4, 2, 3);
    //runTestParallelRedistOverlap(0, 4, 0, 4);
    runTestParallelRedistOverlap(2, 3, 0, 2);

    MPI_Barrier(MPI_COMM_WORLD);
    MPI_Finalize();

    return 0;
}

max_stars_repo_name: tpeterka/decaf

max_issues_repo_name: tpeterka/decaf

max_forks_repo_name: tpeterka/decaf


text:
/**********************************************************************
*  Copyright (c) 2008-2016, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include "Geometry.hpp"
#include "Intersection.hpp"
#include "../data/Matrix.hpp"
#include "../core/Assert.hpp"
#include "../core/Logger.hpp"

#undef BOOST_UBLAS_TYPE_CHECK
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_franklin.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_crossings_multiply.hpp>
#include <boost/geometry/algorithms/within.hpp>

typedef boost::geometry::model::d2::point_xy<double> BoostPoint;
typedef boost::geometry::model::polygon<BoostPoint> BoostPolygon;
typedef boost::geometry::model::ring<BoostPoint> BoostRing;
typedef boost::geometry::model::multi_polygon<BoostPolygon> BoostMultiPolygon;

#include <polypartition/polypartition.h>

#include <list>

// remove_spikes
// adapted from https://github.com/boostorg/geometry/commits/develop/include/boost/geometry/algorithms/remove_spikes.hpp eb3260708eb241d8da337f4be73b41d69d33cd09

/*
Remove spikes from a ring/polygon.
Ring (having 8 vertices, including closing vertex)
+------+
| |
| +--+
| | ^this "spike" is removed, can be located outside/inside the ring
+------+
(the actual determination if it is removed is done by a strategy)

*/

namespace openstudio {

  template <typename Point1, typename Point2, typename Point3>
  static inline bool point_is_spike_or_equal(Point1 const& last_point, Point2 const& segment_a, Point3 const& segment_b)
  {
    // adapted from boost\geometry\algorithms\detail\point_is_spike_or_equal.hpp to include tolerance checking

    // segment_a is at the beginning
    // segment_b is in the middle
    // last_point is at the end

    // segment_b is being considered for deletion

    double normTol = 0.001; // 1 mm
    double tol = 0.001; // relative to 1

    double diff1_x = last_point.x()-segment_b.x();
    double diff1_y = last_point.y()-segment_b.y();
    double norm1 = sqrt(pow(diff1_x, 2) + pow(diff1_y, 2));
    if (norm1 > normTol){
      diff1_x = diff1_x/norm1;
      diff1_y = diff1_y/norm1;
    }else{
      // last point is too close to segment b
      return true;
    }

    double diff2_x = segment_b.x()-segment_a.x();
    double diff2_y = segment_b.y()-segment_a.y();
    double norm2 = sqrt(pow(diff2_x, 2) + pow(diff2_y, 2));
    if (norm2 > normTol){
      diff2_x = diff2_x/norm2;
      diff2_y = diff2_y/norm2;
    }else{
      // segment b is too close to segment a
      return true;
    }

    double crossProduct = diff1_x*diff2_y-diff1_y*diff2_x;
    if (abs(crossProduct) < tol){
      double dotProduct = diff1_x*diff2_x+diff1_y*diff2_y;
      if (dotProduct <= -1.0 + tol){
        // reversal
        return true;
      }
    }

    return false;
  }

}

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace remove_spikes
{

template <typename Range>
struct range_remove_spikes
{
    typedef typename strategy::side::services::default_strategy
    <
        typename cs_tag<Range>::type
    >::type side_strategy;

    typedef typename coordinate_type<Range>::type coordinate_type;
    typedef typename point_type<Range>::type point_type;

    static inline void apply(Range& range)
    {
        std::size_t n = boost::size(range);
        std::size_t const min_num_points = core_detail::closure::minimum_ring_size
            <
                geometry::closure<Range>::value
            >::value;
        if (n < min_num_points)
        {
            return;
        }

        std::deque<point_type> cleaned;
        for (typename boost::range_iterator<Range const>::type it = boost::begin(range);
            it != boost::end(range); ++it)
        {
            // Add point
            cleaned.push_back(*it);

            while(cleaned.size() >= 3
                    && openstudio::point_is_spike_or_equal(cleaned.back(), *(cleaned.end() - 3), *(cleaned.end() - 2)))
            {
                // Remove pen-ultimate point causing the spike (or which was equal)
                cleaned.erase(cleaned.end() - 2);
            }
        }

        // For a closed-polygon, remove closing point, this makes checking first point(s) easier and consistent
        if (geometry::closure<Range>::value == geometry::closed)
        {
            cleaned.pop_back();
        }

        bool found = false;
        do
        {
            found = false;
            // Check for spike in first point
            int const penultimate = 2;
            while(cleaned.size() > 3 && openstudio::point_is_spike_or_equal(cleaned.front(), *(cleaned.end() - penultimate), cleaned.back()))
            {
                cleaned.pop_back();
                found = true;
            }
            // Check for spike in second point
            while(cleaned.size() > 3 && openstudio::point_is_spike_or_equal(*(cleaned.begin() + 1), cleaned.back(), cleaned.front()))
            {
                cleaned.pop_front();
                found = true;
            }
        }
        while (found);

        // Close if necessary
        if (geometry::closure<Range>::value == geometry::closed)
        {
            cleaned.push_back(cleaned.front());
        }

        // Copy output
        geometry::clear(range);
        std::copy(cleaned.begin(), cleaned.end(), std::back_inserter(range));
    }
};

template <typename Polygon>
struct polygon_remove_spikes
{
    static inline void apply(Polygon& polygon)
    {
        typedef typename geometry::ring_type<Polygon>::type ring_type;

        typedef range_remove_spikes<ring_type> per_range;
        per_range::apply(exterior_ring(polygon));

        typename interior_return_type<Polygon>::type rings
                    = interior_rings(polygon);
        for (BOOST_AUTO_TPL(it, boost::begin(rings)); it != boost::end(rings); ++it)
        {
            per_range::apply(*it);
        }
    }
};

}} // namespace detail::remove_spikes
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct remove_spikes
{
    static inline void apply(Geometry&)
    {}
};

template <typename Ring>
struct remove_spikes<Ring, ring_tag>
    : detail::remove_spikes::range_remove_spikes<Ring>
{};

template <typename Polygon>
struct remove_spikes<Polygon, polygon_tag>
    : detail::remove_spikes::polygon_remove_spikes<Polygon>
{};

} // namespace dispatch
#endif

namespace resolve_variant {

template <typename Geometry>
struct remove_spikes
{
    static void apply(Geometry& geometry)
    {
        concept::check<Geometry>();
        dispatch::remove_spikes<Geometry>::apply(geometry);
    }
};

template <BOOST_VARIANT_ENUM_PARAMS(typename T)>
struct remove_spikes<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> >
{
    struct visitor: boost::static_visitor<void>
    {
        template <typename Geometry>
        void operator()(Geometry& geometry) const
        {
            remove_spikes<Geometry>::apply(geometry);
        }
    };

    static inline void apply(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)>& geometry)
    {
        boost::apply_visitor(visitor(), geometry);
    }
};

} // namespace resolve_variant

/*!
\ingroup remove_spikes
\tparam Geometry geometry type
\param geometry the geometry to make remove_spikes
*/
template <typename Geometry>
inline void remove_spikes(Geometry& geometry)
{
    resolve_variant::remove_spikes<Geometry>::apply(geometry);
}

}} // namespace boost::geometry
// remove_spikes

namespace openstudio{

  // Private implementation functions

  BoostPolygon removeSpikes(const BoostPolygon& polygon)
  {
    BoostPolygon temp(polygon);
    boost::geometry::remove_spikes(temp);
    return temp;
  }

  std::vector<BoostPolygon> removeSpikes(const std::vector<BoostPolygon>& polygons)
  {
    std::vector<BoostPolygon> result;
    for (const BoostPolygon& polygon : polygons){
      result.push_back(removeSpikes(polygon));
    }
    return result;
  }

  std::vector<BoostPolygon> removeHoles(const BoostPolygon& boostPolygon)
  {
    std::vector<BoostPolygon> result;

    // convert to vector of TPPLPoly
    std::list<TPPLPoly> polys;

    BoostRing outer = boostPolygon.outer();
    TPPLPoly outerPoly; // must be counter-clockwise
    outerPoly.Init(outer.size() - 1);
    outerPoly.SetHole(false);
    //std::cout << "outer :";
    for(unsigned i = 0; i < outer.size() - 1; ++i){
      outerPoly[i].x = outer[i].x();
      outerPoly[i].y = outer[i].y();
      //std::cout << "(" << outer[i].x() << ", " << outer[i].y() << ") ";
    }
    //std::cout << std::endl;
    outerPoly.SetOrientation(TPPL_CCW);
    polys.push_back(outerPoly);

    std::vector<BoostRing> inners = boostPolygon.inners();
    for (const BoostRing& inner : inners){
      TPPLPoly innerPoly; // must be clockwise
      innerPoly.Init(inner.size() - 1);
      innerPoly.SetHole(true);
      //std::cout << "inner :";
      for(unsigned i = 0; i < inner.size() - 1; ++i){
        innerPoly[i].x = inner[i].x();
        innerPoly[i].y = inner[i].y();
        //std::cout << "(" << inner[i].x() << ", " << inner[i].y() << ") ";
      }
      //std::cout << std::endl;
      innerPoly.SetOrientation(TPPL_CW);
      polys.push_back(innerPoly);
    }

    // do partitioning
    TPPLPartition pp;
    std::list<TPPLPoly> resultPolys;
    int test = pp.ConvexPartition_HM(&polys,&resultPolys);
    if (test == 0){
      LOG_FREE(Error, "utilities.geometry.removeHoles", "Failed to partition polygon");
      return result;
    }

    // convert back to BoostPolygon
    std::list<TPPLPoly>::iterator it, itend;
    for(it = resultPolys.begin(), itend = resultPolys.end(); it != itend; ++it){
      BoostPolygon newBoostPolygon;
      //std::cout << "result :";
      for (long i = 0; i < it->GetNumPoints(); ++i){
        TPPLPoint point = it->GetPoint(i);
        boost::geometry::append(newBoostPolygon, boost::make_tuple(point.x, point.y));
        //std::cout << "(" << point.x << ", " << point.y << ") ";
      }
      TPPLPoint point = it->GetPoint(0);
      boost::geometry::append(newBoostPolygon, boost::make_tuple(point.x, point.y));
      //std::cout << "(" << point.x << ", " << point.y << ") ";
      //std::cout << std::endl;

      boost::geometry::correct(newBoostPolygon);
      result.push_back(newBoostPolygon);
    }

    return result;
  }

  std::vector<BoostPolygon> removeHoles(const std::vector<BoostPolygon>& polygons)
  {
    std::vector<BoostPolygon> result;
    for (const BoostPolygon polygon : polygons){
      if (polygon.inners().empty()){
        // DLM: might also want to partition if this polygon is self intersecting?
        result.push_back(polygon);
      }else{
        std::vector<BoostPolygon> temp = removeHoles(polygon);
        result.insert(result.end(), temp.begin(), temp.end());
      }
    }
    return result;
  }

  // convert a Point3d to a BoostPoint
  boost::tuple<double, double> boostPointFromPoint3d(const Point3d& point3d, std::vector<Point3d>& allPoints, double tol)
  {
    OS_ASSERT(abs(point3d.z()) <= tol);

    // simple method
    //return boost::make_tuple(point3d.x(), point3d.y());

    // detailed method, try to combine points within tolerance
    Point3d resultPoint = getCombinedPoint(point3d, allPoints, tol);

    return boost::make_tuple(resultPoint.x(), resultPoint.y());
  }

  // convert vertices to a boost polygon, all vertices must lie on z = 0 plane
  boost::optional<BoostPolygon> boostPolygonFromVertices(const std::vector<Point3d>& vertices, std::vector<Point3d>& allPoints, double tol)
  {
    if (vertices.size () < 3){
      return boost::none;
    }

    BoostPolygon polygon;
    for (const Point3d& vertex : vertices){

      // should all have zero z coordinate now
      double z = vertex.z();
      if (abs(z) > tol){
        LOG_FREE(Error, "utilities.geometry.boostPolygonFromVertices", "All points must be on z = 0 plane");
        return boost::none;
      }

      // use helper method which combines close points
      boost::geometry::append(polygon, boostPointFromPoint3d(vertex, allPoints, tol));
    }

    // close polygon, use helper method which combines close points
    boost::geometry::append(polygon, boostPointFromPoint3d(vertices[0], allPoints, tol));

    //boost::geometry::correct(polygon);

    boost::optional<double> testArea = boost::geometry::area(polygon);
    if (!testArea || (*testArea < 0)){
      // DLM: we could offer to reverse these vertices here but that might not be the best idea
      return boost::none;
    }

    return polygon;
  }

  boost::optional<BoostPolygon> nonIntersectingBoostPolygonFromVertices(const std::vector<Point3d>& polygon, std::vector<Point3d>& allPoints, double tol)
  {
    boost::optional<BoostPolygon> result = boostPolygonFromVertices(polygon, allPoints, tol);
    if (!result){
      return boost::none;
    }
    // check if polygon overlaps itself
    try{
      boost::geometry::detail::overlay::has_self_intersections(*result);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      //LOG_FREE(Error, "utilities.geometry.nonIntersectingBoostPolygonFromVertices", "Self intersecting polygon");
      return boost::none;
    }
    return result;
  }

  // convert vertices to a boost ring, all vertices must lie on z = 0 plane
  boost::optional<BoostRing> boostRingFromVertices(const std::vector<Point3d>& vertices, std::vector<Point3d>& allPoints, double tol)
  {
    if (vertices.size () < 3){
      return boost::none;
    }

    BoostRing ring;
    for (const Point3d& vertex : vertices){

      // should all have zero z coordinate now
      double z = vertex.z();
      if (abs(z) > tol){
        LOG_FREE(Error, "utilities.geometry.boostRingFromVertices", "All points must be on z = 0 plane");
        return boost::none;
      }

      // use helper method which combines close points
      boost::geometry::append(ring, boostPointFromPoint3d(vertex, allPoints, tol));
    }

    // close polygon, use helper method which combines close points
    boost::geometry::append(ring, boostPointFromPoint3d(vertices[0], allPoints, tol));

    //boost::geometry::correct(ring);

    boost::optional<double> testArea = boost::geometry::area(ring);
    if (!testArea || (*testArea < 0)){
      // DLM: we could offer to reverse these vertices here but that might not be the best idea
      return boost::none;
    }

    return ring;
  }

  boost::optional<BoostRing> nonIntersectingBoostRingFromVertices(const std::vector<Point3d>& polygon, std::vector<Point3d>& allPoints, double tol)
  {
    boost::optional<BoostRing> result = boostRingFromVertices(polygon, allPoints, tol);
    if (!result){
      return boost::none;
    }
    // check if polygon overlaps itself
    try{
      boost::geometry::detail::overlay::has_self_intersections(*result);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      //LOG_FREE(Error, "utilities.geometry.nonIntersectingBoostRingFromVertices", "Self intersecting polygon");
      return boost::none;
    }
    return result;
  }

  // convert a boost polygon to vertices
  std::vector<Point3d> verticesFromBoostPolygon(const BoostPolygon& polygon, std::vector<Point3d>& allPoints, double tol)
  {
    std::vector<Point3d> result;

    BoostRing outer = polygon.outer();
    if (outer.empty()){
      return result;
    }

    // add point for each vertex except final vertex
    for(unsigned i = 0; i < outer.size() - 1; ++i){
      Point3d point3d(outer[i].x(), outer[i].y(), 0.0);

      // try to combine points within tolerance
      Point3d resultPoint = getCombinedPoint(point3d, allPoints, tol);

      // don't keep repeated vertices
      if ((i > 0) && (result.back() == resultPoint)){
        continue;
      }
      result.push_back(resultPoint);
    }

    OS_ASSERT(polygon.inners().empty());

    result = removeCollinear(result);

    // don't keep repeated vertices
    if (result.front() == result.back()){
      result.pop_back();
    }

    if (result.size() < 3){
      return std::vector<Point3d>();
    }

    return result;
  }

  // convert a boost ring to vertices
  std::vector<Point3d> verticesFromBoostRing(const BoostRing& ring, std::vector<Point3d>& allPoints, double tol)
  {
    std::vector<Point3d> result;

    // add point for each vertex except final vertex
    for (unsigned i = 0; i < ring.size() - 1; ++i){
      Point3d point3d(ring[i].x(), ring[i].y(), 0.0);

      // try to combine points within tolerance
      Point3d resultPoint = getCombinedPoint(point3d, allPoints, tol);

      // don't keep repeated vertices
      if ((i > 0) && (result.back() == resultPoint)){
        continue;
      }
      result.push_back(resultPoint);
    }

    result = removeCollinear(result);

    // don't keep repeated vertices
    if (result.front() == result.back()){
      result.pop_back();
    }

    if (result.size() < 3){
      return std::vector<Point3d>();
    }

    return result;
  }

  // struct used to sort polygons in descending order by area
  struct BoostPolygonAreaGreater{
    bool operator()(const BoostPolygon& left, const BoostPolygon& right){
      boost::optional<double> leftA = boost::geometry::area(left);
      if (!leftA){
        leftA = 0;
      }
      boost::optional<double> rightA = boost::geometry::area(right);
      if (!rightA){
        rightA = 0;
      }
      return (*leftA > *rightA);
    }
  };

  // Public functions

  IntersectionResult::IntersectionResult(const std::vector<Point3d>& polygon1,
                                         const std::vector<Point3d>& polygon2,
                                         const std::vector< std::vector<Point3d> >& newPolygons1,
                                         const std::vector< std::vector<Point3d> >& newPolygons2)
    : m_polygon1(polygon1), m_polygon2(polygon2), m_newPolygons1(newPolygons1), m_newPolygons2(newPolygons2)
  {
  }

  std::vector<Point3d> IntersectionResult::polygon1() const
  {
    return m_polygon1;
  }

  std::vector<Point3d> IntersectionResult::polygon2() const
  {
    return m_polygon2;
  }

  std::vector< std::vector<Point3d> > IntersectionResult::newPolygons1() const
  {
    return m_newPolygons1;
  }

  std::vector< std::vector<Point3d> > IntersectionResult::newPolygons2() const
  {
    return m_newPolygons2;
  }

  std::vector<Point3d> removeSpikes(const std::vector<Point3d>& polygon, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> boostPolygon = boostPolygonFromVertices(polygon, allPoints, tol);
    if (!boostPolygon){
      return std::vector<Point3d>();
    }

    BoostPolygon boostResult = removeSpikes(*boostPolygon);

    std::vector<Point3d> result = verticesFromBoostPolygon(boostResult, allPoints, tol);

    return result;
  }

  bool pointInPolygon(const Point3d& point, const std::vector<Point3d>& polygon, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostRing> boostPolygon = nonIntersectingBoostRingFromVertices(polygon, allPoints, tol);
    if (!boostPolygon){
      return false;
    }

    if (abs(point.z()) > tol){
      return false;
    }

    boost::tuple<double, double> p = boostPointFromPoint3d(point, allPoints, tol);
    BoostPoint boostPoint(p.get<0>(), p.get<1>());

    //boost::geometry::strategy::within::winding<BoostPoint> strategy;
    //boost::geometry::strategy::within::franklin<BoostPoint> strategy;
    //boost::geometry::strategy::within::crossings_multiply<BoostPoint> strategy;
    //bool result = boost::geometry::within(boostPoint, *boostPolygon, strategy);

    //bool result = boost::geometry::intersects(boostPoint, *boostPolygon);

    //bool result = boost::geometry::overlaps(boostPoint, *boostPolygon);

    double distance = boost::geometry::distance(boostPoint, *boostPolygon);
    bool result = (distance <= 0.0001);

    return result;

  }

  boost::optional<std::vector<Point3d> > join(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostRing> boostPolygon1 = nonIntersectingBoostRingFromVertices(polygon1, allPoints, tol);
    if (!boostPolygon1){
      return boost::none;
    }

    boost::optional<BoostRing> boostPolygon2 = nonIntersectingBoostRingFromVertices(polygon2, allPoints, tol);
    if (!boostPolygon2){
      return boost::none;
    }

    // union the points in face coordinates,
    std::vector<BoostPolygon> unionResult;
    try{
      boost::geometry::union_(*boostPolygon1, *boostPolygon2, unionResult);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.join", "overlay_invalid_input_exception");
      return boost::none;
    }

    unionResult = removeSpikes(unionResult);

    // should not be any holes, check for that below

    // check that union is ok
    if (unionResult.empty()){
      return boost::none;
    }else if (unionResult.size() > 1){
      return boost::none;
    }

    std::vector<Point3d> unionVertices = verticesFromBoostPolygon(unionResult[0], allPoints, tol);
    boost::optional<double> testArea = boost::geometry::area(unionResult[0]);
    if (!testArea || unionVertices.empty()){
      LOG_FREE(Info, "utilities.geometry.join", "Cannot compute area of union");
      return boost::none;
    }else if (*testArea < tol*tol){
      LOG_FREE(Info, "utilities.geometry.join", "Union has very small area of " << *testArea << " m^2");
      return boost::none;
    }
    try{
      boost::geometry::detail::overlay::has_self_intersections(unionResult[0]);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.join", "Union is self intersecting");
      return boost::none;
    }

    // check for holes
    if (!unionResult[0].inners().empty()){
      LOG_FREE(Error, "utilities.geometry.join", "Union has inner loops");
      return boost::none;
    };

    unionVertices = reorderULC(unionVertices);
    unionVertices = removeCollinear(unionVertices);

    return unionVertices;
  }

  std::vector<std::vector<Point3d> > joinAll(const std::vector<std::vector<Point3d> >& polygons, double tol)
  {
    std::vector<std::vector<Point3d> > result;

    unsigned N = polygons.size();
    if (N <= 1){
      return polygons;
    }

    // compute adjacency matrix
    Matrix A(N,N,0.0);
    for (unsigned i = 0; i < polygons.size(); ++i){
      A(i,i) = 1.0;
      for (unsigned j = i+1; j < polygons.size(); ++j){
        if (join(polygons[i], polygons[j], tol)){
          A(i,j) = 1.0;
          A(j,i) = 1.0;
        }
      }
    }

    std::vector<std::vector<unsigned> > connectedComponents = findConnectedComponents(A);
    for (const std::vector<unsigned>& component : connectedComponents){
      std::vector<Point3d> points;
      for (unsigned i : component){
        if (points.empty()){
          points = polygons[i];
        }else{
          boost::optional<std::vector<Point3d> > joined = join(points, polygons[i], tol);
          if (!joined){
            LOG_FREE(Error, "utilities.geometry.joinAll", "Expected polygons to join together");
          }else{
            points = *joined;
          }
        }
      }
      result.push_back(points);
    }

    return result;
  }

  boost::optional<IntersectionResult> intersect(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol)
  {
    std::vector<Point3d> resultPolygon1;
    std::vector<Point3d> resultPolygon2;
    std::vector< std::vector<Point3d> > newPolygons1;
    std::vector< std::vector<Point3d> > newPolygons2;

    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostRing> boostPolygon1 = nonIntersectingBoostRingFromVertices(polygon1, allPoints, tol);
    if (!boostPolygon1){
      return boost::none;
    }

    boost::optional<BoostRing> boostPolygon2 = nonIntersectingBoostRingFromVertices(polygon2, allPoints, tol);
    if (!boostPolygon2){
      return boost::none;
    }

    // intersect the points in face coordinates,
    std::vector<BoostPolygon> intersectionResult;
    try{
      boost::geometry::intersection(*boostPolygon1, *boostPolygon2, intersectionResult);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.intersect", "overlay_invalid_input_exception");
      return boost::none;
    }

    // check if intersection is empty
    if (intersectionResult.empty()){
      //LOG_FREE(Info, "utilities.geometry.intersect", "Intersection is empty");
      return boost::none;
    }

    intersectionResult = removeSpikes(intersectionResult);
    intersectionResult = removeHoles(intersectionResult);

    // check for multiple intersections
    if (intersectionResult.size() > 1){
      LOG_FREE(Info, "utilities.geometry.intersect", "Intersection has " << intersectionResult.size() << " elements");
      std::sort(intersectionResult.begin(), intersectionResult.end(), BoostPolygonAreaGreater());
    }

    // check that largest intersection is ok
    std::vector<Point3d> intersectionVertices = verticesFromBoostPolygon(intersectionResult[0], allPoints, tol);
    boost::optional<double> testArea = boost::geometry::area(intersectionResult[0]);
    if (!testArea || intersectionVertices.empty()){
      LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of largest intersection");
      return boost::none;
    }else if (*testArea < tol*tol){
      LOG_FREE(Info, "utilities.geometry.intersect", "Largest intersection has very small area of " << *testArea << " m^2");
      return boost::none;
    }
    try{
      boost::geometry::detail::overlay::has_self_intersections(intersectionResult[0]);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.intersect", "Largest intersection is self intersecting");
      return boost::none;
    }
    if (!intersectionResult[0].inners().empty()){
      LOG_FREE(Error, "utilities.geometry.intersect", "Largest intersection has inner loops");
      return boost::none;
    };

    // intersections are the same
    resultPolygon1 = intersectionVertices;
    resultPolygon2 = intersectionVertices;

    // create new polygon for each remaining intersection
    for (unsigned i = 1; i < intersectionResult.size(); ++i){

      std::vector<Point3d> newPolygon = verticesFromBoostPolygon(intersectionResult[i], allPoints, tol);

      testArea = boost::geometry::area(intersectionResult[i]);
      if (!testArea || newPolygon.empty()){
        LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of intersection, result will not include this polygon, " << newPolygon);
        continue;
      }else if (*testArea < tol*tol){
        LOG_FREE(Info, "utilities.geometry.intersect", "Intersection has very small area of " << *testArea << " m^2, result will not include this polygon, " << newPolygon);
        continue;
      }
      try{
        boost::geometry::detail::overlay::has_self_intersections(intersectionResult[i]);
      }catch(const boost::geometry::overlay_invalid_input_exception&){
        LOG_FREE(Error, "utilities.geometry.intersect", "Intersection is self intersecting, result will not include this polygon, " << newPolygon);
        continue;
      }
      if (!intersectionResult[i].inners().empty()){
        LOG_FREE(Error, "utilities.geometry.intersect", "Intersection has inner loops, result will not include this polygon, " << newPolygon);
        continue;
      };

      newPolygons1.push_back(newPolygon);
      newPolygons2.push_back(newPolygon);
    }

    // polygon1 minus polygon2
    std::vector<BoostPolygon> differenceResult1;
    boost::geometry::difference(*boostPolygon1, *boostPolygon2, differenceResult1);
    differenceResult1 = removeSpikes(differenceResult1);
    differenceResult1 = removeHoles(differenceResult1);

    // create new polygon for each difference
    for (unsigned i = 0; i < differenceResult1.size(); ++i){

      std::vector<Point3d> newPolygon1 = verticesFromBoostPolygon(differenceResult1[i], allPoints, tol);

      testArea = boost::geometry::area(differenceResult1[i]);
      if (!testArea || newPolygon1.empty()){
        LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of face difference, result will not include this polygon, " << newPolygon1);
        continue;
      }else if (*testArea < tol*tol){
        LOG_FREE(Info, "utilities.geometry.intersect", "Face difference has very small area of " << *testArea << " m^2, result will not include this polygon, " << newPolygon1);
        continue;
      }
      try{
        boost::geometry::detail::overlay::has_self_intersections(differenceResult1[i]);
      }catch(const boost::geometry::overlay_invalid_input_exception&){
        LOG_FREE(Error, "utilities.geometry.intersect", "Face difference is self intersecting, result will not include this polygon, " << newPolygon1);
        continue;
      }

      newPolygons1.push_back(newPolygon1);
    }

    // polygon2 minus polygon1
    std::vector<BoostPolygon> differenceResult2;
    boost::geometry::difference(*boostPolygon2, *boostPolygon1, differenceResult2);
    differenceResult2 = removeSpikes(differenceResult2);
    differenceResult2 = removeHoles(differenceResult2);

    // create new polygon for each difference
    for (unsigned i = 0; i < differenceResult2.size(); ++i){

      std::vector<Point3d> newPolygon2 = verticesFromBoostPolygon(differenceResult2[i], allPoints, tol);

      testArea = boost::geometry::area(differenceResult2[i]);
      if (!testArea || newPolygon2.empty()){
        LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of face difference, result will not include this polygon, " << newPolygon2);
        continue;
      }else if (*testArea < tol*tol){
        LOG_FREE(Info, "utilities.geometry.intersect", "Face difference has very small area of " << *testArea << " m^2, result will not include this polygon, " << newPolygon2);
        continue;
      }
      try{
        boost::geometry::detail::overlay::has_self_intersections(differenceResult2[i]);
      }catch(const boost::geometry::overlay_invalid_input_exception&){
        LOG_FREE(Error, "utilities.geometry.intersect", "Face difference is self intersecting, result will not include this polygon, " << newPolygon2);
        continue;
      }

      newPolygons2.push_back(newPolygon2);
    }

    return IntersectionResult(resultPolygon1, resultPolygon2, newPolygons1, newPolygons2);
  }

  std::vector<std::vector<Point3d> > subtract(const std::vector<Point3d>& polygon, const std::vector<std::vector<Point3d> >& holes, double tol)
  {
    std::vector<std::vector<Point3d> > result;

    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> initialBoostPolygon = nonIntersectingBoostPolygonFromVertices(polygon, allPoints, tol);
    if (!initialBoostPolygon){
      return result;
    }

    std::vector<BoostPolygon> boostPolygons;
    boostPolygons.push_back(*initialBoostPolygon);

    std::vector<BoostPolygon> newBoostPolygons;
    for (const std::vector<Point3d>& hole : holes){
      boost::optional<BoostPolygon> boostHole = nonIntersectingBoostPolygonFromVertices(hole, allPoints, tol);
      if (!boostHole){
        return result;
      }

      for (const BoostPolygon& boostPolygon : boostPolygons){
        std::vector<BoostPolygon> diffResult;
        boost::geometry::difference(boostPolygon, *boostHole, diffResult);
        diffResult = removeSpikes(diffResult);
        diffResult = removeHoles(diffResult);
        newBoostPolygons.insert(newBoostPolygons.end(), diffResult.begin(), diffResult.end());
      }
      boostPolygons.swap(newBoostPolygons);
    }

    for (const BoostPolygon& boostPolygon : boostPolygons){
      result.push_back(verticesFromBoostPolygon(boostPolygon, allPoints, tol));
    }

    return result;
  }

  bool selfIntersects(const std::vector<Point3d>& polygon, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> bp = nonIntersectingBoostPolygonFromVertices(polygon, allPoints, tol);
    if (!bp){
      return false;
    }
    return true;
  }

  bool intersects(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> bp1 = boostPolygonFromVertices(polygon1, allPoints, tol);
    boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);

    if (bp1 && bp2){
      return boost::geometry::intersects(*bp1, *bp2);
    }

    return false;
  }

  bool within(const Point3d& point1, const std::vector<Point3d>& polygon2, double tol)
  {
    std::vector<Point3d> geometry1;
    geometry1.push_back(point1);
    return within(geometry1, polygon2, tol);
  }

  bool within(const std::vector<Point3d>& geometry1, const std::vector<Point3d>& polygon2, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    if (geometry1.size() == 1){
      if (geometry1[0].z() > tol){
        return false;
      }

      boost::tuple<double, double> p = boostPointFromPoint3d(geometry1[0], allPoints, tol);
      BoostPoint boostPoint(p.get<0>(), p.get<1>());

      boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);

      if (bp2){
        return boost::geometry::within(boostPoint, *bp2);
      }

      return false;
    }

    /*
    // DLM: this is the better implementation, requires boost 1.57
    boost::optional<BoostPolygon> bp1 = boostPolygonFromVertices(geometry1, allPoints, tol);
    boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);
    if (bp1 && bp2){
      return boost::geometry::within(*bp1, *bp2);
    }
    */

    // DLM: temp code
    if (geometry1.size() < 3){
      return false;
    }
    boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);
    if (bp2){
      for (const Point3d& point : geometry1)
      {
        boost::tuple<double, double> p = boostPointFromPoint3d(point, allPoints, tol);
        BoostPoint boostPoint(p.get<0>(), p.get<1>());

        if (!boost::geometry::within(boostPoint, *bp2)){
          return false;
        }
      }
    } else{
      return false;
    }

    return true;
  }

} // openstudio

max_stars_repo_name: jasondegraw/OpenStudio

max_issues_repo_name: jasondegraw/OpenStudio

max_forks_repo_name: jasondegraw/OpenStudio


text: #include <boost/preprocessor/comparison/greater_equal.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
#include "pauli_operator.hpp"

#include <boost/dynamic_bitset.hpp>
#include <csim/stat_ops_dm.hpp>
#include <vector>

#ifdef _USE_GPU
#include <gpusim/stat_ops.h>
#endif

#include "state.hpp"
#include "type.hpp"

void MultiQubitPauliOperator::set_bit(
    const UINT pauli_id, const UINT target_index) {
    while (this->_x.size() <= target_index) {
        this->_x.resize(this->_x.size() * 2 + 1);
    }
    this->_z.resize(this->_x.size());
    if (pauli_id == PAULI_ID_X) {
        this->_x.set(target_index);
    } else if (pauli_id == PAULI_ID_Y) {
        this->_x.set(target_index);
        this->_z.set(target_index);
    } else if (pauli_id == PAULI_ID_Z) {
        this->_z.set(target_index);
    }
}

const std::vector<UINT>& MultiQubitPauliOperator::get_pauli_id_list() const {
    return _pauli_id;
}

const std::vector<UINT>& MultiQubitPauliOperator::get_index_list() const {
    return _target_index;
}

void MultiQubitPauliOperator::add_single_Pauli(
    UINT qubit_index, UINT pauli_type) {
    if (pauli_type >= 4)
        throw std::invalid_argument("pauli type must be any of 0,1,2,3");
    _target_index.push_back(qubit_index);
    _pauli_id.push_back(pauli_type);
    set_bit(pauli_type, qubit_index);
}

CPPCTYPE MultiQubitPauliOperator::get_expectation_value(
    const QuantumStateBase* state) const {
    if (state->get_device_type() == DEVICE_CPU) {
        if (state->is_state_vector()) {
            return expectation_value_multi_qubit_Pauli_operator_partial_list(
                this->_target_index.data(), this->_pauli_id.data(),
                static_cast<UINT>(this->_target_index.size()), state->data_c(),
                state->dim);
        } else {
            return dm_expectation_value_multi_qubit_Pauli_operator_partial_list(
                this->_target_index.data(), this->_pauli_id.data(),
                static_cast<UINT>(this->_target_index.size()), state->data_c(),
                state->dim);
        }
    } else if (state->get_device_type() == DEVICE_GPU) {
#ifdef _USE_GPU
        if (state->is_state_vector()) {
            return expectation_value_multi_qubit_Pauli_operator_partial_list_host(
                this->get_index_list().data(), this->get_pauli_id_list().data(),
                (UINT)this->get_index_list().size(), state->data(), state->dim,
                state->get_cuda_stream(), state->device_number);
        } else {
            throw std::runtime_error(
                "Get expectation value for DensityMatrix on GPU is not "
                "supported");
        }
#else
        throw std::invalid_argument("GPU is not supported in this build");
#endif
    } else {
        throw std::invalid_argument("Unsupported device type");
    }
}

CPPCTYPE MultiQubitPauliOperator::get_transition_amplitude(
    const QuantumStateBase* state_bra,
    const QuantumStateBase* state_ket) const {
    if (state_bra->get_device_type() != state_ket->get_device_type())
        throw std::invalid_argument("Device type is different");
    if (state_bra->is_state_vector() != state_ket->is_state_vector())
        throw std::invalid_argument("is_state_vector is not matched");
    if (state_bra->dim != state_ket->dim)
        throw std::invalid_argument("state_bra->dim != state_ket->dim");

    if (state_bra->get_device_type() == DEVICE_CPU) {
        if (state_bra->is_state_vector()) {
            return transition_amplitude_multi_qubit_Pauli_operator_partial_list(
                this->_target_index.data(), this->_pauli_id.data(),
                (UINT)this->_target_index.size(), state_bra->data_c(),
                state_ket->data_c(), state_bra->dim);
        } else {
            throw std::invalid_argument(
                "TransitionAmplitude for density matrix is not implemtend");
        }
    } else if (state_bra->get_device_type() == DEVICE_GPU) {
#ifdef _USE_GPU
        if (state_bra->is_state_vector()) {
            return transition_amplitude_multi_qubit_Pauli_operator_partial_list_host(
                this->get_index_list().data(), this->get_pauli_id_list().data(),
                (UINT)this->get_index_list().size(), state_bra->data(),
                state_ket->data(), state_bra->dim, state_bra->get_cuda_stream(),
                state_bra->device_number);
        } else {
            throw std::runtime_error(
                "Get expectation value for DensityMatrix on GPU is not "
                "supported");
        }
#else
        throw std::invalid_argument("GPU is not supported in this build");
#endif
    } else {
        throw std::invalid_argument("Unsupported device");
    }
}

MultiQubitPauliOperator* MultiQubitPauliOperator::copy() const {
    auto pauli =
        new MultiQubitPauliOperator(this->_target_index, this->_pauli_id);
    return pauli;
}

bool MultiQubitPauliOperator::operator==(
    const MultiQubitPauliOperator& target) const {
    auto x = this->_x;
    auto z = this->_z;
    auto target_x = target.get_x_bits();
    auto target_z = target.get_z_bits();
    if (target_x.size() != this->_x.size()) {
        size_t max_size = std::max(this->_x.size(), target_x.size());
        x.resize(max_size);
        z.resize(max_size);
        target_x.resize(max_size);
        target_z.resize(max_size);
    }
    return x == target_x && z == target_z;
}

MultiQubitPauliOperator MultiQubitPauliOperator::operator*(
    const MultiQubitPauliOperator& target) const {
    auto x = this->_x;
    auto z = this->_z;
    auto target_x = target.get_x_bits();
    auto target_z = target.get_z_bits();
    if (target_x.size() != this->_x.size()) {
        size_t max_size = std::max(x.size(), target_x.size());
        x.resize(max_size);
        z.resize(max_size);
        target_x.resize(max_size);
        target_z.resize(max_size);
    }
    MultiQubitPauliOperator res(x ^ target_x, z ^ target_z);
    return res;
}

MultiQubitPauliOperator& MultiQubitPauliOperator::operator*=(
    const MultiQubitPauliOperator& target) {
    auto target_x = target.get_x_bits();
    auto target_z = target.get_z_bits();
    size_t max_size = std::max(this->_x.size(), target_x.size());
    if (target_x.size() != this->_x.size()) {
        this->_x.resize(max_size);
        this->_z.resize(max_size);
        target_x.resize(max_size);
        target_z.resize(max_size);
    }
    this->_x ^= target_x;
    this->_z ^= target_z;
    _target_index.clear();
    _pauli_id.clear();
    ITYPE i;
    for (i = 0; i < max_size; i++) {
        UINT pauli_id = PAULI_ID_I;
        if (this->_x[i] && !this->_z[i]) {
            pauli_id = PAULI_ID_X;
        } else if (this->_x[i] && this->_z[i]) {
            pauli_id = PAULI_ID_Y;
        } else if (!this->_x[i] && this->_z[i]) {
            pauli_id = PAULI_ID_Z;
        }
        _target_index.push_back(i);
        _pauli_id.push_back(pauli_id);
    }
    return *this;
}

std::string MultiQubitPauliOperator::to_string() const {
    std::string res;
    std::string id;
    ITYPE i;
    for (i = 0; i < _x.size(); i++) {
        if (!_x[i] && !_z[i]) {
            id = "I";
        } else if (_x[i] && !_z[i]) {
            id = "X";
        } else if (_x[i] && _z[i]) {
            id = "Y";
        } else if (!_x[i] && _z[i]) {
            id = "Z";
        }
        if (id != "I") {
            res += id + " " + std::to_string(i) + " ";
        }
    }
    return res;
}