#include "model/pointcloud.h"

#include <limits>
#include <string>
#include <cstdlib>
#include <cassert>
#include <cstdio>

#include <boost/serialization/shared_ptr.hpp>
#include <boost/iostreams/device/mapped_file.hpp>
#include <boost/iostreams/stream.hpp>
#include <boost/iostreams/stream_buffer.hpp>
#include <boost/make_shared.hpp>
#include <pcl/io/io.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <QDebug>

using namespace Eigen;

#ifdef _WIN32
#   define INFINITY (DBL_MAX+DBL_MAX)
#   define NAN (INFINITY-INFINITY)
#endif

inline bool isNaN(float val){
    return (val != val);
}

PointCloud::PointCloud()
    : pcl::PointCloud<pcl::PointXYZI>() {
    pc_mutex.reset(new std::mutex());
    frame_ = CoordinateFrame::Laser;

    min_bounding_box_ = Eigen::Vector3f(INFINITY, INFINITY, INFINITY);
    max_bounding_box_ = Eigen::Vector3f(-INFINITY, -INFINITY, -INFINITY);
    deleting_ = false;
    visible_ = true;
    translation_speed_ = 1;
}

PointCloud::~PointCloud() {
    qDebug() << "Cloud deleted";
}

inline bool PointCloud::point_matches_label(int idx, std::vector<uint16_t> & labels) {
        for(uint16_t l : labels) {
            if(labels_[idx] == l)
                return true;
        }
    return false;
}

bool PointCloud::save_ptx(const char* filename, std::vector<uint16_t> labels){
    //pc_mutex->lock();
    emit progress(0);
    setlocale(LC_NUMERIC,"C");

    FILE * pfile;
    pfile = fopen(filename , "w");
    if (pfile == NULL){
        qDebug("Error opening file");
        return false;
    }

    fprintf(pfile, "%d\n%d\n", scan_width_, scan_height_);

    fprintf(pfile, "%f %f %f\n", this->sensor_origin_[0],
            this->sensor_origin_[1], this->sensor_origin_[2]);

    // File is column major
    Eigen::Matrix3f rmat(this->sensor_orientation_);

    fprintf(pfile, "%f %f %f\n", rmat(0, 0), rmat(1, 0), rmat(2, 0));
    fprintf(pfile, "%f %f %f\n", rmat(0, 1), rmat(1, 1), rmat(2, 1));
    fprintf(pfile, "%f %f %f\n", rmat(0, 2), rmat(1, 2), rmat(2, 2));

    fprintf(pfile, "%f %f %f %f\n", rmat(0, 0), rmat(1, 0), rmat(2, 0), 0.0f);
    fprintf(pfile, "%f %f %f %f\n", rmat(0, 1), rmat(1, 1), rmat(2, 1), 0.0f);
    fprintf(pfile, "%f %f %f %f\n", rmat(0, 2), rmat(1, 2), rmat(2, 2), 0.0f);
    fprintf(pfile, "%f %f %f %f\n", this->sensor_origin_[0],
            this->sensor_origin_[1], this->sensor_origin_[2], 1.0f);

    // Write points
    uint point_count = scan_height_*scan_width_;
    int update_interval = point_count/100;
    if(update_interval == 0)
        update_interval = 1;

    uint cloud_idx = 0;
    uint next_grid_idx = this->cloud_to_grid_map_[cloud_idx];

    for(uint grid_idx = 0; grid_idx < point_count; grid_idx++) {
        if(grid_idx % update_interval == 0)
            emit progress(100*grid_idx/static_cast<float>(point_count));

        if(next_grid_idx != grid_idx) {
            fprintf(pfile, "%f %f %f %f\n", 0.0f, 0.0f, 0.0f, 0.0f);
            continue;
        }

        next_grid_idx = this->cloud_to_grid_map_[++cloud_idx];

        if(labels.size() != 0 && !point_matches_label(cloud_idx, labels)) {
            fprintf(pfile, "%f %f %f %f\n", 0.0f, 0.0f, 0.0f, 0.0f);
            continue;
        }

        fprintf(pfile, "%f %f %f %f\n", this->points[cloud_idx].x,
                this->points[cloud_idx].y, this->points[cloud_idx].z,
                this->points[cloud_idx].intensity);

    }

    fclose(pfile);

    emit progress(100);
    //pc_mutex->unlock();
    return true;

}

bool PointCloud::load_ptx(const char* filename, int decimation_factor) {
    pc_mutex->lock();
    emit progress(0);
    assert(decimation_factor%2 == 0 || decimation_factor == 1);

    // Avoid using a comma delimiter
    setlocale(LC_NUMERIC,"C");

    qDebug() << "Opening: " << filename;

    FILE * pfile;
    pfile = fopen (filename , "r");
    if (pfile == NULL){
        qDebug("Error opening file");
        return false;
    }

    // Contains nans
    this->is_dense = false;

    // Matrix dimentions
    int file_width, file_height;
    fscanf(pfile, "%d %d", &file_width, &file_height);

    qDebug() << "w x h" << file_width << file_height;
    qDebug() << "total" << file_width * file_height;
    assert(file_width);
    assert(file_width);

    // Subsample
    this->scan_width_ =  file_width/decimation_factor;
    this->scan_height_ = file_height/decimation_factor;

    // Camera offset
    std::fscanf(pfile, "%f %f %f", &this->sensor_origin_[0],
            &this->sensor_origin_[1], &this->sensor_origin_[2]);

    this->sensor_origin_[3] = 0.0f;
    sensor_origin_future_ = sensor_origin_;

    Eigen::Matrix3f orientation_mat;
    for(int row = 0; row < 3; row++ )
        for(int col = 0; col < 3; col++ )
            fscanf(pfile, "%f", &orientation_mat(row,col));

    this->sensor_orientation_ = Eigen::Quaternionf(orientation_mat.transpose());

    // Discard newline and redunadant matrix
    char buff[1024];
    for(int i = 0; i < 5; i++ ){
        fgets(buff, 1024, pfile);
        //qDebug("Skipped Mat '%s'", buff);
    }

    unsigned int sampled_idx = 0;
    int file_sample_idx = 0;
    int row = 0, col = 0;

    int line_count = file_width*file_height;
    int update_interval = line_count/100;

    if(update_interval == 0)
        update_interval = 1;

    pcl::PointXYZI point;
    float & x = point.x;
    float & y = point.y;
    float & z = point.z;
    float & intensity = point.intensity;

    // Tokenize the first line
    char * ch;
    long file_pos = ftell(pfile); // Save restart pos
    fgets(buff, 1024, pfile);
    int tokens = 0;

    ch = strtok(buff, " ");
    while (ch != NULL) {
        ++tokens;

        if(ch == NULL)
            break;
        ch = strtok(NULL, " ");
    }

    // Check if rgb channels are present
    bool has_rgb = false;
    if(tokens == 7)
        has_rgb = true;

    // Move reset the reading pos in file
    fseek(pfile, file_pos, SEEK_SET);

    while(file_sample_idx < line_count){
        if(file_sample_idx % update_interval == 0) {
            double prog = 100.0*file_sample_idx/static_cast<double>(line_count);
            //qDebug("%f%% done.", prog);
            emit progress(prog);
        }

        row = file_sample_idx / file_width;
        col = file_sample_idx % file_width;
        file_sample_idx++;

        // Only process every decimation_factor-ith row and column
        if((row+1)%decimation_factor != 0 || (col+1)%decimation_factor != 0){
            //for(int i = 0; i < tokens; i++ )
            //    fscanf(pfile, "%f", &ign);
            qDebug() << "Skiped point";
            fgets(buff, 1024, pfile);
            continue;
        }

        if(ferror (pfile) || feof(pfile)){
            qDebug() << "File read fail at line " << file_sample_idx << "with sample idx:" << sampled_idx;
            return false;
        }

        //file_pos = ftell(pfile);
        int filled = fscanf(pfile, "%f %f %f %f\n", &x, &y, &z, &intensity);

        if(filled != 4) {
            qDebug() << "File parse fail at line " << file_sample_idx << "with sample idx" << sampled_idx;
            qDebug() << "File pos: " << file_pos;
            if(ferror(pfile))
                qDebug() << "File read fail.";
            else {
                //fgets(buff, 1024, pfile);
                //qDebug("Could not parse '%s'", buff);
                continue;
            }

            points.clear();
            width = 0;
            height = 0;
            scan_width_ = 0;
            scan_height_ = 0;
            return false;
        }

        // skip the rest of the line
        if(has_rgb) {
            fgets(buff, 1024, pfile);
        }

        sampled_idx++;

        // Skip points that are invalid
        if((x == 0) && (y == 0) && (z == 0)) {
            continue;
        }

        // Set bounding box
        if(x < min_bounding_box_.x())
            min_bounding_box_.x() = x;
        if(y < min_bounding_box_.y())
            min_bounding_box_.y() = y;
        if(z < min_bounding_box_.z())
            min_bounding_box_.z() = z;

        if(x > max_bounding_box_.x())
            min_bounding_box_.x() = x;
        if(y > max_bounding_box_.y())
            min_bounding_box_.y() = y;
        if(z > max_bounding_box_.z())
            min_bounding_box_.z() = z;

        point.data_c[2] = cloud_to_grid_map_.size();
        point.data_c[3] = sampled_idx-1; // Store the index in the cloud, new hack
        this->points.push_back(point);
        this->cloud_to_grid_map_.push_back(sampled_idx-1); // Undo the increment
    }

    this->width = this->points.size();
    this->height = 1;
    this->is_dense = true;
    labels_.resize(this->points.size(), 0);
    flags_.resize(this->points.size(), PointFlags(0));

    qDebug()  << "valid points" << this->points.size();

    emit progress(100);
    pc_mutex->unlock();

    // Start loading octree
    fut_octree_ = std::async(std::launch::async, [&, this](){
        double resolution = 0.02;
        qDebug() << "Start octree";
        Octree::Ptr octree = Octree::Ptr(new Octree(resolution));
        assert(this->size());
        pcl::PointCloud<pcl::PointXYZI>::ConstPtr cptr(this, boost::serialization::null_deleter());
        octree->setInputCloud(cptr);
        octree->defineBoundingBox();
        octree->addPointsFromInputCloud();
        qDebug() << "Done with octree";
        return octree;
    });

    fclose(pfile);

    filepath_ = QString(filename);

    return this;
}

void PointCloud::translate(const Eigen::Vector3f& pos) {
    sensor_origin_future_ += Eigen::Vector4f(pos.x(), pos.y(), pos.z(), 0) * translation_speed_;

    if(translation_speed_ < 10)
        translation_speed_ *= 1.1f; // If succesive tranlations are performed, speed things up

    //sensor_origin_ += Eigen::Vector4f(pos.x(), pos.y(), pos.z(), 0);
    emit transformed();
}

void PointCloud::rotate2D(float x, float y) {
    AngleAxis<float> rotX(-x, Vector3f::UnitZ());
    AngleAxis<float> rotY(-y, Vector3f::UnitY());
    sensor_orientation_ = (rotX * rotY) * sensor_orientation_;
    emit transformed();
}

Eigen::Affine3f PointCloud::modelview() {
    if(isMoving())
        sensor_origin_ = sensor_origin_ * 0.5 + sensor_origin_future_ * 0.5;
    else
        translation_speed_ = 1.0f;

    Translation3f tr(sensor_origin_.x(), sensor_origin_.y(), sensor_origin_.z());

    AngleAxis<float> rotation(-M_PI/2, Vector3f::UnitX());
//    if(frame_ == CoordinateFrame::Camera){
//        rotation = AngleAxis<float>(-M_PI/2, Vector3f(0, 0, 1));
//    }

    return  rotation * sensor_orientation_ * tr * Affine3f::Identity();
}

const Octree::Ptr PointCloud::octree() {
    if(octree_.get() != nullptr)
        return octree_;

    octree_ = fut_octree_.get();
    return octree_;
}

