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

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <unistd.h>
#include <vector>
#include <string>

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

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <mynteye_wrapper_d/Temp.h>

#include "mynteyed/camera.h"
#include "mynteyed/utils.h"

#include "pointcloud_generator.h" // NOLINT

MYNTEYE_BEGIN_NAMESPACE

namespace {

void matrix_3x1(const double (*src1)[3], const double (*src2)[1],
    double (*dst)[1]) {
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 1; j++) {
      for (int k = 0; k < 3; k++) {
        dst[i][j] += src1[i][k] * src2[k][j];
      }
    }
  }
}

void matrix_3x3(const double (*src1)[3], const double (*src2)[3],
    double (*dst)[3]) {
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      for (int k = 0; k < 3; k++) {
        dst[i][j] += src1[i][k] * src2[k][j];
      }
    }
  }
}

}  // namespace

namespace enc = sensor_msgs::image_encodings;

class MYNTEYEWrapperNodelet : public nodelet::Nodelet {
 public:
  ros::NodeHandle nh;
  ros::NodeHandle nh_ns;

  image_transport::CameraPublisher pub_left_mono;
  image_transport::CameraPublisher pub_left_color;
  image_transport::CameraPublisher pub_right_mono;
  image_transport::CameraPublisher pub_right_color;
  image_transport::CameraPublisher pub_depth;
  ros::Publisher pub_points;
  ros::Publisher pub_imu;
  ros::Publisher pub_temp;
  ros::Publisher pub_imu_processed;

  sensor_msgs::CameraInfoPtr left_info_ptr;
  sensor_msgs::CameraInfoPtr right_info_ptr;

  // Launch params

  std::int32_t points_frequency;
  double points_factor;
  int gravity;

  std::string base_frame_id;
  std::string left_mono_frame_id;
  std::string left_color_frame_id;
  std::string right_mono_frame_id;
  std::string right_color_frame_id;
  std::string depth_frame_id;
  std::string points_frame_id;
  std::string imu_frame_id;
  std::string temp_frame_id;
  std::string imu_frame_processed_id;

  // MYNTEYE objects
  OpenParams params;
  std::unique_ptr<Camera> mynteye;

  // Others

  std::unique_ptr<PointCloudGenerator> pointcloud_generator;

  std::shared_ptr<MotionIntrinsics> motion_intrinsics;
  bool motion_intrinsics_enabled;

  std::shared_ptr<ImuData> imu_accel;
  std::shared_ptr<ImuData> imu_gyro;

  cv::Mat points_color;
  cv::Mat points_depth;

  typedef struct SubResult {
    bool left_mono;
    bool left_color;
    bool right_mono;
    bool right_color;
    bool depth;
    bool points;
    bool imu;
    bool temp;
    bool imu_processed;
    bool left;
    bool right;
  } sub_result_t;

  sub_result_t sub_result;

  MYNTEYEWrapperNodelet() {
  }

  ~MYNTEYEWrapperNodelet() {
    closeDevice();
    mynteye.reset(nullptr);
    motion_intrinsics = nullptr;
    motion_intrinsics_enabled = false;
  }

  void onInit() override {
    std::string dashes(30, '-');

    nh = getMTNodeHandle();
    nh_ns = getMTPrivateNodeHandle();

    // Launch params
    int dev_index = 0;
    int framerate = 10;
    int dev_mode = 2;
    int color_mode = 0;
    int depth_mode = 0;
    int stream_mode = 0;
    int color_stream_format = 0;
    int depth_stream_format = 0;
    bool state_ae = true;
    bool state_awb = true;
    int ir_intensity = 0;
    bool ir_depth_only = true;
    nh_ns.getParam("dev_index", dev_index);
    nh_ns.getParam("framerate", framerate);
    nh_ns.getParam("dev_mode", dev_mode);
    nh_ns.getParam("color_mode", color_mode);
    nh_ns.getParam("depth_mode", depth_mode);
    nh_ns.getParam("stream_mode", stream_mode);
    nh_ns.getParam("color_stream_format", color_stream_format);
    nh_ns.getParam("depth_stream_format", depth_stream_format);
    nh_ns.getParam("state_ae", state_ae);
    nh_ns.getParam("state_awb", state_awb);
    nh_ns.getParam("ir_intensity", ir_intensity);
    nh_ns.getParam("ir_depth_only", ir_depth_only);

    points_frequency = DEFAULT_POINTS_FREQUENCE;
    points_factor = DEFAULT_POINTS_FACTOR;
    gravity = 9.8;
    nh_ns.getParam("points_frequency", points_frequency);
    nh_ns.getParam("points_factor", points_factor);
    nh_ns.getParam("gravity", gravity);

    base_frame_id = "mynteye_link";
    left_mono_frame_id = "mynteye_left_mono_frame";
    left_color_frame_id = "mynteye_left_color_frame";
    right_mono_frame_id = "mynteye_right_mono_frame";
    right_color_frame_id = "mynteye_right_color_frame";
    depth_frame_id = "mynteye_depth_frame";
    points_frame_id = "mynteye_points_frame";
    imu_frame_id = "mynteye_imu_frame";
    temp_frame_id = "mynteye_temp_frame";
    imu_frame_processed_id = "mynteye_imu_frame_processed";
    nh_ns.getParam("base_frame_id", base_frame_id);
    nh_ns.getParam("left_mono_frame", left_mono_frame_id);
    nh_ns.getParam("left_color_frame", left_color_frame_id);
    nh_ns.getParam("right_mono_frame", right_mono_frame_id);
    nh_ns.getParam("right_color_frame", right_color_frame_id);
    nh_ns.getParam("depth_frame", depth_frame_id);
    nh_ns.getParam("points_frame", points_frame_id);
    nh_ns.getParam("imu_frame", imu_frame_id);
    nh_ns.getParam("temp_frame", temp_frame_id);
    nh_ns.getParam("imu_frame_processed", imu_frame_processed_id);
    NODELET_INFO_STREAM("base_frame: " << base_frame_id);
    NODELET_INFO_STREAM("left_mono_frame: " << left_mono_frame_id);
    NODELET_INFO_STREAM("left_color_frame: " << left_color_frame_id);
    NODELET_INFO_STREAM("right_mono_frame: " << right_mono_frame_id);
    NODELET_INFO_STREAM("right_color_frame: " << right_color_frame_id);
    NODELET_INFO_STREAM("depth_frame: " << depth_frame_id);
    NODELET_INFO_STREAM("points_frame: " << points_frame_id);
    NODELET_INFO_STREAM("imu_frame: " << imu_frame_id);
    NODELET_INFO_STREAM("temp_frame: " << temp_frame_id);
    NODELET_INFO_STREAM("imu_frame_processed: " << imu_frame_processed_id);

    std::string left_mono_topic = "mynteye/left/image_mono";
    std::string left_color_topic = "mynteye/left/image_color";
    std::string right_mono_topic = "mynteye/right/image_mono";
    std::string right_color_topic = "mynteye/right/image_color";
    std::string depth_topic = "mynteye/depth";
    std::string points_topic = "mynteye/points";
    std::string imu_topic = "mynteye/imu";
    std::string temp_topic = "mynteye/temp";
    std::string imu_processed_topic = "mynteye/imu_processed";
    nh_ns.getParam("left_mono_topic", left_mono_topic);
    nh_ns.getParam("left_color_topic", left_color_topic);
    nh_ns.getParam("right_mono_topic", right_mono_topic);
    nh_ns.getParam("right_color_topic", right_color_topic);
    nh_ns.getParam("depth_topic", depth_topic);
    nh_ns.getParam("points_topic", points_topic);
    nh_ns.getParam("imu_topic", imu_topic);
    nh_ns.getParam("temp_topic", temp_topic);
    nh_ns.getParam("imu_processed_topic", imu_processed_topic);

    // MYNTEYE objects
    mynteye.reset(new Camera);
    {
      std::vector<DeviceInfo> dev_infos = mynteye->GetDeviceInfos();
      size_t n = dev_infos.size();
      if (n <= 0 || dev_index < 0 || dev_index >= n) {
        NODELET_ERROR_STREAM("Device not found, index: " << dev_index);
        return;
      }

      NODELET_INFO_STREAM(dashes);
      NODELET_INFO_STREAM("Device Information");
      NODELET_INFO_STREAM(dashes);
      for (auto &&info : dev_infos) {
        NODELET_INFO_STREAM(info.index << " | " << info);
      }
      NODELET_INFO_STREAM(dashes);

      params.dev_index = dev_index;
    }
    {
      std::vector<StreamInfo> color_infos;
      std::vector<StreamInfo> depth_infos;
      mynteye->GetStreamInfos(dev_index, &color_infos, &depth_infos);

      NODELET_INFO_STREAM("Color Stream Information");
      NODELET_INFO_STREAM(dashes);
      for (auto &&info : color_infos) {
        NODELET_INFO_STREAM(info.index << " | " << info);
      }
      NODELET_INFO_STREAM(dashes);

      NODELET_INFO_STREAM("Depth Stream Information");
      NODELET_INFO_STREAM(dashes);
      for (auto &&info : depth_infos) {
        NODELET_INFO_STREAM(info.index << " | " << info);
      }
      NODELET_INFO_STREAM(dashes);
    }
    params.framerate = framerate;
    params.dev_mode = static_cast<DeviceMode>(dev_mode);
    params.color_mode = static_cast<ColorMode>(color_mode);
    params.depth_mode = static_cast<DepthMode>(depth_mode);
    params.stream_mode = static_cast<StreamMode>(stream_mode);
    params.color_stream_format =
        static_cast<StreamFormat>(color_stream_format);
    params.depth_stream_format =
        static_cast<StreamFormat>(depth_stream_format);
    params.state_ae = state_ae;
    params.state_awb = state_awb;
    params.ir_intensity = ir_intensity;
    params.ir_depth_only = ir_depth_only;

    mynteye->EnableProcessMode(ProcessMode::PROC_NONE);

    // Image publishers

    image_transport::ImageTransport it_mynteye(nh);
    // left
    pub_left_mono = it_mynteye.advertiseCamera(left_mono_topic, 1);
    NODELET_INFO_STREAM("Advertized on topic " << left_mono_topic);
    pub_left_color = it_mynteye.advertiseCamera(left_color_topic, 1);
    NODELET_INFO_STREAM("Advertized on topic " << left_color_topic);
    // right
    pub_right_mono = it_mynteye.advertiseCamera(right_mono_topic, 1);
    NODELET_INFO_STREAM("Advertized on topic " << right_mono_topic);
    pub_right_color = it_mynteye.advertiseCamera(right_color_topic, 1);
    NODELET_INFO_STREAM("Advertized on topic " << right_color_topic);
    // depth
    pub_depth = it_mynteye.advertiseCamera(depth_topic, 1);
    NODELET_INFO_STREAM("Advertized on topic " << depth_topic);
    // points
    pub_points = nh.advertise<sensor_msgs::PointCloud2>(points_topic, 1);
    NODELET_INFO_STREAM("Advertized on topic " << points_topic);
    // imu
    pub_imu = nh.advertise<sensor_msgs::Imu>(imu_topic, 100);
    NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
    pub_imu_processed = nh.advertise<sensor_msgs::Imu>(imu_processed_topic, 100); // NOLINT
    NODELET_INFO_STREAM("Advertized on topic " << imu_processed_topic);
    // temp
    pub_temp = nh.advertise<mynteye_wrapper_d::Temp>(temp_topic, 100);
    NODELET_INFO_STREAM("Advertized on topic " << temp_topic);

    // detect subscribers to enable/disable features
    detectSubscribers();
    // open device
    openDevice();

    // loop
    ros::Rate loop_rate(framerate);
    while (nh_ns.ok()) {
      detectSubscribers();
      loop_rate.sleep();
    }
  }

  void detectSubscribers() {
    bool left_mono_sub = pub_left_mono.getNumSubscribers() > 0;
    bool left_color_sub = pub_left_color.getNumSubscribers() > 0;
    bool right_mono_sub = pub_right_mono.getNumSubscribers() > 0;
    bool right_color_sub = pub_right_color.getNumSubscribers() > 0;
    bool depth_sub = pub_depth.getNumSubscribers() > 0;
    bool points_sub = pub_points.getNumSubscribers() > 0;
    bool imu_sub = pub_imu.getNumSubscribers() > 0;
    bool temp_sub = pub_temp.getNumSubscribers() > 0;
    bool imu_processed_sub = pub_imu_processed.getNumSubscribers() > 0;

    bool left_sub = left_mono_sub || left_color_sub;
    bool right_sub = right_mono_sub || right_color_sub;

    if (left_sub || right_sub || depth_sub || points_sub) {
      if (mynteye->IsImageInfoSupported()) {
        mynteye->EnableImageInfo(true);
      }
    } else {
      mynteye->DisableImageInfo();
    }

    if (imu_sub || imu_processed_sub || temp_sub) {
      if (mynteye->IsMotionDatasSupported()) {
        mynteye->EnableMotionDatas(0);
      }
    } else {
      mynteye->DisableMotionDatas();
    }

    sub_result = {
      left_mono_sub, left_color_sub, right_mono_sub, right_color_sub,
      depth_sub, points_sub, imu_sub, temp_sub,
      imu_processed_sub, left_sub, right_sub,
    };
  }

  void openDevice() {
    // if (!mynteye) {
    //   NODELET_ERROR_STREAM("Init firstly");
    // }
    if (mynteye->IsOpened()) return;

    // Set stream data callbacks
    std::vector<ImageType> types{
      ImageType::IMAGE_LEFT_COLOR,
      ImageType::IMAGE_RIGHT_COLOR,
      ImageType::IMAGE_DEPTH,
    };
    for (auto&& type : types) {
      mynteye->SetStreamCallback(type, [this](const StreamData& data) {
        switch (data.img->type()) {
          case ImageType::IMAGE_LEFT_COLOR: {
            if (sub_result.left || sub_result.points) {
              publishLeft(data, sub_result.left_color, sub_result.left_mono);
            }
          } break;
          case ImageType::IMAGE_RIGHT_COLOR: {
            if (sub_result.right) {
              publishRight(data, sub_result.right_color, sub_result.right_mono);
            }
          } break;
          case ImageType::IMAGE_DEPTH: {
            if (sub_result.depth || sub_result.points) {
              publishDepth(data);
            }
          } break;
        }
      });
    }

    // Set motion data callback
    mynteye->SetMotionCallback([this](const MotionData& data) {
      if (data.imu && (sub_result.imu || sub_result.imu_processed ||
          sub_result.temp)) {
        if (data.imu->flag == MYNTEYE_IMU_ACCEL) {
          imu_accel = data.imu;
          publishImu(sub_result.imu, sub_result.imu_processed, sub_result.temp);
        } else if (data.imu->flag == MYNTEYE_IMU_GYRO) {
          imu_gyro = data.imu;
          publishImu(sub_result.imu, sub_result.imu_processed, sub_result.temp);
        }
      }
    });

    mynteye->Open(params);
    if (!mynteye->IsOpened()) {
      NODELET_ERROR_STREAM("Open camera failed");
      return;
    }
    NODELET_INFO_STREAM("Open camera success");

    // camera infos
    bool in_ok;
    auto&& in = mynteye->GetStreamIntrinsics(params.stream_mode, &in_ok);
    if (in_ok) {
      NODELET_INFO_STREAM("Camera info is created");
      left_info_ptr = createCameraInfo(in.left);
      right_info_ptr = createCameraInfo(in.right);
    } else {
      NODELET_WARN_STREAM("Camera info is null");
      left_info_ptr = nullptr;
      right_info_ptr = nullptr;
    }

    // motion intrinsics
    if (motion_intrinsics == nullptr) {
      motion_intrinsics = std::make_shared<MotionIntrinsics>();
    }
    *motion_intrinsics = mynteye->GetMotionIntrinsics(&in_ok);
    if (in_ok) {
      motion_intrinsics_enabled = true;
    } else {
      motion_intrinsics_enabled = false;
      std::cout << "This device not supported to get motion intrinsics."
          << std::endl;
    }

    // pointcloud generator
    pointcloud_generator.reset(new PointCloudGenerator(
      in_ok ? in.left : getDefaultCameraIntrinsics(params.stream_mode),
      [this](sensor_msgs::PointCloud2 msg) {
        // msg.header.seq = 0;
        // msg.header.stamp = ros::Time::now();
        msg.header.frame_id = points_frame_id;
        pub_points.publish(msg);
      }, points_factor, points_frequency));
  }

  void closeDevice() {
    if (mynteye) {
      mynteye->Close();
    }
  }

  void publishLeft(const StreamData& data, bool color_sub, bool mono_sub) {
    publishColor(data, left_info_ptr,
        pub_left_color, color_sub, left_color_frame_id,
        pub_left_mono, mono_sub, left_mono_frame_id, true);
  }

  void publishRight(const StreamData& data, bool color_sub, bool mono_sub) {
    publishColor(data, right_info_ptr,
        pub_right_color, color_sub, right_color_frame_id,
        pub_right_mono, mono_sub, right_mono_frame_id, false);
  }

  void publishColor(const StreamData& data,
      const sensor_msgs::CameraInfoPtr& info,
      const image_transport::CameraPublisher& pub_color, bool color_sub,
      const std::string color_frame_id,
      const image_transport::CameraPublisher& pub_mono, bool mono_sub,
      const std::string mono_frame_id, bool is_left) {
    auto timestamp = data.img_info
        ? hardTimeToSoftTime(data.img_info->timestamp)
        : ros::Time().now();
    auto&& mat = data.img->To(ImageFormat::COLOR_RGB)->ToMat();

    if (color_sub) {
      std_msgs::Header header;
      // header.seq = 0;
      header.stamp = timestamp;
      header.frame_id = color_frame_id;

      auto&& msg = cv_bridge::CvImage(header, enc::RGB8, mat).toImageMsg();
      if (info) info->header.stamp = msg->header.stamp;
      pub_color.publish(msg, info);
    }
    if (mono_sub) {
      std_msgs::Header header;
      // header.seq = 0;
      header.stamp = timestamp;
      header.frame_id = mono_frame_id;

      cv::Mat dst;
      cv::cvtColor(mat, dst, CV_RGB2GRAY);
      auto&& msg = cv_bridge::CvImage(header, enc::MONO8, dst).toImageMsg();
      if (info) info->header.stamp = msg->header.stamp;
      pub_mono.publish(msg, info);
    }

    if (is_left && sub_result.points) {
      points_color = mat;
      publishPoints(timestamp);
    }
  }

  void publishDepth(const StreamData& data) {
    std_msgs::Header header;
    // header.seq = 0;
    header.stamp = data.img_info
        ? hardTimeToSoftTime(data.img_info->timestamp)
        : ros::Time().now();
    header.frame_id = depth_frame_id;

    auto&& info = left_info_ptr;
    if (info) info->header.stamp = header.stamp;
    if (params.depth_mode == DepthMode::DEPTH_RAW) {
      auto&& mat = data.img->To(ImageFormat::DEPTH_RAW)->ToMat();
      pub_depth.publish(
          cv_bridge::CvImage(header, enc::MONO16, mat).toImageMsg(), info);
      if (sub_result.points) {
        points_depth = mat;
        publishPoints(header.stamp);
      }
    } else if (params.depth_mode == DepthMode::DEPTH_GRAY) {
      auto&& mat = data.img->To(ImageFormat::DEPTH_GRAY_24)->ToMat();
      pub_depth.publish(
          cv_bridge::CvImage(header, enc::RGB8, mat).toImageMsg(), info);
    } else if (params.depth_mode == DepthMode::DEPTH_COLORFUL) {
      auto&& mat = data.img->To(ImageFormat::DEPTH_RGB)->ToMat();
      pub_depth.publish(
          cv_bridge::CvImage(header, enc::RGB8, mat).toImageMsg(), info);
    } else {
      NODELET_ERROR_STREAM("Depth mode unsupported");
    }
  }

  void publishPoints(ros::Time stamp) {
    // NODELET_INFO_STREAM("publishPoints ..");
    if (points_color.empty() || points_depth.empty()) {
      // NODELET_INFO_STREAM("publishPoints skipped ..");
      return;
    }
    pointcloud_generator->Push(points_color, points_depth, stamp);
    points_color.release();
    points_depth.release();
  }

  void publishImu(bool imu_sub,
        bool imu_processed_sub, bool temp_sub) {
    if (imu_accel == nullptr || imu_gyro == nullptr) {
      return;
    }

    ros::Time stamp = hardTimeToSoftTime(imu_accel->timestamp);

    if (imu_sub) {
      auto msg = getImuMsgFromData(stamp, imu_frame_id, *imu_accel, *imu_gyro);
      pub_imu.publish(msg);
    }

    if (motion_intrinsics_enabled && imu_processed_sub) {
      auto data_acc1 = ProcImuTempDrift(*imu_accel);
      auto data_gyr1 = ProcImuTempDrift(*imu_gyro);
      auto data_acc2 = ProcImuAssembly(data_acc1);
      auto data_gyr2 = ProcImuAssembly(data_gyr1);
      auto msg = getImuMsgFromData(stamp, imu_frame_processed_id,
                                    data_acc2, data_gyr2);
      pub_imu_processed.publish(msg);
    }

    if (temp_sub) {
      auto msg = getTempMsgFromData(stamp, temp_frame_id, *imu_accel);
      pub_temp.publish(msg);
    }

    imu_accel = nullptr;
    imu_gyro = nullptr;
  }

  sensor_msgs::Imu getImuMsgFromData(ros::Time stamp,
      const std::string& frame_id,
      const ImuData& imu_accel, const ImuData& imu_gyro) {
    sensor_msgs::Imu msg;

    // msg.header.seq = seq;
    msg.header.stamp = stamp;
    msg.header.frame_id = frame_id;

    // acceleration should be in m/s^2 (not in g's)
    msg.linear_acceleration.x = imu_accel.accel[0] * gravity;
    msg.linear_acceleration.y = imu_accel.accel[1] * gravity;
    msg.linear_acceleration.z = imu_accel.accel[2] * gravity;

    msg.linear_acceleration_covariance[0] = 0;
    msg.linear_acceleration_covariance[1] = 0;
    msg.linear_acceleration_covariance[2] = 0;

    msg.linear_acceleration_covariance[3] = 0;
    msg.linear_acceleration_covariance[4] = 0;
    msg.linear_acceleration_covariance[5] = 0;

    msg.linear_acceleration_covariance[6] = 0;
    msg.linear_acceleration_covariance[7] = 0;
    msg.linear_acceleration_covariance[8] = 0;

    // velocity should be in rad/sec
    msg.angular_velocity.x = imu_gyro.gyro[0] * M_PI / 180;
    msg.angular_velocity.y = imu_gyro.gyro[1] * M_PI / 180;
    msg.angular_velocity.z = imu_gyro.gyro[2] * M_PI / 180;

    msg.angular_velocity_covariance[0] = 0;
    msg.angular_velocity_covariance[1] = 0;
    msg.angular_velocity_covariance[2] = 0;

    msg.angular_velocity_covariance[3] = 0;
    msg.angular_velocity_covariance[4] = 0;
    msg.angular_velocity_covariance[5] = 0;

    msg.angular_velocity_covariance[6] = 0;
    msg.angular_velocity_covariance[7] = 0;
    msg.angular_velocity_covariance[8] = 0;

    return msg;
  }

  mynteye_wrapper_d::Temp getTempMsgFromData(ros::Time stamp,
                  const std::string& frame_id,
                  const ImuData& imu_accel) {
    mynteye_wrapper_d::Temp msg;
    msg.header.stamp = stamp;
    msg.header.frame_id = frame_id;
    msg.data = imu_accel.temperature;
    return msg;
  }

  sensor_msgs::CameraInfoPtr createCameraInfo(const CameraIntrinsics& in) {
    // http://docs.ros.org/kinetic/api/sensor_msgs/html/msg/CameraInfo.html
    sensor_msgs::CameraInfo *camera_info = new sensor_msgs::CameraInfo();
    auto camera_info_ptr = sensor_msgs::CameraInfoPtr(camera_info);

    // camera_info->header.frame_id = color_frame_id;
    camera_info->width = in.width;
    camera_info->height = in.height;

    //     [fx  0 cx]
    // K = [ 0 fy cy]
    //     [ 0  0  1]
    camera_info->K.at(0) = in.fx;
    camera_info->K.at(2) = in.cx;
    camera_info->K.at(4) = in.fy;
    camera_info->K.at(5) = in.cy;
    camera_info->K.at(8) = 1;

    //     [fx'  0  cx' Tx]
    // P = [ 0  fy' cy' Ty]
    //     [ 0   0   1   0]
    // for (int i = 0; i < 12; i++) {
    //     camera_info->P.at(i) = in.NewCamMat1[i];
    // }

    camera_info->distortion_model = "plumb_bob";

    // D of plumb_bob: (k1, k2, t1, t2, k3)
    for (int i = 0; i < 5; i++) {
      camera_info->D.push_back(in.coeffs[i]);
    }

    // R to identity matrix
    camera_info->R.at(0) = 1.0;
    camera_info->R.at(1) = 0.0;
    camera_info->R.at(2) = 0.0;
    camera_info->R.at(3) = 0.0;
    camera_info->R.at(4) = 1.0;
    camera_info->R.at(5) = 0.0;
    camera_info->R.at(6) = 0.0;
    camera_info->R.at(7) = 0.0;
    camera_info->R.at(8) = 1.0;

    return camera_info_ptr;
  }