boost::shared_ptr<const std::vector<int> > PointCloud::gridToCloudMap() const {
    if(grid_to_cloud_map_ != nullptr)
        return grid_to_cloud_map_;

    // New map
    grid_to_cloud_map_ = boost::make_shared<std::vector<int>>(
                scan_width_*scan_height_, -1);

    for(uint i = 0; i < size(); i++) {
        int grid_idx = cloud_to_grid_map_[i];
        (*grid_to_cloud_map_)[grid_idx] = i;
    }

    return grid_to_cloud_map_;
}

const std::vector<int> & PointCloud::cloudToGridMap() const {
    return cloud_to_grid_map_;
}

bool PointCloud::isVisible() {
    return visible_;
}

void PointCloud::toggleVisible() {
    visible_ = !visible_;
}

const CoordinateFrame PointCloud::coordinateFrame() {
    return frame_;
}

int PointCloud::scan_width() const {
    return scan_width_;
}

int PointCloud::scan_height() const{
    return scan_height_;
}

void PointCloud::resetOrientation() {
    qDebug() << "reset";
    sensor_orientation_ = sensor_orientation_.setIdentity();
    emit transformed();
}

std::vector<boost::shared_ptr<std::vector<int> > > PointCloud::getSelections() {
    std::vector<boost::shared_ptr<std::vector<int> > > selections(8);

    for(boost::shared_ptr<std::vector<int> > & sel : selections)
        sel.reset(new std::vector<int>());

    auto is_selected = [this] (int idx, uint8_t mask) {
        return bool(mask & uint8_t(flags_[idx]));
    };

    for(size_t idx = 0; idx < flags_.size(); idx++) {
        for(uint8_t maskid = 0; maskid < 8; maskid++) {
            uint8_t mask = 1 << maskid;
            if(is_selected(idx, mask)) {
                selections[maskid]->push_back(idx);
            }
        }

    }

    return selections;
}


/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/

#include "nav_msgs/Odometry.h"
#include "std_msgs/Empty.h"
#include "visualization_msgs/Marker.h"
#include <Eigen/Eigen>
#include <ros/ros.h>

#include <swarmtal_msgs/drone_onboard_command.h>
#include <traj_generator/polynomial_traj.hpp>

using namespace std;

ros::Publisher state_pub, pos_cmd_pub, traj_pub;

nav_msgs::Odometry odom;
bool have_odom;

void displayPathWithColor(vector<Eigen::Vector3d> path, double resolution, Eigen::Vector4d color,
                          int id) {
  visualization_msgs::Marker mk;
  mk.header.frame_id = "world";
  mk.header.stamp = ros::Time::now();
  mk.type = visualization_msgs::Marker::SPHERE_LIST;
  mk.action = visualization_msgs::Marker::DELETE;
  mk.id = id;

  traj_pub.publish(mk);

  mk.action = visualization_msgs::Marker::ADD;
  mk.pose.orientation.x = 0.0;
  mk.pose.orientation.y = 0.0;
  mk.pose.orientation.z = 0.0;
  mk.pose.orientation.w = 1.0;

  mk.color.r = color(0);
  mk.color.g = color(1);
  mk.color.b = color(2);
  mk.color.a = color(3);

  mk.scale.x = resolution;
  mk.scale.y = resolution;
  mk.scale.z = resolution;

  geometry_msgs::Point pt;
  for (int i = 0; i < int(path.size()); i++) {
    pt.x = path[i](0);
    pt.y = path[i](1);
    pt.z = path[i](2);
    mk.points.push_back(pt);
  }
  traj_pub.publish(mk);
  ros::Duration(0.001).sleep();
}

void drawState(Eigen::Vector3d pos, Eigen::Vector3d vec, int id, Eigen::Vector4d color) {
  visualization_msgs::Marker mk_state;
  mk_state.header.frame_id = "world";
  mk_state.header.stamp = ros::Time::now();
  mk_state.id = id;
  mk_state.type = visualization_msgs::Marker::ARROW;
  mk_state.action = visualization_msgs::Marker::ADD;
  mk_state.pose.orientation.w = 1.0;
  mk_state.scale.x = 0.1;
  mk_state.scale.y = 0.2;
  mk_state.scale.z = 0.3;
  geometry_msgs::Point pt;
  pt.x = pos(0);
  pt.y = pos(1);
  pt.z = pos(2);
  mk_state.points.push_back(pt);
  pt.x = pos(0) + vec(0);
  pt.y = pos(1) + vec(1);
  pt.z = pos(2) + vec(2);
  mk_state.points.push_back(pt);
  mk_state.color.r = color(0);
  mk_state.color.g = color(1);
  mk_state.color.b = color(2);
  mk_state.color.a = color(3);
  state_pub.publish(mk_state);
}

void odomCallbck(const nav_msgs::Odometry& msg) {
  if (msg.child_frame_id == "X" || msg.child_frame_id == "O") return;

  odom = msg;
  have_odom = true;
}

int main(int argc, char** argv) {
  /* ---------- initialize ---------- */
  ros::init(argc, argv, "traj_generator");
  ros::NodeHandle node;

  ros::Subscriber odom_sub = node.subscribe("/uwb_vicon_odom", 50, odomCallbck);

  traj_pub = node.advertise<visualization_msgs::Marker>("/traj_generator/traj_vis", 10);
  state_pub = node.advertise<visualization_msgs::Marker>("/traj_generator/cmd_vis", 10);

  // pos_cmd_pub =
  // node.advertise<quadrotor_msgs::PositionCommand>("/traj_generator/position_cmd",
  // 50);

  pos_cmd_pub =
      node.advertise<swarmtal_msgs::drone_onboard_command>("/drone_commander/onboard_command", 10);

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

  /* ---------- wait for odom ready ---------- */
  have_odom = false;
  while (!have_odom && ros::ok()) {
    cout << "no odomeetry." << endl;
    ros::Duration(0.5).sleep();
    ros::spinOnce();
  }

  /* ---------- generate trajectory using close-form minimum jerk ---------- */
  Eigen::MatrixXd pos(9, 3);
  // pos.row(0) =
  //     Eigen::Vector3d(odom.pose.pose.position.x, odom.pose.pose.position.y,
  //     odom.pose.pose.position.z);

  pos.row(0) =
      Eigen::Vector3d(odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z);
  // pos.row(0) = Eigen::Vector3d(-2, 0, 1);
  pos.row(1) = Eigen::Vector3d(-0.5, 0.5, 1);
  pos.row(2) = Eigen::Vector3d(0, 0, 1);
  pos.row(3) = Eigen::Vector3d(0.5, -0.5, 1);
  pos.row(4) = Eigen::Vector3d(1, 0, 1);
  pos.row(5) = Eigen::Vector3d(0.5, 0.5, 1);
  pos.row(6) = Eigen::Vector3d(0, 0, 1);
  pos.row(7) = Eigen::Vector3d(-0.5, -0.5, 1);
  pos.row(8) = Eigen::Vector3d(-1, 0, 1);

  Eigen::VectorXd time(8);
  time(0) = 2.0;
  time(1) = 1.5;
  time(2) = 1.5;
  time(3) = 1.5;
  time(4) = 1.5;
  time(5) = 1.5;
  time(6) = 1.5;
  time(7) = 2.0;

  Eigen::MatrixXd poly = generateTraj(pos, time);
  cout << "poly:\n" << poly << endl;

  cout << "pos:\n" << pos << endl;

  cout << "pos 0 1 2: " << pos(0) << ", " << pos(1) << ", " << pos(2) << endl;

  /* ---------- use polynomials ---------- */
  PolynomialTraj poly_traj;
  for (int i = 0; i < poly.rows(); ++i) {
    vector<double> cx(6), cy(6), cz(6);
    for (int j = 0; j < 6; ++j) {
      cx[j] = poly(i, j), cy[j] = poly(i, j + 6), cz[j] = poly(i, j + 12);
    }
    reverse(cx.begin(), cx.end());
    reverse(cy.begin(), cy.end());
    reverse(cz.begin(), cz.end());
    double ts = time(i);
    poly_traj.addSegment(cx, cy, cz, ts);
  }
  poly_traj.init();
  vector<Eigen::Vector3d> traj_vis = poly_traj.getTraj();

  displayPathWithColor(traj_vis, 0.05, Eigen::Vector4d(1, 0, 0, 1), 1);

  /* ---------- publish command ---------- */
  ros::Time start_time = ros::Time::now();
  ros::Time time_now;

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

  swarmtal_msgs::drone_onboard_command cmd;
  cmd.command_type = swarmtal_msgs::drone_onboard_command::CTRL_POS_COMMAND;
  cmd.param1 = 0;
  cmd.param2 = 0;
  cmd.param3 = 0;
  cmd.param4 = 666666;
  cmd.param5 = 0;
  cmd.param6 = 0;
  cmd.param7 = 0;
  cmd.param8 = 0;
  cmd.param9 = 0;
  cmd.param10 = 0;

  while (ros::ok()) {
    time_now = ros::Time::now();
    double tn = (time_now - start_time).toSec();
    Eigen::Vector3d pt = poly_traj.evaluate(tn);
    Eigen::Vector3d vel = poly_traj.evaluateVel(tn);
    Eigen::Vector3d acc = poly_traj.evaluateAcc(tn);

    cmd.param1 = int(pt(0) * 10000);
    cmd.param2 = int(pt(1) * 10000);
    cmd.param3 = int(pt(2) * 10000);

    cmd.param5 = int(vel(0) * 10000);
    cmd.param6 = int(vel(1) * 10000);

    cmd.param7 = 0;
    cmd.param8 = int(acc(0) * 10000);
    cmd.param9 = int(acc(1) * 10000);

    pos_cmd_pub.publish(cmd);

    drawState(pt, vel, 0, Eigen::Vector4d(0, 1, 0, 1));
    drawState(pt, acc, 1, Eigen::Vector4d(0, 0, 1, 1));
    ros::Duration(0.01).sleep();
  }

  ros::spin();
  return 0;
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [modf]
#include <boost/simd/ieee.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft = bs::pack <float, 4>;

int main()
{
  pack_ft px = {1.0f, 2.0f, -1.0f, 0.5f};
  pack_ft pe, pf;
  std::tie(pe, pf) = bs::modf(pf);
  std::cout
    << "---- simd" << '\n'
    << "<- px =                              " << px << '\n'
    << "-> std::tie(pe, pf) = bs::modf(px) = " << '\n'
    << "-> pe =                              " << pe << '\n'
    << "-> pf =                              " << pf << '\n';

  float x = 2.0f;
  float e, f;
  std::tie(e, f) = bs::modf(x);

  std::cout
    << "---- scalar"  << '\n'
    << "<- xf =                              " << x << '\n'
    << "-> std::tie(e, f) = bs::modf(x) =    " << '\n'
    << "-> e =                               " << e << '\n'
    << "-> f =                               " << f << '\n';
  return 0;
}
//! [modf]


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014-2015, CNRS-LAAS and AIST
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of CNRS-LAAS and AIST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Florent Lamiraux */

#define BOOST_TEST_MODULE "FCL_GEOMETRIC_SHAPES"
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
#include <boost/test/included/unit_test.hpp>

#include <cmath>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/shape/geometric_shapes.h>

BOOST_AUTO_TEST_CASE(distance_capsule_box)
{
  typedef boost::shared_ptr <fcl::CollisionGeometry> CollisionGeometryPtr_t;
  // Capsule of radius 2 and of height 4
  CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.));
  // Box of size 1 by 2 by 4
  CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.));

  // Enable computation of nearest points
  fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
  fcl::DistanceResult distanceResult;

  // Rotate capsule around y axis by pi/2 and move it behind box
  fcl::Transform3f tf1 (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0),
			fcl::Vec3f (-10., 0.8, 1.5));
  fcl::Transform3f tf2;
  fcl::CollisionObject capsule (capsuleGeometry, tf1);
  fcl::CollisionObject box (boxGeometry, tf2);

  // test distance
  distanceResult.clear ();
  fcl::distance (&capsule, &box, distanceRequest, distanceResult);
  fcl::Vec3f o1 = distanceResult.nearest_points [0];
  fcl::Vec3f o2 = distanceResult.nearest_points [1];

  BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-2);
  BOOST_CHECK_CLOSE (o1 [0], -6, 1e-2);
  BOOST_CHECK_CLOSE (o1 [1], 0.8, 1e-1);
  BOOST_CHECK_CLOSE (o1 [2], 1.5, 1e-2);
  BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-2);
  BOOST_CHECK_CLOSE (o2 [1], 0.8, 1e-1);
  BOOST_CHECK_CLOSE (o2 [2], 1.5, 1e-2);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    JacobianFactor.cpp
 * @author  Richard Roberts
 * @author  Christian Potthast
 * @author  Frank Dellaert
 * @date    Dec 8, 2010
 */