  ros::Time hardTimeToSoftTime(double _hard_time) {
    static bool isInited = false;
    static double soft_time_begin(0), hard_time_begin(0);

    if (false == isInited) {
      soft_time_begin = ros::Time::now().toSec();
      hard_time_begin = _hard_time;
      isInited = true;
    }

    return ros::Time(
        soft_time_begin + (_hard_time - hard_time_begin) * 0.00001f);
    // return ros::Time::now();
  }

  ImuData ProcImuAssembly(const ImuData& data) const {
    ImuData res = data;
    if (nullptr == motion_intrinsics) {
      std::cout << "[WARNING!] Motion intrinsic "
                << "is not been supported at this device."
                << std::endl;
      return res;
    }
    double dst[3][3] = {0};
    if (data.flag == 1) {
      matrix_3x3(motion_intrinsics->accel.scale,
          motion_intrinsics->accel.assembly, dst);
      double s[3][1] = {0};
      double d[3][1] = {0};
      for (int i = 0; i < 3; i++) {
        s[i][0] = data.accel[i];
      }
      matrix_3x1(dst, s, d);
      for (int i = 0; i < 3; i++) {
        res.accel[i] = d[i][0];
      }
    } else if (data.flag == 2) {
      matrix_3x3(motion_intrinsics->gyro.scale,
          motion_intrinsics->gyro.assembly, dst);
      double s[3][1] = {0};
      double d[3][1] = {0};
      for (int i = 0; i < 3; i++) {
        s[i][0] = data.gyro[i];
      }
      matrix_3x1(dst, s, d);
      for (int i = 0; i < 3; i++) {
        res.gyro[i] = d[i][0];
      }
    }
    return res;
  }

  ImuData ProcImuTempDrift(const ImuData& data) const {
    ImuData res = data;
    if (nullptr == motion_intrinsics) {
      std::cout << "[WARNING!] Motion intrinsic "
                << "is not been supported at this device."
                << std::endl;
      return res;
    }
    double temp = res.temperature;
    if (res.flag == 1) {
      res.accel[0] -= motion_intrinsics->accel.x[1] * temp
        + motion_intrinsics->accel.x[0];
      res.accel[1] -= motion_intrinsics->accel.y[1] * temp
        + motion_intrinsics->accel.y[0];
      res.accel[2] -= motion_intrinsics->accel.z[1] * temp
        + motion_intrinsics->accel.z[0];
    } else if (res.flag == 2) {
      res.gyro[0] -= motion_intrinsics->gyro.x[1] * temp
        + motion_intrinsics->gyro.x[0];
      res.gyro[1] -= motion_intrinsics->gyro.y[1] * temp
        + motion_intrinsics->gyro.y[0];
      res.gyro[2] -= motion_intrinsics->gyro.z[1] * temp
        + motion_intrinsics->gyro.z[0];
    }
    return res;
  }

  CameraIntrinsics getDefaultCameraIntrinsics(const StreamMode& mode) {
    // {w, h, fx, fy, cx, cy, coeffs[5]{k1,k2,p1,p2,k3}}
    switch (mode) {
      case StreamMode::STREAM_640x480:
        return {640, 480, 979.8, 942.8, 682.3 / 2, 254.9, {0, 0, 0, 0, 0}};
      case StreamMode::STREAM_1280x480:
        return {640, 480, 979.8, 942.8, 682.3, 254.9, {0, 0, 0, 0, 0}};
      case StreamMode::STREAM_1280x720:
        return {1280, 720, 979.8, 942.8, 682.3, 254.9 * 2, {0, 0, 0, 0, 0}};
      case StreamMode::STREAM_2560x720:
        return {1280, 720, 979.8, 942.8, 682.3 * 2, 254.9 * 2, {0, 0, 0, 0, 0}};
      default:
        return {1280, 720, 979.8, 942.8, 682.3, 254.9 * 2, {0, 0, 0, 0, 0}};
    }
  }
};

MYNTEYE_END_NAMESPACE

#include <pluginlib/class_list_macros.h> // NOLINT
PLUGINLIB_EXPORT_CLASS(MYNTEYE_NAMESPACE::MYNTEYEWrapperNodelet, nodelet::Nodelet); // NOLINT


/*
** EPITECH PROJECT, 2019
** oop_indie_studio_2018
** File description:
** Player
*/

#include "Player.hpp"
#include "Bonus.hpp"
#include <boost/lexical_cast.hpp>
#include <iostream>

static const int HITBOX = 50;
static const int PHIT = 30;
static const int NOMOVE_TRESHOLD = 10;
static const int TILESIZE = 50;

game_logic::Player::Player(unsigned int id, std::pair<float, float> position, const bool &isAlive)
    : _bombHandler(new BombHandler(id)),
      _position(position),
      _direction(Direction::NONE),
      _noMove(0),
      _id(id),
      _speed(5),
      _isAlive(isAlive)
{
}

void game_logic::Player::dropBomb(std::vector<Bomb> &bombs)
{
    if (_isAlive == false)
        return;
    std::pair<float, float> roundedPos;
    int q = (int) _position.first % TILESIZE;
    roundedPos.first = (q > TILESIZE / 2) ? std::trunc(_position.first - q) + TILESIZE : std::trunc(_position.first - q);
    q = (int)_position.second % TILESIZE;
    roundedPos.second = (q > TILESIZE / 2) ? std::trunc(_position.second - q) + TILESIZE : std::trunc(_position.second - q);
    auto it = find_if(bombs.begin(), bombs.end(), [&](Bomb const &bomb) {
        return roundedPos == bomb.getPosition();
    });
    if (it == bombs.end())
    {
        _bombHandler->newBomb(bombs, _position.first, _position.second);
    }
}

void game_logic::Player::update(std::vector<Wall> &walls, std::vector<Bomb> &bombs)
{
    if (!_isAlive)
        return;
    _noMove += 1;
    if (_noMove >= NOMOVE_TRESHOLD)
        _direction = NONE;
    _bombHandler->udpate(walls, bombs, _position);
    _isAlive = !_bombHandler->playerHit();
}

bool game_logic::Player::isAlive()
{
    return _isAlive;
}

void game_logic::Player::serialize(std::string &sdata)
{
    sdata += boost::lexical_cast<std::string>(_id) + ",";
    sdata += boost::lexical_cast<std::string>(_position.first) + ",";
    sdata += boost::lexical_cast<std::string>(_position.second) + ",";
    sdata += boost::lexical_cast<std::string>(_direction) + ",";
    sdata += (_isAlive) ? "1;" : "0;";
}

void game_logic::Player::move(std::vector<Wall> &walls, Direction direction)
{
    if (_isAlive == false)
        return;
    _direction = direction;
    _noMove = 0;
    switch (direction)
    {
    case Direction::UP:
        _position.second -= _speed;
        break;
    case Direction::DOWN:
        _position.second += _speed;
        break;
    case Direction::LEFT:
        _position.first -= _speed;
        break;
    case Direction::RIGHT:
        _position.first += _speed;
        break;
    default:
        break;
    }
    auto it = find_if(walls.begin(), walls.end(), [&](Wall const &wall) {
        return _position.first > wall.getPosition().first - PHIT && _position.first < wall.getPosition().first + HITBOX &&
        _position.second > wall.getPosition().second - PHIT && _position.second < wall.getPosition().second + HITBOX && !wall.bonusMode();
    });
    if (it != walls.end()) {
        switch (direction)
        {
        case Direction::UP:
            _position.second = it->getPosition().second + HITBOX;
            break;
        case Direction::DOWN:
            _position.second = it->getPosition().second - PHIT;
            break;
        case Direction::LEFT:
            _position.first = it->getPosition().first + HITBOX;
            break;
        case Direction::RIGHT:
            _position.first = it->getPosition().first - PHIT;
            break;
        default:
            break;
        }
    }
    it = find_if(walls.begin(), walls.end(), [&](Wall const &wall) {
        return _position.first > wall.getPosition().first - PHIT && _position.first < wall.getPosition().first + HITBOX &&
        _position.second > wall.getPosition().second - PHIT && _position.second < wall.getPosition().second + HITBOX && wall.bonusMode();
    });
    if (it == walls.end())
        return;
    switch (it->getBonus())
    {
    case Bonus::POWER:
        _bombHandler->increasePower(1);
        break;
    case Bonus::SPEED:
        _bombHandler->increaseExpSpeed(0.25f);
        break;
    case Bonus::STACK:
        _bombHandler->increaseThreshold(1);
        break;
    case Bonus::PASST:
        // Work in progress
        break;
    default:
        break;
    }
    it->destroy();
}

unsigned int game_logic::Player::getId() const
{
    return _id;
}

void game_logic::Player::setSpeed(unsigned int speed)
{
    _speed = speed;
}

void game_logic::Player::setPosition(std::pair<float, float> position)
{
    _position = position;
}


// Copyright (C) 2006 The Trustees of Indiana University.

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

//  Authors: Nick Edmonds
//           Andrew Lumsdaine

#include <boost/graph/use_mpi.hpp>

#define CSR

#ifdef CSR
#  include <boost/graph/distributed/compressed_sparse_row_graph.hpp>
#else
#  include <boost/graph/distributed/adjacency_list.hpp>
#endif

#include <boost/config.hpp>
#include <boost/throw_exception.hpp>
#include <boost/graph/distributed/mpi_process_group.hpp>
#include <boost/graph/distributed/concepts.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/distributed/betweenness_centrality.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/test/minimal.hpp>

#ifdef BOOST_NO_EXCEPTIONS
void
boost::throw_exception(std::exception const& ex)
{
    std::cout << ex.what() << std::endl;
    abort();
}
#endif

/****************************************************************************
 * Edge weight generator iterator                                           *
 ****************************************************************************/
template<typename F, typename RandomGenerator>
class generator_iterator
{
public:
  typedef std::input_iterator_tag iterator_category;
  typedef typename F::result_type value_type;
  typedef const value_type&       reference;
  typedef const value_type*       pointer;
  typedef void                    difference_type;

  explicit
  generator_iterator(RandomGenerator& gen, const F& f = F())
    : f(f), gen(&gen)
  {
    value = this->f(gen);
  }

  reference operator*() const  { return value; }
  pointer   operator->() const { return &value; }

  generator_iterator& operator++()
  {
    value = f(*gen);
    return *this;
  }

  generator_iterator operator++(int)
  {
    generator_iterator temp(*this);
    ++(*this);
    return temp;
  }

  bool operator==(const generator_iterator& other) const
  { return f == other.f; }

  bool operator!=(const generator_iterator& other) const
  { return !(*this == other); }

private:
  F f;
  RandomGenerator* gen;
  value_type value;
};

template<typename F, typename RandomGenerator>
inline generator_iterator<F, RandomGenerator>
make_generator_iterator( RandomGenerator& gen, const F& f)
{ return generator_iterator<F, RandomGenerator>(gen, f); }

using namespace boost;
using boost::graph::distributed::mpi_process_group;

typedef int weight_type;

struct WeightedEdge {
  WeightedEdge(weight_type weight = 0) : weight(weight) { }

  weight_type weight;

  template<typename Archiver>
  void serialize(Archiver& ar, const unsigned int /*version*/)
  {
    ar & weight;
  }
};