#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/Scatter.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/cholesky.h>

#include <boost/assign/list_of.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/array.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/adaptor/indirected.hpp>
#include <boost/range/adaptor/map.hpp>

#include <cmath>
#include <sstream>
#include <stdexcept>

using namespace std;
using namespace boost::assign;

namespace gtsam {

/* ************************************************************************* */
JacobianFactor::JacobianFactor() :
    Ab_(cref_list_of<1>(1), 0) {
  getb().setZero();
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactor& gf) {
  // Copy the matrix data depending on what type of factor we're copying from
  if (const JacobianFactor* asJacobian = dynamic_cast<const JacobianFactor*>(&gf))
    *this = JacobianFactor(*asJacobian);
  else if (const HessianFactor* asHessian = dynamic_cast<const HessianFactor*>(&gf))
    *this = JacobianFactor(*asHessian);
  else
    throw std::invalid_argument(
        "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(const Vector& b_in) :
    Ab_(cref_list_of<1>(1), b_in.size()) {
  getb() = b_in;
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(Key i1, const Matrix& A1, const Vector& b,
    const SharedDiagonal& model) {
  fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model);
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
    const Matrix& A2, const Vector& b, const SharedDiagonal& model) {
  fillTerms(cref_list_of<2>(make_pair(i1, A1))(make_pair(i2, A2)), b, model);
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
    const Matrix& A2, Key i3, const Matrix& A3, const Vector& b,
    const SharedDiagonal& model) {
  fillTerms(
      cref_list_of<3>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)),
      b, model);
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(const HessianFactor& factor)
    : Base(factor),
      Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) {
  // Copy Hessian into our matrix and then do in-place Cholesky
  Ab_.full() = factor.info().selfadjointView();

  // Do Cholesky to get a Jacobian
  size_t maxrank;
  bool success;
  boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix());

  // Check that Cholesky succeeded OR it managed to factor the full Hessian.
  // THe latter case occurs with non-positive definite matrices arising from QP.
  if (success || maxrank == factor.rows() - 1) {
    // Zero out lower triangle
    Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
        Matrix::Zero(maxrank, Ab_.matrix().cols());
    // FIXME: replace with triangular system
    Ab_.rowEnd() = maxrank;
    model_ = SharedDiagonal();  // is equivalent to Unit::Create(maxrank)
  } else {
    // indefinite system
    throw IndeterminantLinearSystemException(factor.keys().front());
  }
}

/* ************************************************************************* */
// Helper functions for combine constructor
namespace {
boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
    const FastVector<JacobianFactor::shared_ptr>& factors,
    const FastVector<VariableSlots::const_iterator>& variableSlots) {
  gttic(countDims);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
  FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
#else
  FastVector<DenseIndex> varDims(variableSlots.size(),
      numeric_limits<DenseIndex>::max());
#endif
  DenseIndex m = 0;
  DenseIndex n = 0;
  for (size_t jointVarpos = 0; jointVarpos < variableSlots.size();
      ++jointVarpos) {
    const VariableSlots::const_iterator& slots = variableSlots[jointVarpos];

    assert(slots->second.size() == factors.size());

    bool foundVariable = false;
    for (size_t sourceFactorI = 0; sourceFactorI < slots->second.size();
        ++sourceFactorI) {
      const size_t sourceVarpos = slots->second[sourceFactorI];
      if (sourceVarpos != VariableSlots::Empty) {
        const JacobianFactor& sourceFactor = *factors[sourceFactorI];
        if (sourceFactor.cols() > 1) {
          foundVariable = true;
          DenseIndex vardim = sourceFactor.getDim(
              sourceFactor.begin() + sourceVarpos);

#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
          if(varDims[jointVarpos] == numeric_limits<DenseIndex>::max()) {
            varDims[jointVarpos] = vardim;
            n += vardim;
          } else {
            if(!(varDims[jointVarpos] == vardim)) {
              std::stringstream ss;
              ss << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) <<
              " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos];
              throw std::runtime_error(ss.str());
            }
          }
#else

          varDims[jointVarpos] = vardim;
          n += vardim;
          break;
#endif
        }
      }
    }

    if (!foundVariable)
      throw std::invalid_argument(
          "Unable to determine dimensionality for all variables");
  }

  for(const JacobianFactor::shared_ptr& factor: factors) {
    m += factor->rows();
  }

#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
  for(DenseIndex d: varDims) {
    assert(d != numeric_limits<DenseIndex>::max());
  }
#endif

  return boost::make_tuple(varDims, m, n);
}

/* ************************************************************************* */
FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
    const GaussianFactorGraph& factors) {
  gttic(Convert_to_Jacobians);
  FastVector<JacobianFactor::shared_ptr> jacobians;
  jacobians.reserve(factors.size());
  for(const GaussianFactor::shared_ptr& factor: factors) {
    if (factor) {
      if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<
          JacobianFactor>(factor))
        jacobians.push_back(jf);
      else
        jacobians.push_back(boost::make_shared<JacobianFactor>(*factor));
    }
  }
  return jacobians;
}
}

/* ************************************************************************* */
void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph,
    const FastVector<VariableSlots::const_iterator>& orderedSlots) {

  // Cast or convert to Jacobians
  FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(
      graph);

  // Count dimensions
  FastVector<DenseIndex> varDims;
  DenseIndex m, n;
  boost::tie(varDims, m, n) = _countDims(jacobians, orderedSlots);

  // Allocate matrix and copy keys in order
  gttic(allocate);
  Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix
  Base::keys_.resize(orderedSlots.size());
  boost::range::copy(
      // Get variable keys
      orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys,
      Base::keys_.begin());
  gttoc(allocate);

  // Loop over slots in combined factor and copy blocks from source factors
  gttic(copy_blocks);
  size_t combinedSlot = 0;
  for(VariableSlots::const_iterator varslot: orderedSlots) {
    JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot));
    // Loop over source jacobians
    DenseIndex nextRow = 0;
    for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
      // Slot in source factor
      const size_t sourceSlot = varslot->second[factorI];
      const DenseIndex sourceRows = jacobians[factorI]->rows();
      if (sourceRows > 0) {
        JacobianFactor::ABlock::RowsBlockXpr destBlock(
            destSlot.middleRows(nextRow, sourceRows));
        // Copy if exists in source factor, otherwise set zero
        if (sourceSlot != VariableSlots::Empty)
          destBlock = jacobians[factorI]->getA(
              jacobians[factorI]->begin() + sourceSlot);
        else
          destBlock.setZero();
        nextRow += sourceRows;
      }
    }
    ++combinedSlot;
  }
  gttoc(copy_blocks);

  // Copy the RHS vectors and sigmas
  gttic(copy_vectors);
  bool anyConstrained = false;
  boost::optional<Vector> sigmas;
  // Loop over source jacobians
  DenseIndex nextRow = 0;
  for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
    const DenseIndex sourceRows = jacobians[factorI]->rows();
    if (sourceRows > 0) {
      this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb();
      if (jacobians[factorI]->get_model()) {
        // If the factor has a noise model and we haven't yet allocated sigmas, allocate it.
        if (!sigmas)
          sigmas = Vector::Constant(m, 1.0);
        sigmas->segment(nextRow, sourceRows) =
            jacobians[factorI]->get_model()->sigmas();
        if (jacobians[factorI]->isConstrained())
          anyConstrained = true;
      }
      nextRow += sourceRows;
    }
  }
  gttoc(copy_vectors);

  if (sigmas)
    this->setModel(anyConstrained, *sigmas);
}

/* ************************************************************************* */
// Order variable slots - we maintain the vector of ordered slots, as well as keep a list
// 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
// be added after all of the ordered variables.
FastVector<VariableSlots::const_iterator> orderedSlotsHelper(
    const Ordering& ordering,
    const VariableSlots& variableSlots) {
  gttic(Order_slots);

  FastVector<VariableSlots::const_iterator> orderedSlots;
  orderedSlots.reserve(variableSlots.size());

  // If an ordering is provided, arrange the slots first that ordering
  FastList<VariableSlots::const_iterator> unorderedSlots;
  size_t nOrderingSlotsUsed = 0;
  orderedSlots.resize(ordering.size());
  FastMap<Key, size_t> inverseOrdering = ordering.invert();
  for (VariableSlots::const_iterator item = variableSlots.begin();
      item != variableSlots.end(); ++item) {
    FastMap<Key, size_t>::const_iterator orderingPosition =
        inverseOrdering.find(item->first);
    if (orderingPosition == inverseOrdering.end()) {
      unorderedSlots.push_back(item);
    } else {
      orderedSlots[orderingPosition->second] = item;
      ++nOrderingSlotsUsed;
    }
  }
  if (nOrderingSlotsUsed != ordering.size())
    throw std::invalid_argument(
        "The ordering provided to the JacobianFactor combine constructor\n"
            "contained extra variables that did not appear in the factors to combine.");
  // Add the remaining slots
  for(VariableSlots::const_iterator item: unorderedSlots) {
    orderedSlots.push_back(item);
  }

  gttoc(Order_slots);

  return orderedSlots;
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph) {
  gttic(JacobianFactor_combine_constructor);

  // Compute VariableSlots if one was not provided
  // Binds reference, does not copy VariableSlots
  const VariableSlots & variableSlots = VariableSlots(graph);

  gttic(Order_slots);
  // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
  // 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
  // be added after all of the ordered variables.
  FastVector<VariableSlots::const_iterator> orderedSlots;
  orderedSlots.reserve(variableSlots.size());

  // If no ordering is provided, arrange the slots as they were, which will be sorted
  // numerically since VariableSlots uses a map sorting on Key.
  for (VariableSlots::const_iterator item = variableSlots.begin();
      item != variableSlots.end(); ++item)
    orderedSlots.push_back(item);
  gttoc(Order_slots);

  JacobianFactorHelper(graph, orderedSlots);
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
    const VariableSlots& p_variableSlots) {
  gttic(JacobianFactor_combine_constructor);

  // Binds reference, does not copy VariableSlots
  const VariableSlots & variableSlots = p_variableSlots;

  gttic(Order_slots);
  // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
  // 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
  // be added after all of the ordered variables.
  FastVector<VariableSlots::const_iterator> orderedSlots;
  orderedSlots.reserve(variableSlots.size());

  // If no ordering is provided, arrange the slots as they were, which will be sorted
  // numerically since VariableSlots uses a map sorting on Key.
  for (VariableSlots::const_iterator item = variableSlots.begin();
      item != variableSlots.end(); ++item)
    orderedSlots.push_back(item);
  gttoc(Order_slots);

  JacobianFactorHelper(graph, orderedSlots);
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
    const Ordering& ordering) {
  gttic(JacobianFactor_combine_constructor);

  // Compute VariableSlots if one was not provided
  // Binds reference, does not copy VariableSlots
  const VariableSlots & variableSlots = VariableSlots(graph);

  // Order variable slots
  FastVector<VariableSlots::const_iterator> orderedSlots =
    orderedSlotsHelper(ordering, variableSlots);

  JacobianFactorHelper(graph, orderedSlots);
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
    const Ordering& ordering,
    const VariableSlots& p_variableSlots) {
  gttic(JacobianFactor_combine_constructor);

  // Order variable slots
  FastVector<VariableSlots::const_iterator> orderedSlots =
    orderedSlotsHelper(ordering, p_variableSlots);

  JacobianFactorHelper(graph, orderedSlots);
}

/* ************************************************************************* */
void JacobianFactor::print(const string& s,
    const KeyFormatter& formatter) const {
  static const Eigen::IOFormat matlab(
      Eigen::StreamPrecision, // precision
      0, // flags
      " ", // coeffSeparator
      ";\n", // rowSeparator
      "\t",  // rowPrefix
      "", // rowSuffix
      "[\n", // matPrefix
      "\n  ]" // matSuffix
      );
  if (!s.empty())
    cout << s << "\n";
  for (const_iterator key = begin(); key != end(); ++key) {
    cout << boost::format("  A[%1%] = ") % formatter(*key);
    cout << getA(key).format(matlab) << endl;
  }
  cout << formatMatrixIndented("  b = ", getb(), true) << "\n";
  if (model_)
    model_->print("  Noise model: ");
  else
    cout << "  No noise model" << endl;
}

/* ************************************************************************* */
// Check if two linear factors are equal
bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const {
  static const bool verbose = false;
  if (!dynamic_cast<const JacobianFactor*>(&f_)) {
    if (verbose)
      cout << "JacobianFactor::equals: Incorrect type" << endl;
    return false;
  } else {
    const JacobianFactor& f(static_cast<const JacobianFactor&>(f_));

    // Check keys
    if (keys() != f.keys()) {
      if (verbose)
        cout << "JacobianFactor::equals: keys do not match" << endl;
      return false;
    }

    // Check noise model
    if ((model_ && !f.model_) || (!model_ && f.model_)) {
      if (verbose)
        cout << "JacobianFactor::equals: noise model mismatch" << endl;
      return false;
    }
    if (model_ && f.model_ && !model_->equals(*f.model_, tol)) {
      if (verbose)
        cout << "JacobianFactor::equals: noise modesl are not equal" << endl;
      return false;
    }

    // Check matrix sizes
    if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) {
      if (verbose)
        cout << "JacobianFactor::equals: augmented size mismatch" << endl;
      return false;
    }

    // Check matrix contents
    constABlock Ab1(Ab_.range(0, Ab_.nBlocks()));
    constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks()));
    for (size_t row = 0; row < (size_t) Ab1.rows(); ++row)
      if (!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol)
          && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) {
        if (verbose)
          cout << "JacobianFactor::equals: matrix mismatch at row " << row << endl;
        return false;
      }

    return true;
  }
}

/* ************************************************************************* */
Vector JacobianFactor::unweighted_error(const VectorValues& c) const {
  Vector e = -getb();
  for (size_t pos = 0; pos < size(); ++pos)
    e += Ab_(pos) * c[keys_[pos]];
  return e;
}

/* ************************************************************************* */
Vector JacobianFactor::error_vector(const VectorValues& c) const {
  Vector e = unweighted_error(c);
  if (model_) model_->whitenInPlace(e);
  return e;
}

/* ************************************************************************* */
double JacobianFactor::error(const VectorValues& c) const {
  Vector e = unweighted_error(c);
  // Use the noise model distance function to get the correct error if available.
  if (model_) return 0.5 * model_->squaredMahalanobisDistance(e);
  return 0.5 * e.dot(e);
}

/* ************************************************************************* */
Matrix JacobianFactor::augmentedInformation() const {
  if (model_) {
    Matrix AbWhitened = Ab_.full();
    model_->WhitenInPlace(AbWhitened);
    return AbWhitened.transpose() * AbWhitened;
  } else {
    return Ab_.full().transpose() * Ab_.full();
  }
}

/* ************************************************************************* */
Matrix JacobianFactor::information() const {
  if (model_) {
    Matrix AWhitened = this->getA();
    model_->WhitenInPlace(AWhitened);
    return AWhitened.transpose() * AWhitened;
  } else {
    return this->getA().transpose() * this->getA();
  }
}

/* ************************************************************************* */
void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
  for (size_t pos = 0; pos < size(); ++pos) {
    Key j = keys_[pos];
    size_t nj = Ab_(pos).cols();
    auto result = d.emplace(j, nj);

    Vector& dj = result.first->second;

    for (size_t k = 0; k < nj; ++k) {
      Eigen::Ref<const Vector> column_k = Ab_(pos).col(k);
      if (model_) {
        Vector column_k_copy = column_k;
        model_->whitenInPlace(column_k_copy);
        if(!result.second)
          dj(k) += dot(column_k_copy, column_k_copy);
        else
          dj(k) = dot(column_k_copy, column_k_copy);
      } else {
        if (!result.second)
          dj(k) += dot(column_k, column_k);
        else
          dj(k) = dot(column_k, column_k);
      }
    }
  }
}

/* ************************************************************************* */
// Raw memory access version should be called in Regular Factors only currently
void JacobianFactor::hessianDiagonal(double* d) const {
  throw std::runtime_error("JacobianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
}

/* ************************************************************************* */
map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
  map<Key, Matrix> blocks;
  for (size_t pos = 0; pos < size(); ++pos) {
    Key j = keys_[pos];
    Matrix Aj = Ab_(pos);
    if (model_)
      Aj = model_->Whiten(Aj);
    blocks.emplace(j, Aj.transpose() * Aj);
  }
  return blocks;
}

/* ************************************************************************* */
void JacobianFactor::updateHessian(const KeyVector& infoKeys,
                                   SymmetricBlockMatrix* info) const {
  gttic(updateHessian_JacobianFactor);

  if (rows() == 0) return;

  // Whiten the factor if it has a noise model
  const SharedDiagonal& model = get_model();
  if (model && !model->isUnit()) {
    if (model->isConstrained())
      throw invalid_argument(
          "JacobianFactor::updateHessian: cannot update information with "
          "constrained noise model");
    JacobianFactor whitenedFactor = whiten();
    whitenedFactor.updateHessian(infoKeys, info);
  } else {
    // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below
    DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1;

    // Apply updates to the upper triangle
    // Loop over blocks of A, including RHS with j==n
    vector<DenseIndex> slots(n+1);
    for (DenseIndex j = 0; j <= n; ++j) {
      Eigen::Block<const Matrix> Ab_j = Ab_(j);
      const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
      slots[j] = J;
      // Fill off-diagonal blocks with Ai'*Aj
      for (DenseIndex i = 0; i < j; ++i) {
        const DenseIndex I = slots[i];  // because i<j, slots[i] is valid.
        info->updateOffDiagonalBlock(I, J, Ab_(i).transpose() * Ab_j);
      }
      // Fill diagonal block with Aj'*Aj
      info->diagonalBlock(J).rankUpdate(Ab_j.transpose());
    }
  }
}

/* ************************************************************************* */
Vector JacobianFactor::operator*(const VectorValues& x) const {
  Vector Ax(Ab_.rows());
  Ax.setZero();
  if (empty())
    return Ax;

  // Just iterate over all A matrices and multiply in correct config part
  for (size_t pos = 0; pos < size(); ++pos) {
    // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html
    Ax.noalias() += Ab_(pos) * x[keys_[pos]];
  }

  if (model_) model_->whitenInPlace(Ax);
  return Ax;
}

/* ************************************************************************* */
void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
                                          VectorValues& x) const {
  Vector E(e.size());
  E.noalias() = alpha * e;
  if (model_) model_->whitenInPlace(E);
  // Just iterate over all A matrices and insert Ai^e into VectorValues
  for (size_t pos = 0; pos < size(); ++pos) {
    const Key j = keys_[pos];
    // To avoid another malloc if key exists, we explicitly check
    auto it = x.find(j);
    if (it != x.end()) {
      // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html
      it->second.noalias() += Ab_(pos).transpose() * E;
    } else {
      x.emplace(j, Ab_(pos).transpose() * E);
    }
  }
}

/* ************************************************************************* */
void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
    VectorValues& y) const {
  Vector Ax = (*this) * x;
  transposeMultiplyAdd(alpha, Ax, y);
}

/* ************************************************************************* */
/** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
 * Note: this is not assuming a fixed dimension for the variables,
 * but requires the vector accumulatedDims to tell the dimension of
 * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
 * then accumulatedDims is [0 3 9 11 13]
 * NOTE: size of accumulatedDims is size of keys + 1!!
 * TODO Frank asks: why is this here if not regular ????
 */
void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y,
    const std::vector<size_t>& accumulatedDims) const {

  /// Use Eigen magic to access raw memory
  typedef Eigen::Map<Vector> VectorMap;
  typedef Eigen::Map<const Vector> ConstVectorMap;

  if (empty())
    return;
  Vector Ax = Vector::Zero(Ab_.rows());

  /// Just iterate over all A matrices and multiply in correct config part (looping over keys)
  /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
  /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
  for (size_t pos = 0; pos < size(); ++pos) {
    size_t offset = accumulatedDims[keys_[pos]];
    size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
    Ax += Ab_(pos) * ConstVectorMap(x + offset, dim);
  }
  /// Deal with noise properly, need to Double* whiten as we are dividing by variance
  if (model_) {
    model_->whitenInPlace(Ax);
    model_->whitenInPlace(Ax);
  }

  /// multiply with alpha
  Ax *= alpha;

  /// Again iterate over all A matrices and insert Ai^T into y
  for (size_t pos = 0; pos < size(); ++pos) {
    size_t offset = accumulatedDims[keys_[pos]];
    size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
    VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax;
  }
}

/* ************************************************************************* */
VectorValues JacobianFactor::gradientAtZero() const {
  VectorValues g;
  Vector b = getb();
  // Gradient is really -A'*b / sigma^2
  // transposeMultiplyAdd will divide by sigma once, so we need one more
  if (model_) model_->whitenInPlace(b);
  this->transposeMultiplyAdd(-1.0, b, g); // g -= A'*b/sigma^2
  return g;
}

/* ************************************************************************* */
// Raw memory access version should be called in Regular Factors only currently
void JacobianFactor::gradientAtZero(double* d) const {
  throw std::runtime_error("JacobianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
}

/* ************************************************************************* */
Vector JacobianFactor::gradient(Key key, const VectorValues& x) const {
  // TODO: optimize it for JacobianFactor without converting to a HessianFactor
  HessianFactor hessian(*this);
  return hessian.gradient(key, x);
}

/* ************************************************************************* */
pair<Matrix, Vector> JacobianFactor::jacobian() const {
  pair<Matrix, Vector> result = jacobianUnweighted();
  // divide in sigma so error is indeed 0.5*|Ax-b|
  if (model_)
    model_->WhitenSystem(result.first, result.second);
  return result;
}

/* ************************************************************************* */
pair<Matrix, Vector> JacobianFactor::jacobianUnweighted() const {
  Matrix A(Ab_.range(0, size()));
  Vector b(getb());
  return make_pair(A, b);
}

/* ************************************************************************* */
Matrix JacobianFactor::augmentedJacobian() const {
  Matrix Ab = augmentedJacobianUnweighted();
  if (model_)
    model_->WhitenInPlace(Ab);
  return Ab;
}

/* ************************************************************************* */
Matrix JacobianFactor::augmentedJacobianUnweighted() const {
  return Ab_.range(0, Ab_.nBlocks());
}

/* ************************************************************************* */
JacobianFactor JacobianFactor::whiten() const {
  JacobianFactor result(*this);
  if (model_) {
    result.model_->WhitenInPlace(result.Ab_.full());
    result.model_ = SharedDiagonal();
  }
  return result;
}

/* ************************************************************************* */
GaussianFactor::shared_ptr JacobianFactor::negate() const {
  HessianFactor hessian(*this);
  return hessian.negate();
}

/* ************************************************************************* */
std::pair<GaussianConditional::shared_ptr,
    JacobianFactor::shared_ptr> JacobianFactor::eliminate(
    const Ordering& keys) {
  GaussianFactorGraph graph;
  graph.add(*this);
  return EliminateQR(graph, keys);
}

/* ************************************************************************* */
void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) {
  if ((size_t) sigmas.size() != this->rows())
    throw InvalidNoiseModel(this->rows(), sigmas.size());
  if (anyConstrained)
    model_ = noiseModel::Constrained::MixedSigmas(sigmas);
  else
    model_ = noiseModel::Diagonal::Sigmas(sigmas);
}