int test_main(int argc, char* argv[])
{
  mpi::environment env(argc, argv);

#ifdef CSR
  typedef compressed_sparse_row_graph<directedS, no_property, WeightedEdge,
                                      no_property, distributedS<mpi_process_group> >
    Graph;
  typedef compressed_sparse_row_graph<directedS, no_property, WeightedEdge>
    seqGraph;
#else
  typedef adjacency_list<vecS,
                         distributedS<mpi_process_group, vecS>,
                         directedS,
                         no_property,
                         property<edge_weight_t, int> > Graph;

  typedef adjacency_list<vecS, vecS, directedS, no_property,
                         property<edge_weight_t, int> > seqGraph;
#endif

  typedef sorted_erdos_renyi_iterator<minstd_rand, Graph> ERIter;

  typedef graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef graph_traits<Graph>::edge_descriptor edge_descriptor;

  int n = 100;
  double prob = 0.1;
  int C = 3;

  minstd_rand gen;

  gen.seed(1);

  // NOTE: Weighted betweenness centrality only works with non-zero edge weights

  // Build graphs
  Graph g(ERIter(gen, n, prob), ERIter(),
          make_generator_iterator(gen, uniform_int<int>(1, C)),
          n);

  gen.seed(1);  // Re-seed PRNG so we get the same graph

  seqGraph sg(
#ifdef CSR
              edges_are_sorted,
#endif
              ERIter(gen, n, prob), ERIter(),
              make_generator_iterator(gen, uniform_int<int>(1, C)),
              n);

  // Test Betweenness Centrality
  typedef property_map<Graph, vertex_index_t>::const_type IndexMap;
  typedef iterator_property_map<std::vector<int>::iterator, IndexMap>
    CentralityMap;

  std::vector<int> centralityS(num_vertices(g), 0);
  CentralityMap centrality(centralityS.begin(), get(vertex_index, g));

  brandes_betweenness_centrality(g, centrality);

  std::vector<int> weightedCentralityS(num_vertices(g), 0);
  CentralityMap weightedCentrality(weightedCentralityS.begin(), get(vertex_index, g));

  brandes_betweenness_centrality(g, centrality_map(weightedCentrality).
#ifdef CSR
                                 weight_map(get(&WeightedEdge::weight, g)));
#else
                                 weight_map(get(edge_weight, g)));
#endif

  int g_cpd = central_point_dominance(g, centrality);

  //
  //  Non-distributed (embarassingly parallel) Betweenness Centrality
  //
  typedef boost::graph::parallel::process_group_type<Graph>::type
    process_group_type;

  process_group_type pg = process_group(g);

  process_group_type::process_id_type id = process_id(pg);
  process_group_type::process_size_type p = num_processes(pg);

  typedef property_map<seqGraph, vertex_index_t>::const_type seqIndexMap;
  typedef iterator_property_map<std::vector<int>::iterator, seqIndexMap> seqCentralityMap;

  std::vector<int> nonDistributedCentralityS(num_vertices(sg), 0);
  seqCentralityMap nonDistributedCentrality(nonDistributedCentralityS.begin(), get(vertex_index, sg));

  std::vector<int> nonDistributedWeightedCentralityS(num_vertices(sg), 0);
  seqCentralityMap nonDistributedWeightedCentrality(nonDistributedWeightedCentralityS.begin(),
                                                    get(vertex_index, sg));

  non_distributed_brandes_betweenness_centrality(pg, sg, nonDistributedCentrality);

  non_distributed_brandes_betweenness_centrality(pg, sg,
                                                 centrality_map(nonDistributedWeightedCentrality).
#ifdef CSR
                                                 weight_map(get(&WeightedEdge::weight, sg)));
#else
                                                 weight_map(get(edge_weight, sg)));
#endif

  //
  // Verify
  //
  std::vector<int> seqCentralityS(num_vertices(sg), 0);
  seqCentralityMap seqCentrality(seqCentralityS.begin(), get(vertex_index, sg));

  std::vector<int> seqWeightedCentralityS(num_vertices(sg), 0);
  seqCentralityMap seqWeightedCentrality(seqWeightedCentralityS.begin(), get(vertex_index, sg));

  brandes_betweenness_centrality(sg, seqCentrality);

  brandes_betweenness_centrality(sg, centrality_map(seqWeightedCentrality).
#ifdef CSR
                                 weight_map(get(&WeightedEdge::weight, sg)));
#else
                                 weight_map(get(edge_weight, sg)));
#endif

  int sg_cpd = central_point_dominance(sg, seqCentrality);

  // Verify exact betweenness centrality
  //
  // We're cheating when we map vertices in g to vertices in sg
  // since we know that g is using the block distribution here
  typedef property_map<Graph, vertex_owner_t>::const_type OwnerMap;
  typedef property_map<Graph, vertex_local_t>::const_type LocalMap;

  OwnerMap owner = get(vertex_owner, g);
  LocalMap local = get(vertex_local, g);

  bool passed = true;

  {
    typedef graph_traits<Graph>::vertex_iterator vertex_iterator;
    vertex_iterator v, v_end;

    for (tie(v, v_end) = vertices(g); v != v_end; ++v) {
      if (get(centrality, *v) != seqCentralityS[(n/p) * get(owner, *v) + get(local, *v)]) {
        std::cerr << "  " << id << ": Error - centrality of " << get(local, *v) << "@" << get(owner, *v)
                  << " does not match the sequential result (" << get(centrality, *v) << " vs. "
                  << seqCentralityS[(n/p) * get(owner, *v) + get(local, *v)] << ")\n";
        passed = false;
      }

      if (get(weightedCentrality, *v) != seqWeightedCentralityS[(n/p) * get(owner, *v) + get(local, *v)]) {
        std::cerr << "  " << id << ": Error - weighted centrality of " << get(local, *v) << "@" << get(owner, *v)
                  << " does not match the sequential result (" << get(weightedCentrality, *v) << " vs. "
                  << seqWeightedCentralityS[(n/p) * get(owner, *v) + get(local, *v)] << ")\n";
        passed = false;
      }
    }
  }

  if (id == 0) {
    typedef graph_traits<seqGraph>::vertex_iterator vertex_iterator;
    vertex_iterator v, v_end;

    for (tie(v, v_end) = vertices(sg); v != v_end; ++v) {
      if (get(seqCentrality, *v) != get(nonDistributedCentrality, *v)) {
        std::cerr << "  " << id << ": Error - non-distributed centrality of " << *v
                  << " does not match the sequential result (" << get(nonDistributedCentrality, *v)
                  << " vs. " << get(seqCentrality, *v) << ")\n";
        passed = false;
      }

      if (get(seqWeightedCentrality, *v) != get(nonDistributedWeightedCentrality, *v)) {
        std::cerr << "  " << id << ": Error - non-distributed weighted centrality of " << *v
                  << " does not match the sequential result (" << get(nonDistributedWeightedCentrality, *v)
                  << " vs. " << get(seqCentrality, *v) << ")\n";
        passed = false;
      }
    }
  }

  if (g_cpd != sg_cpd) {
    passed = false;
    std::cerr << "Central point dominance did not match the sequential result\n";
  }

  if (!passed)
    MPI_Abort(MPI_COMM_WORLD, -1);

  return 0;
}


/*
 * @brief Helper functions for unpacking messages.
 * @author Jenna Reher (jreher@caltech.edu)
 */

#ifndef CASSIE_CALLBACK_HELPERS_HPP
#define CASSIE_CALLBACK_HELPERS_HPP

#include <Eigen/Dense>
#include <cassie_common_toolbox/geometry.hpp>
#include <cassie_common_toolbox/CassieStateEnum.hpp>
#include <cassie_common_toolbox/cassie_control_msg.h>
#include <cassie_common_toolbox/cassie_estimation_msg.h>
#include <cassie_common_toolbox/cassie_proprioception_msg.h>

void unpack_proprioception(const cassie_common_toolbox::cassie_proprioception_msg::ConstPtr& propmsg, Eigen::VectorXd &q, Eigen::VectorXd &dq, Eigen::VectorXd &radio, Eigen::Vector3d &gyro, Eigen::Vector3d &accel, Eigen::VectorXd &torque, double &leftContact, double &rightContact) {
    for (unsigned int i=0; i<16; ++i)
        radio[i] = propmsg->radio[i];

    q(LeftHipRoll)      = propmsg->encoder_position[0];
    q(LeftHipYaw)       = propmsg->encoder_position[1];
    q(LeftHipPitch)     = propmsg->encoder_position[2];
    q(LeftKneePitch)    = propmsg->encoder_position[3];
    q(LeftShinPitch)    = propmsg->encoder_position[4];
    q(LeftTarsusPitch)  = propmsg->encoder_position[5];
    q(LeftFootPitch)     = propmsg->encoder_position[6];
    q(RightHipRoll)     = propmsg->encoder_position[7];
    q(RightHipYaw)      = propmsg->encoder_position[8];
    q(RightHipPitch)    = propmsg->encoder_position[9];
    q(RightKneePitch)   = propmsg->encoder_position[10];
    q(RightShinPitch)   = propmsg->encoder_position[11];
    q(RightTarsusPitch) = propmsg->encoder_position[12];
    q(RightFootPitch)    = propmsg->encoder_position[13];

    q(LeftHeelSpring)   = propmsg->q_achilles[0];
    q(RightHeelSpring)  = propmsg->q_achilles[1];

    // Get encoder velocities
    dq(LeftHipRoll)      = propmsg->encoder_velocity[0];
    dq(LeftHipYaw)       = propmsg->encoder_velocity[1];
    dq(LeftHipPitch)     = propmsg->encoder_velocity[2];
    dq(LeftKneePitch)    = propmsg->encoder_velocity[3];
    dq(LeftShinPitch)    = propmsg->encoder_velocity[4];
    dq(LeftTarsusPitch)  = propmsg->encoder_velocity[5];
    dq(LeftFootPitch)     = propmsg->encoder_velocity[6];
    dq(RightHipRoll)     = propmsg->encoder_velocity[7];
    dq(RightHipYaw)      = propmsg->encoder_velocity[8];
    dq(RightHipPitch)    = propmsg->encoder_velocity[9];
    dq(RightKneePitch)   = propmsg->encoder_velocity[10];
    dq(RightShinPitch)   = propmsg->encoder_velocity[11];
    dq(RightTarsusPitch) = propmsg->encoder_velocity[12];
    dq(RightFootPitch)    = propmsg->encoder_velocity[13];

    dq(LeftHeelSpring) = propmsg->dq_achilles[0];
    dq(RightHeelSpring) = propmsg->dq_achilles[1];

    // Assign gyroscope
    dq(3) = propmsg->angular_velocity.x;
    dq(4) = propmsg->angular_velocity.y;
    dq(5) = propmsg->angular_velocity.z;
    gyro(0) = propmsg->angular_velocity.x;
    gyro(1) = propmsg->angular_velocity.y;
    gyro(2) = propmsg->angular_velocity.z;

    // Assign accelerometer
    accel(0) = propmsg->linear_acceleration.x;
    accel(1) = propmsg->linear_acceleration.y;
    accel(2) = propmsg->linear_acceleration.z;

    // Get contacts
    leftContact = propmsg->contact[0];
    rightContact = propmsg->contact[1];

    // Get joint torques
    for (unsigned int i=0; i<10; ++i)
        torque(i) = propmsg->motor_torque[i];

}

void get_proprioception_encoders(const cassie_common_toolbox::cassie_proprioception_msg& propmsg, Eigen::VectorXd &q, Eigen::VectorXd &dq) {
    q(LeftHipRoll)      = propmsg.encoder_position[0];
    q(LeftHipYaw)       = propmsg.encoder_position[1];
    q(LeftHipPitch)     = propmsg.encoder_position[2];
    q(LeftKneePitch)    = propmsg.encoder_position[3];
    q(LeftShinPitch)    = propmsg.encoder_position[4];
    q(LeftTarsusPitch)  = propmsg.encoder_position[5];
    q(LeftFootPitch)     = propmsg.encoder_position[6];
    q(RightHipRoll)     = propmsg.encoder_position[7];
    q(RightHipYaw)      = propmsg.encoder_position[8];
    q(RightHipPitch)    = propmsg.encoder_position[9];
    q(RightKneePitch)   = propmsg.encoder_position[10];
    q(RightShinPitch)   = propmsg.encoder_position[11];
    q(RightTarsusPitch) = propmsg.encoder_position[12];
    q(RightFootPitch)    = propmsg.encoder_position[13];

    // Get encoder velocities
    dq(LeftHipRoll)      = propmsg.encoder_velocity[0];
    dq(LeftHipYaw)       = propmsg.encoder_velocity[1];
    dq(LeftHipPitch)     = propmsg.encoder_velocity[2];
    dq(LeftKneePitch)    = propmsg.encoder_velocity[3];
    dq(LeftShinPitch)    = propmsg.encoder_velocity[4];
    dq(LeftTarsusPitch)  = propmsg.encoder_velocity[5];
    dq(LeftFootPitch)     = propmsg.encoder_velocity[6];
    dq(RightHipRoll)     = propmsg.encoder_velocity[7];
    dq(RightHipYaw)      = propmsg.encoder_velocity[8];
    dq(RightHipPitch)    = propmsg.encoder_velocity[9];
    dq(RightKneePitch)   = propmsg.encoder_velocity[10];
    dq(RightShinPitch)   = propmsg.encoder_velocity[11];
    dq(RightTarsusPitch) = propmsg.encoder_velocity[12];
    dq(RightFootPitch)    = propmsg.encoder_velocity[13];
}