/* ************************************************************************* */
std::pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr> EliminateQR(
    const GaussianFactorGraph& factors, const Ordering& keys) {
  gttic(EliminateQR);
  // Combine and sort variable blocks in elimination order
  JacobianFactor::shared_ptr jointFactor;
  try {
    jointFactor = boost::make_shared<JacobianFactor>(factors, keys);
  } catch (std::invalid_argument&) {
    throw InvalidDenseElimination(
        "EliminateQR was called with a request to eliminate variables that are not\n"
        "involved in the provided factors.");
  }

  // Do dense elimination
  // The following QR variants eliminate to fully triangular or trapezoidal
  SharedDiagonal noiseModel;
  VerticalBlockMatrix& Ab = jointFactor->Ab_;
  if (jointFactor->model_) {
    // The noiseModel QR can, in the case of constraints, yield a "staggered" QR where
    // some rows have more leading zeros than in an upper triangular matrix.
    // In either case, QR will put zeros below the "diagonal".
    jointFactor->model_ = jointFactor->model_->QR(Ab.matrix());
  } else {
    // The inplace variant will have no valid rows anymore below m==n
    // and only entries above the diagonal are valid.
    inplace_QR(Ab.matrix());
    // We zero below the diagonal to agree with the result from noieModel QR
    Ab.matrix().triangularView<Eigen::StrictlyLower>().setZero();
    size_t m = Ab.rows(), n = Ab.cols() - 1;
    size_t maxRank = min(m, n);
    jointFactor->model_ = noiseModel::Unit::Create(maxRank);
  }

  // Split elimination result into conditional and remaining factor
  GaussianConditional::shared_ptr conditional =  //
      jointFactor->splitConditional(keys.size());

  return make_pair(conditional, jointFactor);
}

/* ************************************************************************* */
GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFrontals) {
  gttic(JacobianFactor_splitConditional);

  if (!model_) {
    throw std::invalid_argument(
        "JacobianFactor::splitConditional cannot be  given a nullptr noise model");
  }

  if (nrFrontals > size()) {
    throw std::invalid_argument(
        "JacobianFactor::splitConditional was requested to split off more variables than exist.");
  }

  // Convert nr of keys to number of scalar columns
  DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols();

  // Check that the noise model has at least this dimension
  // If this is *not* the case, we do not have enough information on the frontal variables.
  if ((DenseIndex)model_->dim() < frontalDim)
    throw IndeterminantLinearSystemException(this->keys().front());

  // Restrict the matrix to be in the first nrFrontals variables and create the conditional
  const DenseIndex originalRowEnd = Ab_.rowEnd();
  Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
  SharedDiagonal conditionalNoiseModel;
  conditionalNoiseModel =
      noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
  GaussianConditional::shared_ptr conditional =
      boost::make_shared<GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);

  const DenseIndex maxRemainingRows =
      std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim;
  const DenseIndex remainingRows = std::min(model_->sigmas().size() - frontalDim, maxRemainingRows);
  Ab_.rowStart() += frontalDim;
  Ab_.rowEnd() = Ab_.rowStart() + remainingRows;
  Ab_.firstBlock() += nrFrontals;

  // Take lower-right block of Ab to get the new factor
  keys_.erase(begin(), begin() + nrFrontals);
  // Set sigmas with the right model
  if (model_->isConstrained())
    model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(remainingRows));
  else
    model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(remainingRows));
  assert(model_->dim() == (size_t)Ab_.rows());

  return conditional;
}

}  // namespace gtsam


// TnzCore includes
#include "tmeshimage.h"
#include "tgl.h"
#include "tundo.h"

// TnzExt includes
#include "ext/plasticdeformerstorage.h"

// tcg includes
#include "tcg/tcg_macros.h"
#include "tcg/tcg_point_ops.h"

#include <unordered_set>
#include <unordered_map>

using namespace tcg::bgl;

#include <boost/graph/breadth_first_search.hpp>

// STD includes
#include <stack>

#include "plastictool.h"

using namespace PlasticToolLocals;

//****************************************************************************************
//    Local namespace  stuff
//****************************************************************************************

namespace {

typedef PlasticTool::MeshIndex MeshIndex;
typedef TTextureMesh::vertex_type vertex_type;
typedef TTextureMesh::edge_type edge_type;
typedef TTextureMesh::face_type face_type;

//------------------------------------------------------------------------

bool borderEdge(const TTextureMesh &mesh, int e) {
  return (mesh.edge(e).facesCount() < 2);
}

bool borderVertex(const TTextureMesh &mesh, int v) {
  const TTextureVertex &vx = mesh.vertex(v);

  tcg::vertex_traits<TTextureVertex>::edges_const_iterator et,
      eEnd(vx.edgesEnd());
  for (et = vx.edgesBegin(); et != eEnd; ++et) {
    if (borderEdge(mesh, *et)) return true;
  }

  return false;
}

//============================================================================

bool testSwapEdge(const TTextureMesh &mesh, int e) {
  return (mesh.edge(e).facesCount() == 2);
}

//------------------------------------------------------------------------

bool testCollapseEdge(const TTextureMesh &mesh, int e) {
  struct Locals {
    const TTextureMesh &m_mesh;
    int m_e;
    const TTextureMesh::edge_type &m_ed;

    bool testTrianglesCount() {
      // There must be at least one remanining triangle
      return (m_mesh.facesCount() > m_ed.facesCount());
    }

    bool testBoundary() {
      // Must not join two non-adjacent boundary vertices
      return (!borderVertex(m_mesh, m_ed.vertex(0)) ||
              !borderVertex(m_mesh, m_ed.vertex(1)) || borderEdge(m_mesh, m_e));
    }

    bool testAdjacency() {
      // See TriMesh<>::collapseEdge()

      // Retrieve allowed adjacent vertices
      int f, fCount = m_ed.facesCount();
      int allowedV[6], *avt, *avEnd = allowedV + 3 * fCount;

      for (f = 0, avt = allowedV; f != fCount; ++f, avt += 3)
        m_mesh.faceVertices(m_ed.face(f), avt[0], avt[1], avt[2]);

      // Test adjacent vertices
      int v0 = m_ed.vertex(0), v1 = m_ed.vertex(1);

      const vertex_type &vx0 = m_mesh.vertex(v0);

      tcg::vertex_traits<vertex_type>::edges_const_iterator et,
          eEnd = vx0.edgesEnd();
      for (et = vx0.edgesBegin(); et != eEnd; ++et) {
        int otherV = m_mesh.edge(*et).otherVertex(v0);

        if (m_mesh.edgeInciding(v1, otherV) >= 0) {
          // Adjacent vertex - must be found in the allowed list
          if (std::find(allowedV, avEnd, otherV) == avEnd) return false;
        }
      }

      return true;
    }
  } locals = {mesh, e, mesh.edge(e)};

  return (locals.testTrianglesCount() && locals.testBoundary() &&
          locals.testAdjacency());
}

}  // namespace

//****************************************************************************************
//    PlasticToolLocals  stuff
//****************************************************************************************

namespace PlasticToolLocals {

struct Closer {
  const TTextureMesh &m_mesh;
  TPointD m_pos;

  double dist2(const TTextureMesh::vertex_type &a) {
    return tcg::point_ops::dist2<TPointD>(a.P(), m_pos);
  }

  double dist2(const TTextureMesh::edge_type &a) {
    const TTextureMesh::vertex_type &avx0 = m_mesh.vertex(a.vertex(0)),
                                    &avx1 = m_mesh.vertex(a.vertex(1));

    return sq(tcg::point_ops::segDist<TPointD>(avx0.P(), avx1.P(), m_pos));
  }

  bool operator()(const TTextureMesh::vertex_type &a,
                  const TTextureMesh::vertex_type &b) {
    return (dist2(a) < dist2(b));
  }

  bool operator()(const TTextureMesh::edge_type &a,
                  const TTextureMesh::edge_type &b) {
    return (dist2(a) < dist2(b));
  }
};

//==============================================================================

static std::pair<double, int> closestVertex(const TTextureMesh &mesh,
                                            const TPointD &pos) {
  Closer closer = {mesh, pos};
  int vIdx      = int(
      std::min_element(mesh.vertices().begin(), mesh.vertices().end(), closer)
          .index());

  return std::make_pair(closer.dist2(mesh.vertex(vIdx)), vIdx);
}

//------------------------------------------------------------------------

static std::pair<double, int> closestEdge(const TTextureMesh &mesh,
                                          const TPointD &pos) {
  Closer closer = {mesh, pos};
  int eIdx =
      int(std::min_element(mesh.edges().begin(), mesh.edges().end(), closer)
              .index());

  return std::make_pair(closer.dist2(mesh.edge(eIdx)), eIdx);
}

//------------------------------------------------------------------------

std::pair<double, MeshIndex> closestVertex(const TMeshImage &mi,
                                           const TPointD &pos) {
  std::pair<double, MeshIndex> closest((std::numeric_limits<double>::max)(),
                                       MeshIndex());

  const TMeshImage::meshes_container &meshes = mi.meshes();

  TMeshImage::meshes_container::const_iterator mt, mEnd = meshes.end();
  for (mt = meshes.begin(); mt != mEnd; ++mt) {
    const std::pair<double, int> &candidateIdx = closestVertex(**mt, pos);

    std::pair<double, MeshIndex> candidate(
        candidateIdx.first,
        MeshIndex(mt - meshes.begin(), candidateIdx.second));

    if (candidate < closest) closest = candidate;
  }

  return closest;
}

//------------------------------------------------------------------------

std::pair<double, MeshIndex> closestEdge(const TMeshImage &mi,
                                         const TPointD &pos) {
  std::pair<double, MeshIndex> closest((std::numeric_limits<double>::max)(),
                                       MeshIndex());

  const TMeshImage::meshes_container &meshes = mi.meshes();

  TMeshImage::meshes_container::const_iterator mt, mEnd = meshes.end();
  for (mt = meshes.begin(); mt != mEnd; ++mt) {
    const std::pair<double, int> &candidateIdx = closestEdge(**mt, pos);

    std::pair<double, MeshIndex> candidate(
        candidateIdx.first,
        MeshIndex(mt - meshes.begin(), candidateIdx.second));

    if (candidate < closest) closest = candidate;
  }

  return closest;
}

}  // namespace

//****************************************************************************************
//    Cut Mesh  operation
//****************************************************************************************

namespace {

struct EdgeCut {
  int m_vIdx;  //!< Vertex index to cut from.
  int m_eIdx;  //!< Edge index to cut.

  EdgeCut(int vIdx, int eIdx) : m_vIdx(vIdx), m_eIdx(eIdx) {}
};

struct VertexOccurrence {
  int m_count;               //!< Number of times a vertex occurs.
  int m_adjacentEdgeIdx[2];  //!< Edge indexes of which a vertex is endpoint.
};

//============================================================================

bool buildEdgeCuts(const TMeshImage &mi,
                   const PlasticTool::MeshSelection &edgesSelection,
                   int &meshIdx, std::vector<EdgeCut> &edgeCuts) {
  typedef PlasticTool::MeshSelection::objects_container edges_container;
  typedef PlasticTool::MeshIndex MeshIndex;
  typedef std::unordered_map<int, VertexOccurrence> VertexOccurrencesMap;

  struct locals {
    static bool differentMesh(const MeshIndex &a, const MeshIndex &b) {
      return (a.m_meshIdx != b.m_meshIdx);
    }

    static int testSingleMesh(const edges_container &edges) {
      assert(!edges.empty());
      return (std::find_if(edges.begin(), edges.end(),
                           [&edges](const MeshIndex &x) {
                             return differentMesh(x, edges.front());
                           }) == edges.end())
                 ? edges.front().m_meshIdx
                 : -1;
    }

    static bool testNoBoundaryEdge(const TTextureMesh &mesh,
                                   const edges_container &edges) {
      edges_container::const_iterator et, eEnd = edges.end();
      for (et = edges.begin(); et != eEnd; ++et)
        if (::borderEdge(mesh, et->m_idx)) return false;

      return true;
    }

    static bool buildVertexOccurrences(
        const TTextureMesh &mesh, const edges_container &edges,
        VertexOccurrencesMap &vertexOccurrences) {
      // Calculate vertex occurrences as edge endpoints
      edges_container::const_iterator et, eEnd = edges.end();
      for (et = edges.begin(); et != eEnd; ++et) {
        const edge_type &ed = mesh.edge(et->m_idx);
        int v0 = ed.vertex(0), v1 = ed.vertex(1);

        VertexOccurrence &vo0 = vertexOccurrences[v0],
                         &vo1 = vertexOccurrences[v1];

        if (vo0.m_count > 1 || vo1.m_count > 1) return false;

        vo0.m_adjacentEdgeIdx[vo0.m_count++] =
            vo1.m_adjacentEdgeIdx[vo1.m_count++] = et->m_idx;
      }

      return true;
    }

    static bool buildEdgeCuts(const TTextureMesh &mesh,
                              const edges_container &edges,
                              std::vector<EdgeCut> &edgeCuts) {
      VertexOccurrencesMap vertexOccurrences;
      if (!buildVertexOccurrences(mesh, edges, vertexOccurrences)) return false;

      // Build endpoints (exactly 2)
      int endPoints[2];
      int epCount = 0;

      VertexOccurrencesMap::iterator ot, oEnd = vertexOccurrences.end();
      for (ot = vertexOccurrences.begin(); ot != oEnd; ++ot) {
        if (ot->second.m_count == 1) {
          if (epCount > 1) return false;

          endPoints[epCount++] = ot->first;
        }
      }

      if (epCount != 2) return false;

      // Pick the first endpoint on the boundary, if any (otherwise, just pick
      // one)
      int *ept, *epEnd = endPoints + 2;

      ept = std::find_if(endPoints, epEnd,
                         [&mesh](int v) { return borderVertex(mesh, v); });
      if (ept == epEnd) {
        // There is no boundary endpoint
        if (edges.size() < 2)  // We should not cut the mesh on a
          return false;        // single edge - no vertex to duplicate!

        ept = endPoints;
      }

      // Build the edge cuts list, expanding the edges selection from
      // the chosen endpoint

      edgeCuts.push_back(EdgeCut(  // Build the first EdgeCut separately
          *ept, vertexOccurrences[*ept].m_adjacentEdgeIdx[0]));

      int e, eCount = int(edges.size());  // Build the remaining ones
      for (e = 1; e != eCount; ++e) {
        const EdgeCut &lastCut = edgeCuts.back();

        int vIdx = mesh.edge(lastCut.m_eIdx).otherVertex(lastCut.m_vIdx);

        const int(&adjEdges)[2] = vertexOccurrences[vIdx].m_adjacentEdgeIdx;
        int eIdx = (adjEdges[0] == lastCut.m_eIdx) ? adjEdges[1] : adjEdges[0];

        edgeCuts.push_back(EdgeCut(vIdx, eIdx));
      }

      return true;
    }
  };

  const edges_container &edges = edgesSelection.objects();

  // Trivial early bailouts
  if (edges.empty()) return false;

  // Selected edges must lie on the same mesh
  meshIdx = locals::testSingleMesh(edges);
  if (meshIdx < 0) return false;

  const TTextureMesh &mesh = *mi.meshes()[meshIdx];

  // No selected edge must be on the boundary
  return (locals::testNoBoundaryEdge(mesh, edges) &&
          locals::buildEdgeCuts(mesh, edges, edgeCuts));
}

//------------------------------------------------------------------------

inline bool testCutMesh(const TMeshImage &mi,
                        const PlasticTool::MeshSelection &edgesSelection) {
  std::vector<EdgeCut> edgeCuts;
  int meshIdx;

  return buildEdgeCuts(mi, edgesSelection, meshIdx, edgeCuts);
}

//------------------------------------------------------------------------

void slitMesh(TTextureMesh &mesh,
              int e)  //! Opens a slit along the specified edge index.
{
  TTextureMesh::edge_type &ed = mesh.edge(e);

  assert(ed.facesCount() == 2);

  // Duplicate the edge and pass one face to the duplicate
  TTextureMesh::edge_type edDup(ed.vertex(0), ed.vertex(1));

  int f = ed.face(1);

  edDup.addFace(f);
  ed.eraseFace(ed.facesBegin() + 1);

  int eDup = mesh.addEdge(edDup);

  // Alter the face to host the duplicate
  TTextureMesh::face_type &fc = mesh.face(f);

  (fc.edge(0) == e)
      ? fc.setEdge(0, eDup)
      : (fc.edge(1) == e) ? fc.setEdge(1, eDup) : fc.setEdge(2, eDup);
}

//------------------------------------------------------------------------

/*!
  \brief    Duplicates a mesh edge-vertex pair (the 'cut') and separates their
            connections to adjacent mesh primitives.

  \remark   The starting vertex is supposed to be on the mesh boundary.
  \remark   Edges with a single neighbouring face can be duplicated, too.
*/
void cutEdge(TTextureMesh &mesh, const EdgeCut &edgeCut) {
  struct locals {
    static void transferEdge(TTextureMesh &mesh, int e, int vFrom, int vTo) {
      edge_type &ed = mesh.edge(e);

      vertex_type &vxFrom = mesh.vertex(vFrom), &vxTo = mesh.vertex(vTo);

      (ed.vertex(0) == vFrom) ? ed.setVertex(0, vTo) : ed.setVertex(1, vTo);

      vxTo.addEdge(e);
      vxFrom.eraseEdge(
          std::find(vxFrom.edges().begin(), vxFrom.edges().end(), e));
    }

    static void transferFace(TTextureMesh &mesh, int eFrom, int eTo) {
      edge_type &edFrom = mesh.edge(eFrom), &edTo = mesh.edge(eTo);

      int f = mesh.edge(eFrom).face(1);
      {
        face_type &fc = mesh.face(f);

        (fc.edge(0) == eFrom)
            ? fc.setEdge(0, eTo)
            : (fc.edge(1) == eFrom) ? fc.setEdge(1, eTo) : fc.setEdge(2, eTo);

        edTo.addFace(f);
        edFrom.eraseFace(edFrom.facesBegin() + 1);
      }
    }

  };  // locals

  int vOrig = edgeCut.m_vIdx, eOrig = edgeCut.m_eIdx;

  // Create a new vertex at the same position of the original
  int vDup = mesh.addVertex(vertex_type(mesh.vertex(vOrig).P()));

  int e = eOrig;
  if (mesh.edge(e).facesCount() == 2) {
    // Duplicate the cut edge
    e = mesh.addEdge(edge_type(vDup, mesh.edge(eOrig).otherVertex(vOrig)));

    // Transfer one face from the original to the duplicate
    locals::transferFace(mesh, eOrig, e);
  } else {
    // Transfer the original edge to the duplicate vertex
    locals::transferEdge(mesh, eOrig, vOrig, vDup);
  }

  // Edges adjacent to the original vertex that are also adjacent
  // to the transferred face above must be transferred too
  int f = mesh.edge(e).face(0);

  while (f >= 0) {
    // Retrieve the next edge to transfer
    int otherE = mesh.otherFaceEdge(f, mesh.edge(e).otherVertex(vDup));

    // NOTE: Not "mesh.edgeInciding(vOrig, mesh.otherFaceVertex(f, e))" in the
    // calculation
    //       of otherE. This is required since by transferring each edge at a
    //       time,
    //       we're 'breaking' faces up - f is adjacent to both vOrig AND vDup!
    //
    //       The chosen calculation, instead, just asks for the one edge which
    //       does
    //       not have a specific vertex in common to the 2 other edges in the
    //       face.

    locals::transferEdge(mesh, otherE, vOrig, vDup);

    // Update e and f
    e = otherE;
    f = mesh.edge(otherE).otherFace(f);
  }
}

//------------------------------------------------------------------------
}
namespace locals_ {      // Need to use a named namespace due to
                         // a known gcc 4.2 bug with compiler-generated
struct VertexesRecorder  // copy constructors.
{
  std::unordered_set<int> &m_examinedVertexes;

public:
  typedef boost::on_examine_vertex event_filter;

public:
  VertexesRecorder(std::unordered_set<int> &examinedVertexes)
      : m_examinedVertexes(examinedVertexes) {}

  void operator()(int v, const TTextureMesh &) { m_examinedVertexes.insert(v); }
};
}
namespace {  //

void splitUnconnectedMesh(TMeshImage &mi, int meshIdx) {
  struct locals {
    static void buildConnectedComponent(const TTextureMesh &mesh,
                                        std::unordered_set<int> &vertexes) {
      // Prepare BFS algorithm
      std::unique_ptr<UCHAR[]> colorMapP(new UCHAR[mesh.vertices().nodesCount()]());

      locals_::VertexesRecorder vertexesRecorder(vertexes);
      std::stack<int> verticesQueue;

      // Launch it
      boost::breadth_first_visit(
          mesh, int(mesh.vertices().begin().index()), verticesQueue,
          boost::make_bfs_visitor(vertexesRecorder), colorMapP.get());
    }
  };  // locals

  // Retrieve the list of vertexes in the first connected component
  TTextureMesh &origMesh = *mi.meshes()[meshIdx];

  std::unordered_set<int> firstComponent;
  locals::buildConnectedComponent(origMesh, firstComponent);

  if (firstComponent.size() == origMesh.verticesCount()) return;

  // There are (exactly) 2 connected components. Just duplicate the mesh
  // and keep/delete found vertexes.
  TTextureMeshP dupMeshPtr(new TTextureMesh(origMesh));
  TTextureMesh &dupMesh = *dupMeshPtr;

  TTextureMesh::vertices_container &vertices = origMesh.vertices();

  TTextureMesh::vertices_container::iterator vt, vEnd = vertices.end();
  for (vt = vertices.begin(); vt != vEnd;) {
    int v = int(vt.index());
    ++vt;

    if (firstComponent.count(v))
      dupMesh.removeVertex(v);
    else
      origMesh.removeVertex(v);
  }

  dupMesh.squeeze();
  origMesh.squeeze();

  mi.meshes().push_back(dupMeshPtr);
}

//------------------------------------------------------------------------

void splitMesh(TMeshImage &mi, int meshIdx, int lastBoundaryVertex) {
  // Retrieve a cutting edge with a single adjacent face - cutting that
  // will just duplicate the vertex and separate the mesh in 2 connected
  // components
  TTextureMesh &mesh = *mi.meshes()[meshIdx];

  int e;
  {
    const vertex_type &lbVx = mesh.vertex(lastBoundaryVertex);

    vertex_type::edges_const_iterator et =
        std::find_if(lbVx.edgesBegin(), lbVx.edgesEnd(),
                     [&mesh](int e) { return borderEdge(mesh, e); });
    assert(et != lbVx.edgesEnd());

    e = *et;
  }

  cutEdge(mesh, EdgeCut(lastBoundaryVertex, e));

  // At this point, separate the 2 resulting connected components
  // in 2 separate meshes (if necessary)
  splitUnconnectedMesh(mi, meshIdx);
}

//------------------------------------------------------------------------

bool cutMesh(TMeshImage &mi, const PlasticTool::MeshSelection &edgesSelection) {
  struct locals {
    static int lastVertex(const TTextureMesh &mesh,
                          const std::vector<EdgeCut> &edgeCuts) {
      return mesh.edge(edgeCuts.back().m_eIdx)
          .otherVertex(edgeCuts.back().m_vIdx);
    }

    static int lastBoundaryVertex(const TTextureMesh &mesh,
                                  const std::vector<EdgeCut> &edgeCuts) {
      int v = lastVertex(mesh, edgeCuts);
      return ::borderVertex(mesh, v) ? v : -1;
    }

  };  // locals

  std::vector<EdgeCut> edgeCuts;
  int meshIdx;

  if (!::buildEdgeCuts(mi, edgesSelection, meshIdx, edgeCuts)) return false;

  TTextureMesh &mesh = *mi.meshes()[meshIdx];

  int lastBoundaryVertex = locals::lastBoundaryVertex(mesh, edgeCuts);

  // Slit the mesh on the first edge, in case the cuts do not start
  // on the mesh boundary
  std::vector<EdgeCut>::iterator ecBegin = edgeCuts.begin();

  if (!::borderVertex(mesh, ecBegin->m_vIdx)) {
    ::slitMesh(mesh, ecBegin->m_eIdx);
    ++ecBegin;
  }

  // Cut edges, in the order specified by edgeCuts
  std::for_each(ecBegin, edgeCuts.end(), [&mesh](const EdgeCut &edgeCut) {
    return cutEdge(mesh, edgeCut);
  });

  // Finally, the mesh could have been split in 2 - we need to separate
  // the pieces if needed
  if (lastBoundaryVertex >= 0) splitMesh(mi, meshIdx, lastBoundaryVertex);

  return true;
}

}  // namespace