void get_proprioception_orientation(const cassie_common_toolbox::cassie_proprioception_msg& propmsg, Eigen::VectorXd &q, Eigen::VectorXd &dq, Eigen::Quaterniond &quat) {
    // Get the pelvis rotation via the IMU
    // Assign the pelvis rotation
    quat.w() = propmsg.orientation.w;
    quat.x() = propmsg.orientation.x;
    quat.y() = propmsg.orientation.y;
    quat.z() = propmsg.orientation.z;

    Eigen::Matrix3d RateMatrix;
    //RateMatrix <<
    //    1., sin(q(BaseRotX))*tan(q(BaseRotY)), cos(q(BaseRotX))*tan(q(BaseRotY)),
    //    0, cos(q(BaseRotX)), -sin(q(BaseRotX)),
    //    0, sin(q(BaseRotX))/cos(q(BaseRotY)), cos(q(BaseRotX))/cos(q(BaseRotY));
    RateMatrix <<
        1.0/cos(q(BaseRotY)), 0.0, 0.0,
        0,                    1.0, 0.0,
        tan(q(BaseRotY)),     0.0, 1.0;

    dq(BaseRotX) = propmsg.angular_velocity.x;
    dq(BaseRotY) = propmsg.angular_velocity.y;
    dq(BaseRotZ) = propmsg.angular_velocity.z;
    dq.segment(BaseRotX,3) = RateMatrix * dq.segment(BaseRotX,3);

    // Do euler angles - SUBTRACTING OUT THE YAW!!!
    Eigen::Matrix3d R = quat.toRotationMatrix();
    Eigen::EulerAnglesXYZd euler = Eigen::EulerAnglesXYZd::FromRotation<false, false, false>(quat);
    eulerXYZ(quat, euler);
    Eigen::Matrix3d Rz;
    Rz << cos(euler.gamma()), -sin(euler.gamma()), 0,
          sin(euler.gamma()), cos(euler.gamma()),  0,
          0,                  0,                   1;
    R = Rz.transpose() * R;
    Eigen::Quaterniond tempquat(R);
    eulerXYZ(tempquat, euler);
    q(BaseRotX) = euler.alpha(); // roll
    q(BaseRotY) = euler.beta();  // pitch
    q(BaseRotZ) = euler.gamma(); // yaw

    // Linear velocity
    dq(BasePosX) = propmsg.linear_velocity.x;
    dq(BasePosY) = propmsg.linear_velocity.y;
    dq(BasePosZ) = propmsg.linear_velocity.z;
}

void unpack_estimation(const cassie_common_toolbox::cassie_estimation_msg::ConstPtr& estmsg, Eigen::VectorXd &q, Eigen::VectorXd &dq, Eigen::Quaterniond &quat) {
    q(BasePosX) = estmsg->pose.position.x;
    q(BasePosY) = estmsg->pose.position.y;
    q(BasePosZ) = estmsg->pose.position.z;

    quat.w() = estmsg->pose.orientation.w;
    quat.x() = estmsg->pose.orientation.x;
    quat.y() = estmsg->pose.orientation.y;
    quat.z() = estmsg->pose.orientation.z;

    // Do euler angles - SUBTRACTING OUT THE YAW!!!
    Eigen::Matrix3d R = quat.toRotationMatrix();
    Eigen::EulerAnglesXYZd euler = Eigen::EulerAnglesXYZd::FromRotation<false, false, false>(quat);
    eulerXYZ(quat, euler);
    Eigen::Matrix3d Rz;
    Rz << cos(euler.gamma()), -sin(euler.gamma()), 0,
          sin(euler.gamma()), cos(euler.gamma()),  0,
          0,                  0,                   1;
    R = Rz.transpose() * R;
    Eigen::Quaterniond tempquat(R);
    eulerXYZ(tempquat, euler);
    q(BaseRotX) = euler.alpha(); // roll
    q(BaseRotY) = euler.beta();  // pitch
    q(BaseRotZ) = euler.gamma(); // yaw

    // Velocity is forward facing (remove yaw)
    //dq(BasePosX) = estmsg->twist.linear.x;
    //dq(BasePosY) = estmsg->twist.linear.y;
    //dq(BasePosZ) = estmsg->twist.linear.z;
    //dq.block(BasePosX,0,3,1) = Rz.transpose() * dq.block(BasePosX,0,3,1);
}

#endif // CASSIE_CALLBACK_HELPERS_HPP


#ifndef SGM_RP_HANSER96_HH_
#define SGM_RP_HANSER96_HH_

#include <boost/graph/adjacency_list.hpp>

#include "sgm/RingPerception.hh"

#include <limits>
#include <set>

namespace sgm {

	 /*! @brief Ring enumeration ala Hanser et al.
	  *
	  * Implementation of the exhaustive ring perception algorithm by
	  * Hanser et al. (1996)
	  *
	  *   A New Algorithm for Exhaustive Ring Perception in a Molecular Graph
	  *   T. Hanser, P. Jauffret, and G. Kaufmann
	  *   J. Chem. Inf. Comput. Sci., 1996, 36, 1146-1152
	  *
	  * @author Martin Mann - 2010 - http://www.bioinf.uni-freiburg.de/~mmann/
	  *
	  */
	class RP_Hanser96 : public RingPerception {
	public:

		typedef std::set< std::pair<size_t,size_t> > BondSet;

		  //! construction
		RP_Hanser96();

		  //! destruction
		virtual ~RP_Hanser96();

		  /*!
		   * Finds ALL rings within the given graph and reports each found
		   * ring to the assigned reporter. The enumeration can be restricted
		   * to rings up to a given ring size, ie. number of nodes per ring.
		   *
		   * @param graph the graph to be analyzed
		   * @param reporter the RingReporter to report all found rings to
		   * @param maxRingSize the maximal size of rings to report
		   * @return the number of all rings within the gaph
		   */
		size_t
		findRings( const Graph_Interface & graph,
					RingReporter & reporter,
					const size_t maxRingSize = std::numeric_limits<size_t>::max());

		  /*!
		   * Identifies all bonds participating in rings and stores the
		   * according vertex index pairs in the provided container.
		   *
		   * @param graph the graph to search for ring bonds
		   * @param ringBonds OUT : the container to fill with the ring bonds.
		   *   NOTE: the container is cleared at the beginning of the call.
		   * @param maxRingSize the maximal size of rings to consider
		   *
		   * @return the number of ring bonds within the graph
		   */
		size_t
		findRingBonds( const Graph_Interface & graph
					, BondSet & ringBonds
					, const size_t maxRingSize = std::numeric_limits<size_t>::max() );

	protected:

		  //! information on the current ring part
		struct RingInfo {
		public:
			RingReporter::RingNodes nodes;
			RingReporter::RingList path;
		};

		  //! The properties available for the nodes of the P-graph
		typedef	boost::property<	boost::vertex_index_t, size_t >
						Graph_NodeProperties;

		  //! The properties available for the edges of the P-graph
		typedef	boost::property<	boost::edge_name_t, RingInfo >
						Graph_EdgeProperties;

		  //! the P-graph type that is used to detect all rings
		typedef boost::adjacency_list<
							boost::multisetS,      			// store edges
							boost::vecS,       				// store vertices
							boost::undirectedS,				// is an undirected graph
							Graph_NodeProperties,  			// index information
							Graph_EdgeProperties   			// ring information
						>
							P_Graph;

	protected:

		  //! the P-graph to be compressed during the ring perception
		P_Graph pGraph;

		  //! the index access for pGraph
		boost::property_map<P_Graph, boost::vertex_index_t>::type pGraphIndex;
		  //! the edge label access for pGraph, i.e. the according RingList
		boost::property_map<P_Graph, boost::edge_name_t>::type pGraphPath;

		  //! the degree of each node in pGraph
		std::vector<size_t> pGraphDegree;
		  //! the nodes to be removed during the ring perception from pGraph
		std::vector<size_t> toRemove;

	protected:

		  /*!
		   * Initializes the pGraph member with the given graph. If direct loops
		   * are found, these are directly reported and counted.
		   *
		   * @param graph the graph to be encoded in the pGraph member
		   * @param reporter the RingReporter to report all rings to
		   * @return the number of rings found
		   */
		size_t
		initializeP_Graph( const Graph_Interface & graph
							, RingReporter& reporter);

		  /*!
		   * Virtually removes the vertex nextToRemove from the pGraph and
		   * reports all found rings to the given reporter.
		   * The removal is only virtual, since the node remains within the
		   * pGraph but all adjacent edges are removed, i.e. it is not
		   * accessible anymore in later iterations. This is to ease the
		   * maintenance of the temporary data structures.
		   *
		   * @param nextToRemove the index of the vertex to be removed
		   * @param graph the graph searched (needed for ring report)
		   * @param reporter the RingReporter to report all rings to
		   * @param maxRingSize the maximal ring size to consider
		   * @return the number of rings found
		   */
		size_t
		remove_vertex( const size_t nextToRemove
						, const Graph_Interface& graph
						, RingReporter& reporter
						, const size_t maxRingSize);

		struct degree_sort {
			const std::vector<size_t> & degree;
			degree_sort( const std::vector<size_t> & degree_ )
			 : degree(degree_)
			{}
			bool operator()(const size_t& i1, const size_t& i2 ) const {
				return degree[i1] > degree[i2];
			}
		};

		/**
		 * Stores the bond information for loops.
		 */
		class LoopBondReporter : public RingReporter {

		protected:

			BondSet & bondSet;

		public:
			  //! construction
			LoopBondReporter( BondSet & bondSet );
			  //! destruction
			virtual ~LoopBondReporter();

			  /*!
			   * Is called to report a ring.
			   * @param graph the graph that contains the ring
			   * @param ringList the ring to report
			   */
			virtual
			void
			reportRing( const Graph_Interface& graph, const RingList & ringList );

		};

	};

} // namespace sgm

#endif /* SGM_RP_HANSER96_HH_ */


#include <boost/hana/mod.hpp>


// Boost.Polygon library voronoi_diagram_test.cpp file

//          Copyright Andrii Sydorchuk 2010-2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#define BOOST_TEST_MODULE voronoi_diagram_test
#include <boost/test/test_case_template.hpp>

#include <boost/polygon/voronoi_diagram.hpp>
#include <boost/polygon/voronoi_geometry_type.hpp>
using namespace boost::polygon;

typedef voronoi_cell<double> voronoi_cell_type;
typedef voronoi_vertex<double> voronoi_vertex_type;
typedef voronoi_edge<double> voronoi_edge_type;
typedef voronoi_diagram<double> voronoi_diagram_type;

BOOST_AUTO_TEST_CASE(voronoi_cell_test) {
  voronoi_cell_type cell(1, SOURCE_CATEGORY_INITIAL_SEGMENT);
  cell.color(27);
  BOOST_CHECK(!cell.contains_point());
  BOOST_CHECK(cell.contains_segment());
  BOOST_CHECK(cell.is_degenerate());
  BOOST_CHECK(cell.source_index() == 1);
  BOOST_CHECK(cell.source_category() == SOURCE_CATEGORY_INITIAL_SEGMENT);
  BOOST_CHECK(cell.incident_edge() == NULL);
  BOOST_CHECK(cell.color() == 27);

  voronoi_edge_type edge(true, true);
  cell.incident_edge(&edge);
  BOOST_CHECK(!cell.is_degenerate());
  BOOST_CHECK(cell.incident_edge() == &edge);
}

BOOST_AUTO_TEST_CASE(voronoi_vertex_test) {
  voronoi_vertex_type vertex(1, 2);
  vertex.color(27);
  BOOST_CHECK(vertex.is_degenerate());
  BOOST_CHECK(vertex.x() == 1);
  BOOST_CHECK(vertex.y() == 2);
  BOOST_CHECK(vertex.incident_edge() == NULL);
  BOOST_CHECK(vertex.color() == 27);

  voronoi_edge_type edge(true, true);
  vertex.incident_edge(&edge);
  BOOST_CHECK(!vertex.is_degenerate());
  BOOST_CHECK(vertex.incident_edge() == &edge);
}