//****************************************************************************************
//    Undo  definitions
//****************************************************************************************

namespace {

class MoveVertexUndo_Mesh final : public TUndo {
  int m_row, m_col;  //!< Xsheet coordinates

  std::vector<MeshIndex> m_vIdxs;     //!< Moved vertices
  std::vector<TPointD> m_origVxsPos;  //!< Original vertex positions
  TPointD m_posShift;                 //!< Vertex positions shift

public:
  MoveVertexUndo_Mesh(const std::vector<MeshIndex> &vIdxs,
                      const std::vector<TPointD> &origVxsPos,
                      const TPointD &posShift)
      : m_row(::row())
      , m_col(::column())
      , m_vIdxs(vIdxs)
      , m_origVxsPos(origVxsPos)
      , m_posShift(posShift) {
    assert(m_vIdxs.size() == m_origVxsPos.size());
  }

  int getSize() const override {
    return int(sizeof(*this) +
               m_vIdxs.size() * (sizeof(int) + 2 * sizeof(TPointD)));
  }

  void redo() const override {
    PlasticTool::TemporaryActivation tempActivate(m_row, m_col);

    l_plasticTool.setMeshVertexesSelection(m_vIdxs);
    l_plasticTool.moveVertex_mesh(m_origVxsPos, m_posShift);

    l_plasticTool.invalidate();
    l_plasticTool
        .notifyImageChanged();  // IMPORTANT: In particular, sets the level's
  }                             //            dirty flag, so Toonz knows it has
                                //            to be saved!
  void undo() const override {
    PlasticTool::TemporaryActivation tempActivate(m_row, m_col);

    l_plasticTool.setMeshVertexesSelection(m_vIdxs);
    l_plasticTool.moveVertex_mesh(m_origVxsPos, TPointD());

    l_plasticTool.invalidate();
    l_plasticTool.notifyImageChanged();
  }
};

//==============================================================================

class SwapEdgeUndo final : public TUndo {
  int m_row, m_col;             //!< Xsheet coordinates
  mutable MeshIndex m_edgeIdx;  //!< Edge index

public:
  SwapEdgeUndo(const MeshIndex &edgeIdx)
      : m_row(::row()), m_col(::column()), m_edgeIdx(edgeIdx) {}

  int getSize() const override { return sizeof(*this); }

  void redo() const override {
    PlasticTool::TemporaryActivation tempActivate(m_row, m_col);

    const TMeshImageP &mi = TMeshImageP(TTool::getImage(true));
    assert(mi);

    // Perform swap
    TTextureMesh &mesh = *mi->meshes()[m_edgeIdx.m_meshIdx];

    m_edgeIdx.m_idx = mesh.swapEdge(m_edgeIdx.m_idx);
    assert(m_edgeIdx.m_idx >= 0);

    // Invalidate any deformer associated with mi
    PlasticDeformerStorage::instance()->releaseMeshData(mi.getPointer());

    // Update tool selection
    l_plasticTool.setMeshEdgesSelection(m_edgeIdx);

    l_plasticTool.invalidate();
    l_plasticTool.notifyImageChanged();
  }

  void undo() const override { redo(); }  // Operation is idempotent (indices
                                          // are perfectly restored, too)
};

//==============================================================================

class TTextureMeshUndo : public TUndo {
protected:
  int m_row, m_col;  //!< Xsheet coordinates

  int m_meshIdx;  //!< Mesh index in the image at stored xsheet coords
  mutable TTextureMesh m_origMesh;  //!< Copy of the original mesh

public:
  TTextureMeshUndo(int meshIdx)
      : m_row(::row()), m_col(::column()), m_meshIdx(meshIdx) {}

  // Let's say 1MB each - storing the mesh is costly
  int getSize() const override { return 1 << 20; }

  TMeshImageP getMeshImage() const {
    const TMeshImageP &mi = TMeshImageP(TTool::getImage(true));
    assert(mi);

    return mi;
  }
};

//==============================================================================

class CollapseEdgeUndo final : public TTextureMeshUndo {
  int m_eIdx;  //!< Collapsed edge index

public:
  CollapseEdgeUndo(const MeshIndex &edgeIdx)
      : TTextureMeshUndo(edgeIdx.m_meshIdx), m_eIdx(edgeIdx.m_idx) {}

  void redo() const override {
    PlasticTool::TemporaryActivation tempActivate(m_row, m_col);

    const TMeshImageP &mi = getMeshImage();

    // Store the original mesh
    TTextureMesh &mesh = *mi->meshes()[m_meshIdx];
    m_origMesh         = mesh;

    // Collapse
    mesh.collapseEdge(m_eIdx);
    mesh.squeeze();

    // Invalidate any cached deformer associated with the modified mesh image
    PlasticDeformerStorage::instance()->releaseMeshData(mi.getPointer());

    // Refresh the tool
    l_plasticTool.clearMeshSelections();

    l_plasticTool.invalidate();
    l_plasticTool.notifyImageChanged();
  }

  void undo() const override {
    PlasticTool::TemporaryActivation tempActivate(m_row, m_col);

    const TMeshImageP &mi = getMeshImage();

    // Restore the original mesh
    TTextureMesh &mesh = *mi->meshes()[m_meshIdx];
    mesh               = m_origMesh;

    PlasticDeformerStorage::instance()->releaseMeshData(mi.getPointer());

    // Restore selection
    l_plasticTool.setMeshEdgesSelection(MeshIndex(m_meshIdx, m_eIdx));

    l_plasticTool.invalidate();
    l_plasticTool.notifyImageChanged();
  }
};

//==============================================================================

class SplitEdgeUndo final : public TTextureMeshUndo {
  int m_eIdx;  //!< Split edge index

public:
  SplitEdgeUndo(const MeshIndex &edgeIdx)
      : TTextureMeshUndo(edgeIdx.m_meshIdx), m_eIdx(edgeIdx.m_idx) {}

  void redo() const override {
    PlasticTool::TemporaryActivation tempActivate(m_row, m_col);

    const TMeshImageP &mi = getMeshImage();

    // Store the original mesh
    TTextureMesh &mesh = *mi->meshes()[m_meshIdx];
    m_origMesh         = mesh;

    // Split
    mesh.splitEdge(m_eIdx);
    // mesh.squeeze();                                                     //
    // There should be no need to squeeze

    assert(mesh.vertices().size() == mesh.vertices().nodesCount());  //
    assert(mesh.edges().size() == mesh.edges().nodesCount());        //
    assert(mesh.faces().size() == mesh.faces().nodesCount());        //

    PlasticDeformerStorage::instance()->releaseMeshData(mi.getPointer());

    l_plasticTool.clearMeshSelections();

    l_plasticTool.invalidate();
    l_plasticTool.notifyImageChanged();
  }

  void undo() const override {
    PlasticTool::TemporaryActivation tempActivate(m_row, m_col);

    const TMeshImageP &mi = getMeshImage();

    // Restore the original mesh
    TTextureMesh &mesh = *mi->meshes()[m_meshIdx];
    mesh               = m_origMesh;

    PlasticDeformerStorage::instance()->releaseMeshData(mi.getPointer());

    // Restore selection
    l_plasticTool.setMeshEdgesSelection(MeshIndex(m_meshIdx, m_eIdx));

    l_plasticTool.invalidate();
    l_plasticTool.notifyImageChanged();
  }
};

//==============================================================================

class CutEdgesUndo final : public TUndo {
  int m_row, m_col;         //!< Xsheet coordinates
  TMeshImageP m_origImage;  //!< Clone of the original image

  PlasticTool::MeshSelection m_edgesSelection;  //!< Selection to operate on

public:
  CutEdgesUndo(const PlasticTool::MeshSelection &edgesSelection)
      : m_row(::row())
      , m_col(::column())
      , m_origImage(TTool::getImage(false)->cloneImage())
      , m_edgesSelection(edgesSelection) {}

  int getSize() const override { return 1 << 20; }

  bool do_() const {
    TMeshImageP mi = TTool::getImage(true);

    if (::cutMesh(*mi, m_edgesSelection)) {
      PlasticDeformerStorage::instance()->releaseMeshData(mi.getPointer());

      l_plasticTool.clearMeshSelections();

      l_plasticTool.invalidate();
      l_plasticTool.notifyImageChanged();

      return true;
    }

    return false;
  }

  void redo() const override {
    PlasticTool::TemporaryActivation tempActivate(m_row, m_col);

    bool ret = do_();
    (void)ret;
    assert(ret);
  }

  void undo() const override {
    PlasticTool::TemporaryActivation tempActivate(m_row, m_col);

    TMeshImageP mi = TTool::getImage(true);

    // Restore the original image
    *mi = *m_origImage;

    PlasticDeformerStorage::instance()->releaseMeshData(mi.getPointer());

    // Restore selection
    l_plasticTool.setMeshEdgesSelection(m_edgesSelection);

    l_plasticTool.invalidate();
    l_plasticTool.notifyImageChanged();
  }
};

}  // namespace

//****************************************************************************************
//    PlasticTool  functions
//****************************************************************************************

void PlasticTool::storeMeshImage() {
  TMeshImageP mi = getImage(false);

  if (mi != m_mi) {
    m_mi = mi;
    clearMeshSelections();
  }
}

//------------------------------------------------------------------------

void PlasticTool::setMeshSelection(MeshSelection &target,
                                   const MeshSelection &newSel) {
  if (newSel.isEmpty()) {
    target.selectNone();
    target.makeNotCurrent();

    return;
  }

  target.setObjects(newSel.objects());

  target.notifyView();
  target.makeCurrent();
}

//------------------------------------------------------------------------

void PlasticTool::toggleMeshSelection(MeshSelection &target,
                                      const MeshSelection &addition) {
  typedef MeshSelection::objects_container objects_container;

  const objects_container &storedIdxs = target.objects();
  const objects_container &addedIdxs  = addition.objects();

  // Build new selection
  objects_container selectedIdxs;

  if (target.contains(addition)) {
    std::set_difference(storedIdxs.begin(), storedIdxs.end(), addedIdxs.begin(),
                        addedIdxs.end(), std::back_inserter(selectedIdxs));
  } else {
    std::set_union(storedIdxs.begin(), storedIdxs.end(), addedIdxs.begin(),
                   addedIdxs.end(), std::back_inserter(selectedIdxs));
  }

  setMeshSelection(target, selectedIdxs);
}

//------------------------------------------------------------------------

void PlasticTool::clearMeshSelections() {
  m_mvHigh = m_meHigh = MeshIndex();

  m_mvSel.selectNone();
  m_mvSel.makeNotCurrent();

  m_meSel.selectNone();
  m_meSel.makeNotCurrent();
}

//------------------------------------------------------------------------

void PlasticTool::setMeshVertexesSelection(const MeshSelection &vSel) {
  setMeshSelection(m_meSel, MeshSelection()), setMeshSelection(m_mvSel, vSel);
}