BOOST_AUTO_TEST_CASE(voronoi_edge_test) {
  voronoi_edge_type edge1(false, false);
  edge1.color(13);
  BOOST_CHECK(!edge1.is_primary());
  BOOST_CHECK(edge1.is_secondary());
  BOOST_CHECK(!edge1.is_linear());
  BOOST_CHECK(edge1.is_curved());
  BOOST_CHECK(!edge1.is_finite());
  BOOST_CHECK(edge1.is_infinite());
  BOOST_CHECK(edge1.color() == 13);

  voronoi_edge_type edge2(true, true);
  edge2.color(14);
  BOOST_CHECK(edge2.is_primary());
  BOOST_CHECK(!edge2.is_secondary());
  BOOST_CHECK(edge2.is_linear());
  BOOST_CHECK(!edge2.is_curved());
  BOOST_CHECK(!edge2.is_finite());
  BOOST_CHECK(edge2.is_infinite());
  BOOST_CHECK(edge2.color() == 14);

  edge1.twin(&edge2);
  edge2.twin(&edge1);
  BOOST_CHECK(edge1.twin() == &edge2);
  BOOST_CHECK(edge2.twin() == &edge1);

  edge1.next(&edge2);
  edge1.prev(&edge2);
  edge2.next(&edge1);
  edge2.prev(&edge1);
  BOOST_CHECK(edge1.next() == &edge2);
  BOOST_CHECK(edge1.prev() == &edge2);
  BOOST_CHECK(edge1.rot_next() == &edge1);
  BOOST_CHECK(edge1.rot_prev() == &edge1);

  voronoi_cell_type cell(1, SOURCE_CATEGORY_INITIAL_SEGMENT);
  edge1.cell(&cell);
  BOOST_CHECK(edge1.cell() == &cell);

  voronoi_vertex_type vertex0(1, 2);
  edge1.vertex0(&vertex0);
  BOOST_CHECK(edge1.vertex0() == &vertex0);
  BOOST_CHECK(edge2.vertex1() == &vertex0);

  voronoi_vertex_type vertex1(2, 1);
  edge2.vertex0(&vertex1);
  BOOST_CHECK(edge1.vertex1() == &vertex1);
  BOOST_CHECK(edge2.vertex0() == &vertex1);

  BOOST_CHECK(edge1.is_finite());
  BOOST_CHECK(edge2.is_finite());
}

BOOST_AUTO_TEST_CASE(voronoi_diagram_test) {
  voronoi_diagram_type vd;
  BOOST_CHECK(vd.num_cells() == 0);
  BOOST_CHECK(vd.num_vertices() == 0);
  BOOST_CHECK(vd.num_edges() == 0);
  vd.clear();
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_SSE_AVX_SPLAT_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_SSE_AVX_SPLAT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/operator/functions/splat.hpp>

//==============================================================================
// Implementation when type A0 is double
//==============================================================================
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splat_, boost::simd::tag::avx_, (A0)(A1)
                            , (scalar_< fundamental_<A0> >)
                              ((target_< simd_< double_<A1>, boost::simd::tag::avx_ > >))
                      )
  {
    typedef typename A1::type result_type;
    BOOST_DISPATCH_FORCE_INLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return _mm256_set1_pd(double(a0));
    }
  };
} } }

//==============================================================================
// Implementation when type A0 is float
//==============================================================================
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splat_, boost::simd::tag::avx_, (A0)(A1)
                            , (scalar_< fundamental_<A0> >)
                              ((target_< simd_< single_<A1>, boost::simd::tag::avx_ > >))
                      )
  {
    typedef typename A1::type result_type;
    BOOST_DISPATCH_FORCE_INLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return _mm256_set1_ps(float(a0));
    }
  };
} } }

//==============================================================================
// Implementation when type A0 is ints8
//==============================================================================
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splat_, boost::simd::tag::avx_, (A0)(A1)
                            , (scalar_< fundamental_<A0> >)
                              ((target_< simd_< ints8_<A1>, boost::simd::tag::avx_ > >))
                      )
  {
    typedef typename A1::type result_type;
    BOOST_DISPATCH_FORCE_INLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return _mm256_set1_epi8(a0);
    }
  };
} } }

//==============================================================================
// Implementation when type A0 is ints16
//==============================================================================
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splat_, boost::simd::tag::avx_, (A0)(A1)
                            , (scalar_< fundamental_<A0> >)
                              ((target_< simd_< ints16_<A1>, boost::simd::tag::avx_ > >))
                      )
  {
    typedef typename A1::type result_type;
    BOOST_DISPATCH_FORCE_INLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return _mm256_set1_epi16(a0);
    }
  };
} } }

//==============================================================================
// Implementation when type A0 is ints32
//==============================================================================
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splat_, boost::simd::tag::avx_, (A0)(A1)
                            , (scalar_< fundamental_<A0> >)
                              ((target_< simd_< ints32_<A1>, boost::simd::tag::avx_ > >))
                      )
  {
    typedef typename A1::type result_type;
    BOOST_DISPATCH_FORCE_INLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return _mm256_set1_epi32(a0);
    }
  };
} } }

//==============================================================================
// Implementation when type A0 is ints64
//==============================================================================
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splat_, boost::simd::tag::avx_, (A0)(A1)
                            , (scalar_< fundamental_<A0> >)
                              ((target_< simd_< ints64_<A1>, boost::simd::tag::avx_ > >))
                      )
  {
    typedef typename A1::type result_type;
    BOOST_DISPATCH_FORCE_INLINE result_type operator()(A0 const& a0, A1 const&) const
    {
#ifndef BOOST_MSVC
      result_type that = _mm256_set1_epi64x(a0);
#else
      result_type that = make<result_type>(a0, a0, a0, a0);
#endif
      return that;
    }
  };
} } }

#endif
#endif


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

#ifndef STATE_ESTIMATION__MEASUREMENT__LINEAR_MEASUREMENT_HPP_
#define STATE_ESTIMATION__MEASUREMENT__LINEAR_MEASUREMENT_HPP_

#include <common/type_traits.hpp>
#include <state_estimation/measurement/measurement_interface.hpp>

#include <Eigen/Core>

#include <chrono>
#include <tuple>

namespace autoware
{
namespace common
{
namespace state_estimation
{

///
/// @brief      A class that represents a linear measurement.
///
/// @details    A linear measurement is one that measures directly variables from some unknown real
///             state, thus requiring no measurement Jacobian computation. The mapping matrix will
///             only have 1s and 0s in it.
///
/// @tparam     StateT  A measured state.
///
template<typename StateT>
class STATE_ESTIMATION_PUBLIC LinearMeasurement
  : public MeasurementInterface<LinearMeasurement<StateT>>
{
  template<typename OtherStateT>
  using MappingMatrixFrom =
    Eigen::Matrix<typename StateT::Scalar, StateT::size(), OtherStateT::size()>;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using State = StateT;
  /// @brief      Default constructor.
  LinearMeasurement() = default;
  ///
  /// @brief      Construct a new instance of the measurement from a state vector.
  ///
  /// @param[in]  measurement  The measurement
  /// @param[in]  covariance   The covariance
  ///
  explicit LinearMeasurement(
    const typename StateT::Vector & measurement,
    const typename StateT::Matrix & covariance)
  : m_measurement{measurement},
    m_covariance{covariance} {}
  ///
  /// @brief      Convenience factory function to construct a measurement from a vector of standard
  ///             deviations.
  ///
  /// @details    This results in a diagonal covariance matrix, with the squared standard
  ///             deviations as entries. In a diagonal covariance matrix, the variables are
  ///             uncorrelated.
  ///
  /// @param[in]  measurement         The measurement
  /// @param[in]  standard_deviation  The standard deviation for each variable.
  ///
  static LinearMeasurement create_with_stddev(
    const typename StateT::Vector & measurement,
    const typename StateT::Vector & standard_deviation)
  {
    return LinearMeasurement{measurement,
      standard_deviation.array().square().matrix().asDiagonal()};
  }

  ///
  /// @brief      Cast to another scalar type.
  ///
  /// @return     The measurement with a different scalar type.
  ///
  template<typename NewScalarT>
  auto cast() const noexcept
  {
    using NewState = decltype(m_measurement.template cast<NewScalarT>());
    return LinearMeasurement<NewState>{
      m_measurement.vector().template cast<NewScalarT>(),
      m_covariance.template cast<NewScalarT>()
    };
  }

protected:
  // Allow the CRTP interface to call private functions from this class.
  friend MeasurementInterface<LinearMeasurement<StateT>>;

  /// @brief      Get state vector function that is to be called by the CRTP interface.
  inline StateT & crtp_state() noexcept {return m_measurement;}
  /// @brief      Get state vector function that is to be called by the CRTP interface.
  inline const StateT & crtp_state() const noexcept {return m_measurement;}
  /// @brief      Get covariance function that is to be called by the CRTP interface.
  inline typename StateT::Matrix & crtp_covariance() noexcept {return m_covariance;}
  /// @brief      Get covariance function that is to be called by the CRTP interface.
  inline const typename StateT::Matrix & crtp_covariance() const noexcept {return m_covariance;}
  ///
  /// @brief      Create a new instance of the measurement from another state.
  ///
  /// @details    This is a linear measurement, i.e., it directly observes the variables of another
  ///             state, thus this mapping function is linear and just copied values for the
  ///             matching variables in both states.
  ///
  /// @note       It is expected that the measurement StateT is a direct sub-state of the
  ///             OtherStateT.
  ///
  /// @param[in]  other_state  The other state
  ///
  /// @tparam     OtherStateT  State to which we map the internal state StateT.
  ///
  /// @return     A vector that represents a mapping of another state to this measurement's frame.
  ///
  template<typename OtherStateT>
  StateT crtp_create_new_instance_from(const OtherStateT & other_state) const
  {
    return other_state.template copy_into<StateT>();
  }
  ///
  /// @brief      Copy this measurement into another state space.
  ///
  /// @note       Values present in other_state but not in this measurement will stay intact.
  ///
  /// @param[in]  other_state  The other state
  ///
  /// @tparam     OtherStateT  State to which we map the internal state StateT.
  ///
  /// @return     A vector that represents a mapping of another state to this measurement's frame.
  ///
  template<typename OtherStateT>
  OtherStateT crtp_map_into(const OtherStateT & other_state) const noexcept
  {
    return m_measurement.copy_into(other_state);
  }
  ///
  /// @brief      Get the mapping matrix between the two states called by the CRTP interface.
  ///
  /// @details    As this is a linear measurement (one that directly measures variables of some
  ///             state) this function only fills the entries of the mapping matrix with 1s and 0s.
  ///
  /// @note       The OtherStateT input parameter is not used in this implementation. It is required
  ///             for other implementation that must map into the measurement state space in a
  ///             non-linear way.
  ///
  /// @tparam     OtherStateT  State to which we map the internal state StateT.
  ///
  /// @return     A matrix M, such that other_state = M * this_state.
  ///
  template<typename OtherStateT>
  MappingMatrixFrom<OtherStateT> crtp_mapping_matrix_from(const OtherStateT &) const
  {
    MappingMatrixFrom<OtherStateT> m{MappingMatrixFrom<OtherStateT>::Zero()};
    auto fill_mapping_matrix = [&m, this](auto variable) {
        using VariableT = std::decay_t<decltype(variable)>;
        constexpr auto index_in_this_state = StateT::template index_of<VariableT>();
        constexpr auto index_in_other_state = OtherStateT::template index_of<VariableT>();
        m(index_in_this_state, index_in_other_state) = 1;
      };
    using CommonVariablesTuple = typename autoware::common::type_traits::intersect<
      typename StateT::Variables, typename OtherStateT::Variables>::type;
    common::type_traits::visit(CommonVariablesTuple{}, fill_mapping_matrix);
    return m;
  }

  /// @brief An equality operator.
  friend bool operator==(const LinearMeasurement & lhs, const LinearMeasurement & rhs)
  {
    return (lhs.state() == rhs.state()) &&
           lhs.covariance().isApprox(rhs.covariance());
  }

private:
  /// Current measurement vector.
  StateT m_measurement{};
  /// Current measurement covariance matrix.
  typename StateT::Matrix m_covariance{StateT::Matrix::Zero()};
};

}  // namespace state_estimation
}  // namespace common
}  // namespace autoware

#endif  // STATE_ESTIMATION__MEASUREMENT__LINEAR_MEASUREMENT_HPP_