//------------------------------------------------------------------------

void PlasticTool::toggleMeshVertexesSelection(const MeshSelection &vSel) {
  setMeshSelection(m_meSel, MeshSelection()),
      toggleMeshSelection(m_mvSel, vSel);
}

//------------------------------------------------------------------------

void PlasticTool::setMeshEdgesSelection(const MeshSelection &eSel) {
  setMeshSelection(m_meSel, eSel), setMeshSelection(m_mvSel, MeshSelection());
}

//------------------------------------------------------------------------

void PlasticTool::toggleMeshEdgesSelection(const MeshSelection &eSel) {
  toggleMeshSelection(m_meSel, eSel),
      setMeshSelection(m_mvSel, MeshSelection());
}

//------------------------------------------------------------------------

void PlasticTool::mouseMove_mesh(const TPointD &pos, const TMouseEvent &me) {
  // Track mouse position
  m_pos = pos;  // Needs to be done now - ensures m_pos is valid

  m_mvHigh = MeshIndex();  // Reset highlighted primitives

  if (m_mi) {
    // Look for nearest primitive
    std::pair<double, MeshIndex> closestVertex = ::closestVertex(*m_mi, pos),
                                 closestEdge = ::closestEdge(*m_mi, pos);

    // Discriminate on fixed metric
    const double hDistSq = sq(getPixelSize() * MESH_HIGHLIGHT_DISTANCE);

    m_mvHigh = m_meHigh = MeshIndex();

    if (closestEdge.first < hDistSq) m_meHigh = closestEdge.second;

    if (closestVertex.first < hDistSq)
      m_mvHigh = closestVertex.second, m_meHigh = MeshIndex();
  }

  assert(!(m_mvHigh &&
           m_meHigh));  // Vertex and edge highlights are mutually exclusive

  invalidate();
}

//------------------------------------------------------------------------

void PlasticTool::leftButtonDown_mesh(const TPointD &pos,
                                      const TMouseEvent &me) {
  struct Locals {
    PlasticTool *m_this;

    void updateSelection(MeshSelection &sel, const MeshIndex &idx,
                         const TMouseEvent &me) {
      if (idx) {
        MeshSelection newSel(idx);

        if (me.isCtrlPressed())
          m_this->toggleMeshSelection(sel, newSel);
        else if (!sel.contains(newSel))
          m_this->setMeshSelection(sel, newSel);
      } else
        m_this->setMeshSelection(sel, MeshSelection());
    }
  } locals = {this};

  // Track mouse position
  m_pressedPos = m_pos = pos;

  // Update selection
  locals.updateSelection(m_mvSel, m_mvHigh, me);
  locals.updateSelection(m_meSel, m_meHigh, me);

  // Store original vertex positions
  if (!m_mvSel.isEmpty()) {
    std::vector<TPointD> v;
    for (auto const &e : m_mvSel.objects()) {
      v.push_back(m_mi->meshes()[e.m_meshIdx]->vertex(e.m_idx).P());
    }
    m_pressedVxsPos = std::move(v);
  }

  // Redraw selections
  invalidate();
}

//------------------------------------------------------------------------

void PlasticTool::leftButtonDrag_mesh(const TPointD &pos,
                                      const TMouseEvent &me) {
  // Track mouse position
  m_pos = pos;

  if (!m_mvSel.isEmpty()) {
    moveVertex_mesh(m_pressedVxsPos, pos - m_pressedPos);
    invalidate();
  }
}

//------------------------------------------------------------------------

void PlasticTool::leftButtonUp_mesh(const TPointD &pos, const TMouseEvent &me) {
  // Track mouse position
  m_pos = pos;

  if (m_dragged && !m_mvSel.isEmpty()) {
    TUndoManager::manager()->add(new MoveVertexUndo_Mesh(
        m_mvSel.objects(), m_pressedVxsPos, pos - m_pressedPos));

    invalidate();
    notifyImageChanged();  // Sets the level's dirty flag      -.-'
  }
}

//------------------------------------------------------------------------

void PlasticTool::addContextMenuActions_mesh(QMenu *menu) {
  bool ret = true;

  if (!m_meSel.isEmpty()) {
    if (m_meSel.hasSingleObject()) {
      const MeshIndex &mIdx    = m_meSel.objects().front();
      const TTextureMesh &mesh = *m_mi->meshes()[mIdx.m_meshIdx];

      if (::testSwapEdge(mesh, mIdx.m_idx)) {
        QAction *swapEdge = menu->addAction(tr("Swap Edge"));
        ret = ret && connect(swapEdge, SIGNAL(triggered()), &l_plasticTool,
                             SLOT(swapEdge_mesh_undo()));
      }

      if (::testCollapseEdge(mesh, mIdx.m_idx)) {
        QAction *collapseEdge = menu->addAction(tr("Collapse Edge"));
        ret = ret && connect(collapseEdge, SIGNAL(triggered()), &l_plasticTool,
                             SLOT(collapseEdge_mesh_undo()));
      }

      QAction *splitEdge = menu->addAction(tr("Split Edge"));
      ret = ret && connect(splitEdge, SIGNAL(triggered()), &l_plasticTool,
                           SLOT(splitEdge_mesh_undo()));
    }

    if (::testCutMesh(*m_mi, m_meSel)) {
      QAction *cutEdges = menu->addAction(tr("Cut Mesh"));
      ret = ret && connect(cutEdges, SIGNAL(triggered()), &l_plasticTool,
                           SLOT(cutEdges_mesh_undo()));
    }

    menu->addSeparator();
  }

  assert(ret);
}

//------------------------------------------------------------------------

void PlasticTool::moveVertex_mesh(const std::vector<TPointD> &origVxsPos,
                                  const TPointD &posShift) {
  if (m_mvSel.isEmpty() || !m_mi) return;

  assert(origVxsPos.size() == m_mvSel.objects().size());

  // Move selected vertices
  TMeshImageP mi = getImage(true);
  assert(m_mi == mi);

  int v, vCount = int(m_mvSel.objects().size());
  for (v = 0; v != vCount; ++v) {
    const MeshIndex &meshIndex = m_mvSel.objects()[v];

    TTextureMesh &mesh               = *mi->meshes()[meshIndex.m_meshIdx];
    mesh.vertex(meshIndex.m_idx).P() = origVxsPos[v] + posShift;
  }

  // Mesh must be recompiled
  PlasticDeformerStorage::instance()->invalidateMeshImage(
      mi.getPointer(), PlasticDeformerStorage::MESH);
}

//------------------------------------------------------------------------

void PlasticTool::swapEdge_mesh_undo() {
  if (!(m_mi && m_meSel.hasSingleObject())) return;

  // Test current edge swapability
  {
    const MeshIndex &eIdx    = m_meSel.objects().front();
    const TTextureMesh &mesh = *m_mi->meshes()[eIdx.m_meshIdx];

    if (!::testSwapEdge(mesh, eIdx.m_idx)) return;
  }

  // Perform operation
  std::unique_ptr<TUndo> undo(new SwapEdgeUndo(m_meSel.objects().front()));
  undo->redo();

  TUndoManager::manager()->add(undo.release());
}

//------------------------------------------------------------------------

void PlasticTool::collapseEdge_mesh_undo() {
  if (!(m_mi && m_meSel.hasSingleObject())) return;

  // Test collapsibility of current edge
  {
    const MeshIndex &eIdx    = m_meSel.objects().front();
    const TTextureMesh &mesh = *m_mi->meshes()[eIdx.m_meshIdx];

    if (!::testCollapseEdge(mesh, eIdx.m_idx)) return;
  }

  // Perform operation
  std::unique_ptr<TUndo> undo(new CollapseEdgeUndo(m_meSel.objects().front()));
  undo->redo();

  TUndoManager::manager()->add(undo.release());
}

//------------------------------------------------------------------------

void PlasticTool::splitEdge_mesh_undo() {
  if (!(m_mi && m_meSel.hasSingleObject())) return;

  std::unique_ptr<TUndo> undo(new SplitEdgeUndo(m_meSel.objects().front()));
  undo->redo();

  TUndoManager::manager()->add(undo.release());
}

//------------------------------------------------------------------------

void PlasticTool::cutEdges_mesh_undo() {
  if (!m_mi) return;

  std::unique_ptr<CutEdgesUndo> undo(new CutEdgesUndo(m_meSel.objects()));

  if (undo->do_()) TUndoManager::manager()->add(undo.release());
}

//------------------------------------------------------------------------

void PlasticTool::draw_mesh() {
  struct Locals {
    PlasticTool *m_this;
    double m_pixelSize;

    void drawLine(const TPointD &a, const TPointD &b) {
      glVertex2d(a.x, a.y);
      glVertex2d(b.x, b.y);
    }

    void drawVertexSelections() {
      typedef MeshSelection::objects_container objects_container;
      const objects_container &objects = m_this->m_mvSel.objects();

      glColor3ub(255, 0, 0);  // Red
      glLineWidth(1.0f);

      const double hSize = MESH_SELECTED_HANDLE_SIZE * m_pixelSize;

      objects_container::const_iterator vt, vEnd = objects.end();
      for (vt = objects.begin(); vt != vEnd; ++vt) {
        const TTextureVertex &vx =
            m_this->m_mi->meshes()[vt->m_meshIdx]->vertex(vt->m_idx);

        ::drawFullSquare(vx.P(), hSize);
      }
    }

    void drawEdgeSelections() {
      typedef MeshSelection::objects_container objects_container;
      const objects_container &objects = m_this->m_meSel.objects();

      glColor3ub(0, 0, 255);  // Blue
      glLineWidth(2.0f);

      glBegin(GL_LINES);

      objects_container::const_iterator et, eEnd = objects.end();
      for (et = objects.begin(); et != eEnd; ++et) {
        const TTextureVertex
            &vx0 =
                m_this->m_mi->meshes()[et->m_meshIdx]->edgeVertex(et->m_idx, 0),
            &vx1 =
                m_this->m_mi->meshes()[et->m_meshIdx]->edgeVertex(et->m_idx, 1);

        drawLine(vx0.P(), vx1.P());
      }

      glEnd();
    }

    void drawVertexHighlights() {
      if (m_this->m_mvHigh) {
        const MeshIndex &vHigh = m_this->m_mvHigh;

        const TTextureMesh::vertex_type &vx =
            m_this->m_mi->meshes()[vHigh.m_meshIdx]->vertex(vHigh.m_idx);

        glColor3ub(255, 0, 0);  // Red
        glLineWidth(1.0f);

        const double hSize = MESH_HIGHLIGHTED_HANDLE_SIZE * m_pixelSize;

        ::drawSquare(vx.P(), hSize);
      }
    }

    void drawEdgeHighlights() {
      if (m_this->m_meHigh) {
        const MeshIndex &eHigh = m_this->m_meHigh;

        const TTextureMesh::vertex_type
            &vx0 = m_this->m_mi->meshes()[eHigh.m_meshIdx]->edgeVertex(
                eHigh.m_idx, 0),
            &vx1 = m_this->m_mi->meshes()[eHigh.m_meshIdx]->edgeVertex(
                eHigh.m_idx, 1);

        {
          glPushAttrib(GL_LINE_BIT);

          glEnable(GL_LINE_STIPPLE);
          glLineStipple(1, 0xCCCC);

          glColor3ub(0, 0, 255);  // Blue
          glLineWidth(1.0f);

          glBegin(GL_LINES);
          drawLine(vx0.P(), vx1.P());
          glEnd();

          glPopAttrib();
        }
      }
    }

  } locals = {this, getPixelSize()};

  // The selected mesh image is already drawn by the stage

  // Draw additional overlays
  if (m_mi) {
    locals.drawVertexSelections();
    locals.drawEdgeSelections();

    locals.drawVertexHighlights();
    locals.drawEdgeHighlights();
  }
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

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

#include <boost/simd/function/definition/atan.hpp>
#include <boost/simd/arch/common/generic/function/atan.hpp>

#endif