// Copyright Ryan Schmidt 2011.
// Distributed under the Boost Software License, Version 1.0.
// (See copy at http://www.boost.org/LICENSE_1_0.txt)

#include "opengl.h"
#include "LaplacianSmoother.h"
#include <MeshUtils.h>
#include <Wm4LinearSystem.h>
#include <SparseLinearSystem.h>
#include <Solver_TAUCS.h>
#include <rmsdebug.h>

#include <Eigen/Core>
#include <Eigen/Sparse>
typedef Eigen::SparseMatrix<double,Eigen::RowMajor> SparseMatrixType;

using namespace rms;

LaplacianSmoother::LaplacianSmoother()
{
	m_pMesh = NULL;
	m_pSolver = NULL;
	m_pSystemM = NULL;
	m_pLs = new gsi::SparseMatrix();
	m_pM = new gsi::SparseMatrix();
	m_pSystem = new gsi::SparseMatrix();
	m_pRHS = new gsi::Vector[3];

	m_fLaplacianVectorScale = 0.0f;
	m_fInteriorConstraintWeightScale = 1.0f;

	m_eWeightMode = Weights_Cotan;
	m_eVertexAreaMode = Area_Mixed;

	m_nROISize = 0;

	m_bWeightsValid = false;
	m_bMatricesValid = false;
	m_bSolutionValid = false;
}
LaplacianSmoother::~LaplacianSmoother()
{
	if ( m_pLs )
		delete m_pLs;
	if ( m_pM )
		delete m_pM;
	if ( m_pSystem )
		delete m_pSystem;
	if ( m_pRHS )
		delete [] m_pRHS;
}

gsi::Solver_TAUCS * LaplacianSmoother::GetSolver()
{
	if ( m_pSolver == NULL )
		m_pSolver = new gsi::Solver_TAUCS(GetSystem());
	return m_pSolver;
}

gsi::SparseLinearSystem * LaplacianSmoother::GetSystem()
{
	if ( m_pSystemM == NULL )
		m_pSystemM = new gsi::SparseLinearSystem();
	return m_pSystemM;
}

void LaplacianSmoother::SetMesh(rms::VFTriangleMesh * pMesh)
{
	m_pMesh = pMesh;
	m_vConstraints.resize(0);

	m_vROI.clear();
	m_vROIBoundary.clear();
	m_nROISize = 0;

	if ( m_pMesh == NULL )
		return;

	m_vMap.Resize(m_pMesh->GetMaxVertexID(), m_pMesh->GetMaxVertexID());

	VFTriangleMesh::vertex_iterator curv(m_pMesh->BeginVertices()), endv(m_pMesh->EndVertices());
	unsigned int nIndex = 0;
	while ( curv != endv ) {
		IMesh::VertexID vID = *curv++;

		if ( m_pMesh->IsIsolated(vID) )
			continue;

		m_vROI.insert(vID);
		m_vMap.SetMap( vID, nIndex );
		if ( m_pMesh->IsBoundaryVertex(vID) )
			m_vROIBoundary.insert(nIndex);
		nIndex++;
	}
	m_nROISize = nIndex;

	m_vVertices.resize(0);
	m_bWeightsValid = false;
	m_bMatricesValid = false;
}
void LaplacianSmoother::SetROI( const std::vector<IMesh::VertexID> & vROI )
{
	m_vConstraints.resize(0);

	m_nROISize = vROI.size();
	m_vROI.clear();
	m_vMap.Resize(m_pMesh->GetMaxVertexID(), (unsigned int)m_nROISize);
	m_vROIBoundary.clear();
	std::vector<IMesh::VertexID> vUseROI;

	unsigned int nIndex = 0;
	for ( unsigned int k = 0; k < m_nROISize; ++k ) {
		IMesh::VertexID vID = vROI[k];
		if ( m_pMesh->IsIsolated(vID) )
			continue;
		m_vROI.insert(vID);
		m_vMap.SetMap( vID, nIndex );
		vUseROI.push_back(vID);
		nIndex++;
	}
	m_nROISize = nIndex;

	std::vector<IMesh::VertexID> vOneRing;
	for ( unsigned int k = 0; k < m_nROISize; ++k ) {
		IMesh::VertexID vID = vUseROI[k];

		bool bBoundary = m_pMesh->IsBoundaryVertex(vID);
		if ( ! bBoundary ) {
			vOneRing.resize(0);
			MeshUtils::VertexOneRing(*m_pMesh, vID, vOneRing, false);
			size_t nCount = vOneRing.size();
			for ( unsigned int j = 0; !bBoundary && j < nCount; ++j ) {
				if ( m_vROI.find(vOneRing[j]) ==  m_vROI.end() )
					bBoundary = true;
			}
		}
		if ( bBoundary )
			m_vROIBoundary.insert(k);
	}

	m_vVertices.resize(0);
	m_bWeightsValid = false;
	m_bMatricesValid = false;
}

void LaplacianSmoother::AddBoundaryConstraints(float fWeight)
{
	std::set<unsigned int>::iterator curv(m_vROIBoundary.begin()), endv(m_vROIBoundary.end());
	while ( curv != endv ) {
		IMesh::VertexID vID = m_vMap.GetOld( *curv++ );
		Wml::Vector3f v;
		m_pMesh->GetVertex(vID, v);
		UpdateConstraint(vID, v, fWeight, CType_SoftBoundary);
	}
}
void LaplacianSmoother::AddSoftBoundaryConstraints(float fWeight, int nRings, bool bBlendWeight)
{
	ValidateWeights();		// have to do this because we use m_vVertices.vNbrs below...

	std::set<unsigned int> vDone;
	std::set<unsigned int> vCur(m_vROIBoundary);
	std::vector<unsigned int> vNext;
	std::vector<IMesh::VertexID> vOneRing;
	for ( int k = 0; k < nRings; ++k ) {

		vNext.resize(0);
		std::set<unsigned int>::iterator curv(vCur.begin()), endv(vCur.end());
		while ( curv != endv ) {
			unsigned int nIndex = *curv++;
			IMesh::VertexID vID = m_vMap.GetOld( nIndex );
			Wml::Vector3f v;
			m_pMesh->GetVertex(vID, v);
			UpdateConstraint(vID, v, fWeight,
				(k == 0) ? CType_SoftBoundary_Ring0 : CType_SoftBoundary );
			vNext.push_back( nIndex );
			vDone.insert(nIndex);
		}

		if ( bBlendWeight )
			fWeight = fWeight/2;

		vCur.clear();
		size_t nNext = vNext.size();
		for ( unsigned int k = 0; k < nNext; ++k ) {
			std::vector<unsigned int> & vNbrs = m_vVertices[vNext[k]].vNbrs;
			size_t nCount = vNbrs.size();
			for ( unsigned int k = 0; k < nCount; ++k ) {
				if ( vDone.find(vNbrs[k]) == vDone.end() )
					vCur.insert(vNbrs[k]);
			}
		}
	}

}

void LaplacianSmoother::AddAllInteriorConstraints(float fWeight)
{
	for ( unsigned int k = 0; k < m_nROISize; ++k ) {
		if ( m_vROIBoundary.find(k) != m_vROIBoundary.end() )
			continue;
		IMesh::VertexID vID = m_vMap.GetOld( k );
		Wml::Vector3f v;
		m_pMesh->GetVertex(vID, v);
		UpdateConstraint(vID, v, fWeight, CType_SoftInterior);
	}
}

void LaplacianSmoother::UpdateConstraint( IMesh::VertexID vID, const Wml::Vector3f & vPosition, float fWeight, ConstraintType eType )
{
	unsigned int nIndex = m_vMap.GetNew(vID);
	if ( nIndex == IMesh::InvalidID )
		return;

	bool bFound = false;
	size_t nCount = m_vConstraints.size();
	for ( unsigned int k = 0; !bFound && k < nCount; ++k ) {
		if ( m_vConstraints[k].vID == vID ) {
			m_vConstraints[k].vPosition = vPosition;
			if ( m_vConstraints[k].eType != eType && eType != CType_Unspecified ) {
				m_bMatricesValid = false;
				m_vConstraints[k].eType = eType;
			}
			if ( m_vConstraints[k].fWeight != fWeight ) {
				m_bMatricesValid = false;
				m_vConstraints[k].fWeight = fWeight;
			}
			bFound = true;
		}
	}
	if ( ! bFound ) {
		Constraint c;
		c.eType = eType;
		c.vID = vID;
		c.nIndex = nIndex;
		c.vPosition = vPosition;
		c.fWeight = fWeight;
		m_vConstraints.push_back(c);
		m_bMatricesValid = false;
	} else
		m_bSolutionValid = false;
}

void LaplacianSmoother::UpdateConstraint( IMesh::VertexID vID, const Wml::Vector3f & vPosition )
{
	unsigned int nIndex = m_vMap.GetNew(vID);
	if ( nIndex == IMesh::InvalidID )
		return;

	size_t nCount = m_vConstraints.size();
	for ( unsigned int k = 0; k < nCount; ++k ) {
		if ( m_vConstraints[k].vID == vID ) {
			m_vConstraints[k].vPosition = vPosition;
			m_bSolutionValid = false;
			return;
		}
	}
}

LaplacianSmoother::ConstraintType LaplacianSmoother::GetConstraint( IMesh::VertexID vID )
{
	size_t nCount = m_vConstraints.size();
	for ( unsigned int k = 0; k < nCount; ++k ) {
		if ( m_vConstraints[k].vID == vID )
			return m_vConstraints[k].eType;
	}
	return CType_Unconstrained;
}

void LaplacianSmoother::UpdateConstraintsFromMesh()
{
	size_t nVerts = m_vConstraints.size();
	for ( unsigned int i = 0; i < nVerts; ++i ) {
		Constraint & ci = m_vConstraints[i];
		ci.vPosition = m_pMesh->GetVertex( ci.vID );
	}
	m_bSolutionValid = false;
}

void LaplacianSmoother::SnapRing0BoundaryConstraints()
{
	size_t nVerts = m_vConstraints.size();
	for ( unsigned int i = 0; i < nVerts; ++i ) {
		Constraint & ci = m_vConstraints[i];
		if ( ci.eType == CType_SoftBoundary_Ring0 )
			m_pMesh->SetVertex( ci.vID, ci.vPosition );
	}
}

void LaplacianSmoother::ValidateWeights()
{
	if ( m_bWeightsValid )
		return;

	// restore vertex positions (?? only need to do this after SetWeightMode/SetVertexArea!! should set flag or something)
	if (! m_vVertices.empty() && m_vVertices.size() == m_nROISize ) {
		for ( unsigned int i = 0; i < m_nROISize; ++i ) {
			VtxInfo & vi = m_vVertices[i];
			IMesh::VertexID vID = m_vMap.GetOld(i);
			m_pMesh->SetVertex( vID, vi.vOrigPosition );
		}
	}
	m_vVertices.resize(0);

	m_vVertices.resize( m_nROISize );
	m_nEdges = 0;

	m_fAvgVtxArea = 0.0f;
	std::vector<IMesh::VertexID> vOneRing;
	for ( unsigned int i = 0; i < m_nROISize; ++i ) {
		VtxInfo & vi = m_vVertices[i];
		IMesh::VertexID vID = m_vMap.GetOld(i);

		vi.vOrigPosition = m_pMesh->GetVertex(vID);

		vOneRing.resize(0);
		MeshUtils::VertexOneRing(*m_pMesh, vID, vOneRing, false);

		// don't need to do this if we do not have ROI...
		size_t nNbrs = vOneRing.size();
		for ( unsigned int k = 0; k < nNbrs; ++k ) {
			if ( m_vROI.find(vOneRing[k]) != m_vROI.end() )
				vi.vNbrs.push_back(vOneRing[k]);
		}
		if ( vi.vNbrs.size() == 0 ) {
			bool bIsolated = m_pMesh->IsIsolated(vID);
			lgBreakToDebugger();
		}
		m_nEdges += (unsigned int)vi.vNbrs.size();

		// compute laplacian vector
		switch ( m_eWeightMode ) {
			case Weights_Uniform:
				MeshUtils::UniformWeights(*m_pMesh, vID, vi.vNbrs, vi.vNbrWeights);
				break;
			case Weights_Cotan:
			default:
				MeshUtils::CotangentWeights(*m_pMesh, vID, vi.vNbrs, vi.vNbrWeights);
				break;
		}
		vi.vMeshLaplacian = MeshUtils::MeshLaplacian(*m_pMesh, vID, vi.vNbrs, vi.vNbrWeights);
		vi.vCurLaplacian = vi.vMeshLaplacian;

		// compute vertex area
		switch ( m_eVertexAreaMode ) {
			case Area_One:
				vi.vVtxArea = 1.0f;
				break;
			case Area_Mixed:
			default:
				vi.vVtxArea = MeshUtils::VertexArea_Mixed(*m_pMesh, vID);
		}
		m_fAvgVtxArea += (vi.vVtxArea / (float)m_nROISize);

		// convert neighbour VertexIDs to indices
		for ( unsigned int k = 0; k < vi.vNbrs.size(); ++k )
			vi.vNbrs[k] = m_vMap.GetNew(vi.vNbrs[k]);
	}

	// rescale vertex areas, so that constraint weight scales are not affected
	for ( unsigned int i = 0; i < m_nROISize; ++i ) {
		VtxInfo & vi = m_vVertices[i];
		vi.vVtxArea /= m_fAvgVtxArea;
	}

	m_bWeightsValid = true;
	m_bMatricesValid = false;
	m_bSolutionValid = false;
}

void LaplacianSmoother::UpdateSytemMatrix_ThinPlate()
{
	ValidateWeights();

	if ( m_bMatricesValid )
		return;

	unsigned int nVerts = (unsigned int)m_vVertices.size();

	GetSystem()->Resize(nVerts, nVerts);
	GetSystem()->ResizeRHS(3);
	for ( int k = 0; k < 3; ++k )
		m_pRHS[k].Resize( nVerts );

	gsi::SparseMatrix & Msys = (*m_pSystem);
	Msys.Clear();
	Msys.Resize(nVerts,nVerts);

#if 0
	gsi::SparseMatrix & Minv = (*m_pM);
	Minv.Resize(nVerts, nVerts);
	gsi::SparseMatrix & Ls = (*m_pLs);
	Ls.Resize(nVerts, nVerts);

	gsi::SparseMatrix & Minv = (*m_pM);
	Minv.Resize(nVerts, nVerts);

	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];
		size_t nNbrs = vi.vNbrs.size();

		double dSum = 0.0f;
		for ( unsigned int k = 0; k < nNbrs; ++k ) {
			Ls(ri, vi.vNbrs[k]) = vi.vNbrWeights[k];
			dSum += vi.vNbrWeights[k];
		}
		Ls(ri, ri) = -dSum;
		Minv(ri,ri) = 1.0f / vi.vVtxArea;
	}

	// construct system
	Msys = Ls * Minv * Ls;
#endif

#if 1
	SparseMatrixType Ls(nVerts,nVerts);
	Ls.reserve(m_nEdges+nVerts);
	SparseMatrixType Minv(nVerts, nVerts);
	Minv.reserve(nVerts);
	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];
		size_t nNbrs = vi.vNbrs.size();

		double dSum = 0.0f;
		for ( unsigned int k = 0; k < nNbrs; ++k ) {
			Ls.insert(ri, vi.vNbrs[k]) = vi.vNbrWeights[k];
			dSum += vi.vNbrWeights[k];
		}
		Ls.insert(ri, ri) = -dSum;

		Minv.insert(ri,ri) = 1.0f / vi.vVtxArea;
	}
	Ls.finalize();
	Minv.finalize();
	SparseMatrixType LsMinvLs = Ls * Minv * Ls;

	for (unsigned int k=0; k < nVerts; ++k) {
		for ( SparseMatrixType::InnerIterator it(LsMinvLs,k); it; ++it)
			Msys.Set( it.row(), it.col(), it.value() );
	}
#endif

	m_bMatricesValid = true;
	m_bSolverValid = false;
	m_bSolutionValid = false;
}

void LaplacianSmoother::UpdateSolver_ThinPlate()
{
	if ( m_bSolverValid )
		return;

	GetSystem()->SetMatrix(*m_pSystem);

	// add soft constraints
	unsigned int nCons = (unsigned int)m_vConstraints.size();
	for ( unsigned int ci = 0; ci < nCons; ++ci ) {
		Constraint & c = m_vConstraints[ci];
		int ri = c.nIndex;
		float fConsWeight = c.fWeight;
		if ( c.eType == CType_SoftInterior )
			fConsWeight *= m_fInteriorConstraintWeightScale;
		GetSystem()->Set(  ri,ri,   GetSystem()->Get(ri,ri) + (fConsWeight*fConsWeight)  );
	}

	if ( ! GetSystem()->Matrix().IsSymmetric() )
		lgBreakToDebugger();

	GetSolver()->OnMatrixChanged();
	GetSolver()->SetStoreFactorization(true);
	GetSolver()->SetSolverMode( gsi::Solver_TAUCS::TAUCS_LLT );
	GetSolver()->SetOrderingMode( gsi::Solver_TAUCS::TAUCS_METIS );

	m_bSolverValid = true;
	m_bSolutionValid = false;
}

void LaplacianSmoother::UpdateRHS_ThinPlate()
{
	unsigned int nVerts = (unsigned int)m_vVertices.size();

	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];
		double x[3] = {0,0,0};
		float fWeightSum = 0.0f;
		size_t nNbrs = vi.vNbrs.size();
		for ( unsigned int k = 0; k < nNbrs; ++k ) {
			unsigned int nNbrIndex = vi.vNbrs[k];
			float fWeight = vi.vNbrWeights[k];
			fWeightSum += fWeight;
			const Wml::Vector3f & vNbrLaplacian = m_vVertices[nNbrIndex].vCurLaplacian;
			for ( int i = 0; i < 3; ++i ) {
				x[i] += fWeight*vNbrLaplacian[i] * m_fLaplacianVectorScale;
			}
		}
		const Wml::Vector3f & vLaplacian = vi.vCurLaplacian;
		for ( int i = 0; i < 3; ++i )
			m_pRHS[i][ri] = x[i] - fWeightSum*vLaplacian[i] * m_fLaplacianVectorScale;
	}

	unsigned int nCons = (unsigned int)m_vConstraints.size();
	for ( unsigned int ci = 0; ci < nCons; ++ci ) {
		Constraint & c = m_vConstraints[ci];
		int ri = c.nIndex;
		float fConsWeight = c.fWeight;
		if ( c.eType == CType_SoftInterior )
			fConsWeight *= m_fInteriorConstraintWeightScale;
		for ( int k = 0; k < 3; ++k )
			m_pRHS[k][ri] += c.vPosition[k]*fConsWeight*fConsWeight;
	};

	gsi::SparseLinearSystem * pSystem = GetSystem();
	pSystem->SetRHS(0, m_pRHS[0]);
	pSystem->SetRHS(1, m_pRHS[1]);
	pSystem->SetRHS(2, m_pRHS[2]);

	m_bSolutionValid = false;
}

void LaplacianSmoother::UpdateMatrices_Shell()
{
	ValidateWeights();

	if ( m_bMatricesValid )
		return;

	unsigned int nVerts = (unsigned int)m_vVertices.size();

	GetSystem()->Resize(nVerts, nVerts);
	GetSystem()->ResizeRHS(3);

	gsi::SparseMatrix & Ls = (*m_pLs);
	Ls.Resize(nVerts, nVerts);
	gsi::SparseMatrix & Minv = (*m_pM);
	Minv.Resize(nVerts, nVerts);

	float ks = 1;		// weight on membrane term
	float kb = 0;		// weight on thin-plate term

	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];
		size_t nNbrs = vi.vNbrs.size();

		double dSum = 0.0f;
		for ( unsigned int k = 0; k < nNbrs; ++k ) {
			Ls(ri, vi.vNbrs[k]) = vi.vNbrWeights[k];
			dSum += vi.vNbrWeights[k];
		}
		Ls(ri, ri) = -dSum;
		Minv(ri,ri) = 1.0f / vi.vVtxArea;
	}

	// fold in area weights matrix M here

	// construct system
	gsi::SparseMatrix Msys( Ls*(-ks)   +   Ls * Minv * Ls * kb);

	// add soft constraints
	unsigned int nCons = (unsigned int)m_vConstraints.size();
	for ( unsigned int ci = 0; ci < nCons; ++ci ) {
		Constraint & c = m_vConstraints[ci];
		int ri = c.nIndex;
		float fConsWeight = c.fWeight;
		if ( c.eType == CType_SoftInterior )
			fConsWeight *= m_fInteriorConstraintWeightScale;
		Msys(ri,ri) = Msys(ri,ri) + (fConsWeight*fConsWeight);
	}

	GetSystem()->SetMatrix(Msys);

	if ( ! GetSystem()->Matrix().IsSymmetric() )
		lgBreakToDebugger();

	GetSolver()->OnMatrixChanged();
	GetSolver()->SetStoreFactorization(true);
	GetSolver()->SetSolverMode( gsi::Solver_TAUCS::TAUCS_LLT );
	GetSolver()->SetOrderingMode( gsi::Solver_TAUCS::TAUCS_METIS );

	m_bMatricesValid = true;
	m_bSolverValid = true;
	m_bSolutionValid = false;
}

void LaplacianSmoother::UpdateRHS_Shell()
{
	unsigned int nVerts = (unsigned int)m_vVertices.size();

	gsi::SparseLinearSystem * pSystem = GetSystem();

	// RHS is 0 in shell energy
	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];
		Wml::Vector3f vLaplacian = Wml::Vector3f::ZERO;
		for ( int k = 0; k < 3; ++k )
			pSystem->SetRHS(ri, vLaplacian[k], k);
	}

	unsigned int nCons = (unsigned int)m_vConstraints.size();
	for ( unsigned int ci = 0; ci < nCons; ++ci ) {
		Constraint & c = m_vConstraints[ci];
		int ri = c.nIndex;
		float fConsWeight = c.fWeight;
		if ( c.eType == CType_SoftInterior )
			fConsWeight *= m_fInteriorConstraintWeightScale;
		Wml::Vector3f vConsVal = fConsWeight*fConsWeight*c.vPosition;
		for ( int k = 0; k < 3; ++k )
			pSystem->SetRHS( ri, pSystem->GetRHS(ri,k) + vConsVal[k], k );
	};

	m_bSolutionValid = false;
}

bool LaplacianSmoother::Solve()
{
	if ( ! m_bSolutionValid ) {

		UpdateSytemMatrix_ThinPlate();
		UpdateSolver_ThinPlate();
		UpdateRHS_ThinPlate();

		//UpdateMatrices_Shell();
		//UpdateRHS_Shell();

		bool bOK = GetSolver()->Solve();
		if ( ! bOK )
			return false;

		m_bSolutionValid = true;
	}

	int nMatrixCols = (int)m_vVertices.size();
	for ( int i = 0; i < nMatrixCols; ++i ) {
		Wml::Vector3f v((float)GetSystem()->GetSolution(i,0), (float)GetSystem()->GetSolution(i,1), (float)GetSystem()->GetSolution(i,2) );
		IMesh::VertexID vID = m_vMap.GetOld(i);
		m_pMesh->SetVertex(vID, v);
	}
	return true;
}

void LaplacianSmoother::Render()
{
	glPushAttrib(GL_ENABLE_BIT | GL_POINT_BIT | GL_LINE_BIT);
	glDisable(GL_LIGHTING);

//	glDepthFunc(GL_LEQUAL);

	Wml::Vector3f v;

	// render constraint points
	glPointSize(5.0f);
	glBegin(GL_POINTS);
	for ( unsigned int i = 0; i < m_vConstraints.size(); ++i ) {
		glColor3f(0.0f, 1.0f, 0.0f);
		glVertex3fv( m_pMesh->GetVertex(m_vConstraints[i].vID) );
		glColor3f(1.0f, 0.0f, 0.0f);
		glVertex3fv(m_vConstraints[i].vPosition);
	}
	glEnd();
	glBegin(GL_LINES);
	glColor3f(0.0f, 0.0f, 0.0f);
	for ( unsigned int i = 0; i < m_vConstraints.size(); ++i ) {
		glVertex3fv( m_pMesh->GetVertex(m_vConstraints[i].vID) );
		glVertex3fv(m_vConstraints[i].vPosition);
	}
	glEnd();

	// draw current laplacian vectors
	glLineWidth(2.0f);
	glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	for ( unsigned int i = 0; i < m_vVertices.size(); ++i ) {
		m_pMesh->GetVertex( i, v );
		glVertex3fv( v );
		glVertex3fv( v + m_vVertices[i].vCurLaplacian );
	}
	glEnd();

	glPopAttrib();

}