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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_DETAIL_ACCELERATED_SHULL_HPP
#define BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_DETAIL_ACCELERATED_SHULL_HPP

#include <vector>
#include <utility>
#include <tuple>
#include <iterator>
#include <algorithm>

#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>
#include <boost/geometry/extensions/triangulation/geometries/triangulation.hpp>
#include <boost/geometry/extensions/triangulation/strategies/cartesian/in_circle_robust.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>

namespace boost { namespace geometry { namespace strategy
{

#ifndef DOXYGEN_NO_DETAIL

namespace delaunay_triangulation { namespace detail
{

template<typename Point, typename CalculationType, typename SideStrategy>
inline CalculationType comparable_circumcircle_diameter(
    const Point& p1, const Point& p2, const Point& p3)
{
    typedef typename coordinate_type<Point>::type coordinate_type;
    CalculationType comp_area = SideStrategy::template
        side_value<coordinate_type, CalculationType>(p1, p2, p3);
    return comparable_distance(p1, p2)
         * comparable_distance(p1, p3)
         * comparable_distance(p2, p3)
         / (comp_area * comp_area);
}

template
<
    typename PointOut,
    bool ClockWise,
    typename PointIn
>
inline PointOut circumcircle_center(const PointIn& p1,
                                    const PointIn& p2,
                                    const PointIn& p3)
{
    typedef typename coordinate_type<PointIn>::type coordinate_type;
    coordinate_type ax = get<0>( p1 );
    coordinate_type ay = get<1>( p1 );
    coordinate_type bx = get<0>( !ClockWise ? p2 : p3 );
    coordinate_type by = get<1>( !ClockWise ? p2 : p3 );
    coordinate_type cx = get<0>( !ClockWise ? p3 : p2 );
    coordinate_type cy = get<1>( !ClockWise ? p3 : p2 );
    coordinate_type d =
        2 * ( ax * (by - cy) + bx * (cy - ay) + cx * (ay - by));
    coordinate_type x = ( (ax * ax + ay * ay) * (by - cy)
                        + (bx * bx + by * by) * (cy - ay)
                        + (cx * cx + cy * cy) * (ay - by)) / d;
    coordinate_type y = ( (ax * ax + ay * ay) * (cx - bx )
                        + (bx * bx + by * by) * (ax - cx)
                        + (cx * cx + cy * cy) * (bx - ax)) / d;
    return boost::geometry::make<PointOut>(x, y);
}

struct accelerated_shull
{
    template
    <
        typename PointContainer,
        typename Triangulation,
        typename SideStrategy,
        typename InCircleStrategy,
        typename CalculationType = double
    >
    static inline void apply(PointContainer const & in,
                             Triangulation& out,
                             bool legalize)
    {
        typedef typename PointContainer::value_type point_type;
        typedef typename default_distance_result<point_type, point_type>::type
            distance_type;
        const bool is_cw =
            boost::geometry::point_order<typename Triangulation::face_type>::value
                == clockwise;
        typedef CalculationType ct;
        std::vector<std::tuple<point_type, ct, ct>> points;
        points.reserve(in.size());
        //Step 1 & 2 & 3
        point_type zero;
        set<0>(zero, 0);
        set<1>(zero, 0);
        std::transform(std::begin(in), std::end(in), std::back_inserter(points),
            [&points, &zero](point_type const& p) {
                return std::tuple<point_type, ct, ct>(p,
                boost::geometry::comparable_distance(p, zero),
                0);
            });
        std::sort(std::begin(points),
                  std::end(points),
                  [](std::tuple<point_type, ct, ct> const& p0,
                     std::tuple<point_type, ct, ct> const& p1)
                    { return std::get<1>(p0) < std::get<1>(p1); });
        for(std::tuple<point_type, ct, ct>& p : points)
        {
            std::get<1>(p) = boost::geometry::comparable_distance(std::get<0>(p),
                                                                  std::get<0>(points[ 0 ]));
        }
        std::sort(std::begin(points) + 1,
                  std::end(points),
                  [](std::tuple<point_type, ct, ct> const& p0,
                     std::tuple<point_type, ct, ct> const& p1)
                    { return std::get<1>(p0) < std::get<1>(p1); });
        //Step 4
        {
            distance_type min_circumdiameter =
                std::numeric_limits<distance_type>::max();
            std::size_t min_index = 2;
            for( std::size_t i = 2 ;
                std::get<1>(points[i]) < min_circumdiameter && i < points.size() ;
                ++i )
            {
                distance_type diam =
                    comparable_circumcircle_diameter<point_type, ct, SideStrategy>(
                        std::get<0>(points[ 0 ]),
                        std::get<0>(points[ 1 ]),
                        std::get<0>(points[ i ]));
                if(diam < min_circumdiameter) {
                    min_index = i;
                    min_circumdiameter = diam;
                }
            }
            std::swap(points[ 2 ], points[ min_index ]);
        }
        //Step 5
        if( (    !is_cw
            && SideStrategy::apply(std::get<0>(points[ 0 ]),
                                   std::get<0>(points[ 1 ]),
                                   std::get<0>(points[ 2 ])) < 0 )
            || ( is_cw
               && SideStrategy::apply(std::get<0>(points[ 0 ]),
                                      std::get<0>(points[ 1 ]),
                                      std::get<0>(points[ 2 ])) > 0 ))
        {
            std::swap(points[ 1 ], points[ 2 ]);
        }
        const double PI = 3.14159265358979323846;
        //Step 6

        typedef typename Triangulation::vertex_iterator vertex_iterator;
        typedef typename Triangulation::face_iterator face_iterator;
        vertex_iterator const p1 = out.add_vertex(std::get<0>(points[ 0 ]));
        vertex_iterator const p2 = out.add_vertex(std::get<0>(points[ 1 ]));
        vertex_iterator const p3 = out.add_vertex(std::get<0>(points[ 2 ]));
        face_iterator const seed_face = out.add_isolated_face(p1, p2, p3);
        point_type cen;
        set<0>(cen,
               ( get<0>(std::get<0>(points[ 0 ]))
               + get<0>(std::get<0>(points[ 1 ]))
               + get<0>(std::get<0>(points[ 2 ])) ) / 3);
        set<1>(cen,
               ( get<1>(std::get<0>(points[ 0 ]))
               + get<1>(std::get<0>(points[ 1 ]))
               + get<1>(std::get<0>(points[ 2 ])) ) / 3);

        point_type C = circumcircle_center<point_type, is_cw>(
            std::get<0>(points[ 0 ]),
            std::get<0>(points[ 1 ]),
            std::get<0>(points[ 2 ]));
        {
            std::for_each(std::begin(points), std::end(points),
                [&C, &cen, &PI](std::tuple<point_type, ct, ct>& p){
                    std::get<1>(p) = comparable_distance(std::get<0>(p), C);
                    std::get<2>(p) =
                        ! is_cw ?
                          std::atan2(
                                get<1>(std::get<0>(p)) - get<1>(cen),
                                get<0>(std::get<0>(p)) - get<0>(cen)) + PI
                          : 2 * PI - (std::atan2(
                                get<1>(std::get<0>(p)) - get<1>(cen),
                                get<0>(std::get<0>(p)) - get<0>(cen)) + PI);
                });
            std::sort(std::begin(points) + 3,
                      std::end(points),
                      [](std::tuple<point_type, ct, ct> const& p0,
                         std::tuple<point_type, ct, ct> const& p1)
                      { return std::get<1>(p0) < std::get<1>(p1); });
        }
        //Step 7 & 8
        typedef typename Triangulation::halfedge_index halfedge_index;
        typedef typename Triangulation::point_type out_point_type;
        typedef typename Triangulation::segment_type segment_type;
        {
            halfedge_index e1 = out.face_edge(seed_face);
            halfedge_index e2 = out.next(e1);
            halfedge_index e3 = out.next(e2);
            typedef std::pair<typename Triangulation::halfedge_index, ct>
                convex_hull_edge;
            typedef std::vector<convex_hull_edge> convex_hull_container;
            typedef typename convex_hull_container::iterator convex_hull_iterator;
            convex_hull_container convex_hull {
                std::make_pair(e1, ct(std::get<2>(points[1]))),
                std::make_pair(e2, ct(std::get<2>(points[2]))),
                std::make_pair(e3, ct(std::get<2>(points[0]))) };
            for (std::size_t i = 3; i < points.size(); ++i)
            {
                vertex_iterator new_vertex =
                    out.add_vertex(std::get<0>(points[ i ]));
                out_point_type const& p = out.vertex(new_vertex);
                auto is_visible = [&out, &p](convex_hull_edge const& bep)
                    {
                        const halfedge_index& be = std::get<0>(bep);
                        const segment_type s = out.face_segment(be);
                        const out_point_type p1 = s.first;
                        const out_point_type p2 = s.second;
                        auto det = SideStrategy::apply(p1,p2,p);
                        bool result = (!is_cw && det < 0) || (is_cw && det > 0);
                        return result;
                    };
                const ct& ref_angle = std::get<1>(*convex_hull.begin());
                ct angle = std::get<2>(points[ i ]) - ref_angle;
                if(angle < 0) angle += 2 * PI;
                ct opposite = angle - PI;
                if(opposite < 0) opposite += 2 * PI;

                auto pred = [&ref_angle, &PI]
                    (convex_hull_edge const& be, ct const& a)
                    {
                        ct ba = std::get<1>(be) - ref_angle;
                        if(ba < 0) ba += 2*PI;
                        return ba < a;
                    };
                convex_hull_iterator vis_edge = std::lower_bound(
                    convex_hull.begin(), convex_hull.end(),
                    angle, pred);
                if( vis_edge == convex_hull.begin() )
                    vis_edge = convex_hull.end() - 1;
                else --vis_edge;
                convex_hull_iterator invis_edge = std::lower_bound(
                    convex_hull.begin(), convex_hull.end(),
                    opposite, pred);
                if( invis_edge == convex_hull.begin() )
                    invis_edge = convex_hull.end() - 1;
                else --invis_edge;
                if(!is_visible(*vis_edge)) {
                    invis_edge = vis_edge;
                    for(auto it = convex_hull.begin();
                        it != convex_hull.end();
                        ++it) {
                        if(is_visible(*it)) {
                            vis_edge = it;
                            break;
                        }
                    }
                }
                auto vis_small = [&is_visible]
                    (convex_hull_edge const& be, int const&)
                    {
                        if(is_visible(be)) return true;
                        else return false;
                    };
                auto vis_large = [&is_visible]
                    (convex_hull_edge const& be, int const&)
                    {
                        if(is_visible(be)) return false;
                        else return true;
                    };
                convex_hull_iterator first_visible, last_visible,
                    fv2 = convex_hull.end();
                if(vis_edge < invis_edge) {
                    if(is_visible(*convex_hull.begin())) {
                        first_visible = convex_hull.begin();
                        last_visible = std::lower_bound(vis_edge, invis_edge,
                            0, vis_small);
                        fv2 = std::lower_bound(invis_edge, convex_hull.end(),
                            0, vis_large);
                    } else {
                        first_visible = std::lower_bound(convex_hull.begin(),
                            vis_edge, 0, vis_large);
                        last_visible = std::lower_bound(vis_edge,
                            convex_hull.end(), 0, vis_small);
                    }
                } else {
                    if(is_visible(*convex_hull.begin())) {
                        first_visible = convex_hull.begin();
                        last_visible = std::lower_bound(
                            convex_hull.begin()+1, invis_edge,
                            0, vis_small);
                        fv2 = std::lower_bound(invis_edge, vis_edge,
                            0, vis_large);
                    } else {
                        first_visible = std::lower_bound(invis_edge, vis_edge,
                            0, vis_large);
                        last_visible = std::lower_bound(vis_edge,
                            convex_hull.end(), 0, vis_small);
                    }
                }
                ct first_visible_angle = std::get<1>(*first_visible);
                const bool begin_visible =
                    first_visible == std::begin(convex_hull);
                face_iterator prev, fnf2;
                bool looped = false;
                ct fv2_angle;
                if(begin_visible && is_visible(convex_hull.back()))
                {
                    looped = true;
                    fv2_angle = std::get<1>(*fv2);
                    fnf2 = out.add_face_on_boundary(std::get<0>(*fv2), new_vertex);
                    prev = fnf2;
                    for(convex_hull_iterator it = fv2 + 1 ;
                        it != std::end(convex_hull) ;
                        ++it)
                    {
                        face_iterator next =
                            out.add_face_on_boundary(std::get<0>(*it), new_vertex);
                        out.connect(out.next(out.face_edge(next)),
                                    out.prev(out.face_edge(prev)));
                        prev = next;
                    }
                    convex_hull.erase(fv2, std::end(convex_hull));
                }
                const face_iterator fnf =
                    out.add_face_on_boundary(std::get<0>(*first_visible),
                                             new_vertex);
                if(looped) {
                    out.connect(out.next(out.face_edge(fnf)),
                                out.prev(out.face_edge(prev)));
                }
                prev = fnf;
                for (convex_hull_iterator it = first_visible + 1;
                     it != last_visible;
                     ++it)
                {
                    face_iterator next =
                        out.add_face_on_boundary(std::get<0>(*it), new_vertex);
                    out.connect(out.next(out.face_edge(next)),
                                out.prev(out.face_edge(prev)));
                    prev = next;
                }
                convex_hull_iterator ip =
                    convex_hull.erase(first_visible, last_visible);
                ip = convex_hull.insert(ip,
                    std::make_pair(out.prev(out.face_edge(prev)),
                                   ct(std::get<2>(points[i]))));
                if(looped)
                    convex_hull.insert(ip,
                        std::make_pair(out.next(out.face_edge(fnf2)),
                                       ct(fv2_angle)));
                else
                    convex_hull.insert(ip,
                        std::make_pair(out.next(out.face_edge(fnf)),
                                       ct(first_visible_angle)));
            }
        }
        //Step 9
        if(legalize)
        {
            typedef typename Triangulation::face_vertex_index face_vertex_index;
            std::vector<halfedge_index> L;
            for(face_iterator i = out.faces_begin(); i != out.faces_end(); ++i)
            {
                for(face_vertex_index j = 0; j<2 ; ++j)
                    if(   out.neighbour(i, j)!=Triangulation::invalid()
                          && &(*i) > &(*out.neighbour(i, j))) {
                        L.push_back(halfedge_index(i,j));
                    }
            }
            auto edge_legal = [&out](halfedge_index const& e)
                {
                    out_point_type const& p1 = out.face_vertex(e.m_f, 0);
                    out_point_type const& p2 = out.face_vertex(e.m_f, 1);
                    out_point_type const& p3 = out.face_vertex(e.m_f, 2);
                    out_point_type const& p = out.face_vertex(
                            out.neighbour(e.m_f, e.m_v),
                            out.opposite(e.m_f, e.m_v)
                        );
                    return !is_cw ?
                             InCircleStrategy::apply(p1, p2, p3, p) <= 0
                          || InCircleStrategy::apply(p1, p2, p, p3) >  0
                        :    InCircleStrategy::apply(p1, p3, p2, p) <= 0
                          || InCircleStrategy::apply(p1, p3, p, p2) >  0;
                };
            while( !L.empty() )
            {
                halfedge_index e = L.back();
                L.pop_back();
                face_iterator const f1 = e.m_f;
                face_vertex_index const v1 = e.m_v;
                face_iterator const f2 = out.neighbour(f1, v1);
                if(f2 == face_iterator())
                    continue;
                face_vertex_index const v2 = out.opposite(f1, v1);
                if( !edge_legal(e) ) {
                    L.emplace_back(f1, v1);
                    L.emplace_back(f2, v2);
                    L.emplace_back(f1, v1 == 0 ? 2 : v1 - 1);
                    L.emplace_back(f2, v2 == 0 ? 2 : v2 - 1);
                    out.flip(e);
                }
            }
        }
    }
};

}} // namespace delaunay_triangulation::detail

#endif // DOXYGEN_NO_DETAIL

}}} // namespace boost::geometry::strategy

#endif // BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_DETAIL_ACCELERATED_SHULL_HPP


// BSD 3-Clause License
// Copyright (c) 2021 The Trustees of the University of Pennsylvania. All Rights Reserved
// Authors:
// J. Diego Caporale <jdcap@seas.upenn.edu>

/* Example script running joints to their mechanical limits before switching direction. The functions to implement are
 * CalcTorques and PrepareLog. In this example we run the motor forward until its
 * position error is large enough to detect a obstacle, then switching directions
 */

#include "kodlab_mjbots_sdk/mjbots_control_loop.h"
#include "kodlab_mjbots_sdk/joint_moteus.h"
#include "ManyMotorLog.hpp"
#include "kodlab_mjbots_sdk/lcm_subscriber.h"
#include <sys/mman.h>
#include <limits>
#include <Eigen/Core>
#include <Eigen/Dense>

class ProprioJoints : public kodlab::mjbots::MjbotsControlLoop<ManyMotorLog> {
  using MjbotsControlLoop::MjbotsControlLoop;
  void CalcTorques() override {

    float omega_des = 0.3; //velocity in rads per sec
    Eigen::VectorXf omega = Eigen::VectorXf::Ones(num_motors_)*omega_des; //radians

    Eigen::VectorXf positions  = Eigen::Map<Eigen::VectorXf,Eigen::Unaligned> ( robot_->GetJointPositions().data(), num_motors_);
    Eigen::VectorXf velocities = Eigen::Map<Eigen::VectorXf,Eigen::Unaligned> (robot_->GetJointVelocities().data(), num_motors_);

    static Eigen::VectorXf  phase = positions; //radians
    static Eigen::VectorXf    dir = Eigen::VectorXf::Ones(num_motors_); //radians
    static Eigen::VectorXf max_pos = Eigen::VectorXf::Ones(num_motors_) * ( -std::numeric_limits<float>::infinity() );
    static Eigen::VectorXf min_pos = Eigen::VectorXf::Ones(num_motors_) * (  std::numeric_limits<float>::infinity() );

    phase.array() += dir.array()*(omega.array() / frequency_) ;

    // Detect collision
    float switch_threshold = 0.1;
    auto switch_bool =  (phase.array()-positions.array()).abs() < switch_threshold;
    dir = (switch_bool).select(dir,-dir);
    phase = (switch_bool).select(phase,positions);

    // Update limits
    max_pos = (phase.array() > max_pos.array()).select(phase, max_pos);
    min_pos = (phase.array() < min_pos.array()).select(phase, min_pos);

    //Calculate torques
    std::vector<float> torques(num_motors_, 0);
    Eigen::VectorXf tau = 100*(phase-positions) + 1*(omega-velocities);
    Eigen::VectorXf::Map(&torques[0], num_motors_) = tau;

    // Print limits
    std::cout<<"*********\n";
    std::cout<<max_pos.transpose()<<"\n";
    std::cout<<min_pos.transpose()<<"\n";
    std::cout<<"*********"<<std::endl;

    robot_->SetTorques(torques);
  }

  void PrepareLog() override {
    for (int servo = 0; servo < num_motors_; servo++) {
      log_data_.positions[servo]  = robot_->GetJointPositions()[servo];
      log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
      log_data_.modes[servo] = static_cast<int>(robot_->GetJointModes()[servo]);
      log_data_.torques[servo] = robot_->GetJointTorqueCmd()[servo];
    }
    for (int servo = num_motors_; servo < 13; servo++) {
      log_data_.positions[servo] = 0;
      log_data_.velocities[servo] = 0;
      log_data_.modes[servo] = 0;
      log_data_.torques[servo] = 0;
    }
  }
};

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

  //Setup joints
  std::vector<kodlab::mjbots::JointMoteus> joints;

  joints.emplace_back(108, 4, 1, -0.4674585, 1, 1);

  // Define robot options
  kodlab::mjbots::ControlLoopOptions options;
  options.log_channel_name = "motor_data";
  options.frequency = 1000;
  options.realtime_params.main_cpu = 3;
  options.realtime_params.can_cpu  = 2;

  // Create control loop
  ProprioJoints control_loop(joints, options);

  // Starts the loop, and then join it
  control_loop.Start();
  control_loop.Join();
  return 0;
}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

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

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

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

#include <Core/Algorithms/Legacy/Fields/RegisterWithCorrespondences.h>

#include <Core/Algorithms/Base/AlgorithmVariableNames.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/GeometryPrimitives/Vector.h>
#include <Core/GeometryPrimitives/Point.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Eigen/SVD>
#include <Core/Datatypes/Legacy/Field/VMesh.h>

#include <sstream>

using namespace SCIRun;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Utility;
using namespace SCIRun::Core::Algorithms::Fields;
using namespace SCIRun::Core::Geometry;

static void printMatrix(const DenseMatrix& /*m*/, const std::string& tag = "tag")
{
  (void)tag;
#if 0
  std::cout << tag << std::endl;
  std::cout << "Size: " << m.nrows() << " x " << m.ncols() << std::endl;
  std::cout << "Min/Max: " << std::setprecision(15) << m.minCoeff() << " , " << m.maxCoeff() << std::endl;
#endif
}

RegisterWithCorrespondencesAlgo::RegisterWithCorrespondencesAlgo()
{
  addParameter(Variables::Operator, static_cast<int>(TransformType::AFFINE));
}

AlgorithmOutput RegisterWithCorrespondencesAlgo::run(const AlgorithmInput& input) const
{
  auto input_field = input.get<Field>(Variables::InputField);
  auto corres1 = input.get<Field>(Correspondences1);
  auto corres2 = input.get<Field>(Correspondences2);

  FieldHandle return_field;
  DenseMatrixHandle transform;

  auto op = TransformType(get(Variables::Operator).toInt());
  switch (op)
  {
  case TransformType::MORPH:
    transform = runMorph(input_field, corres1, corres2, return_field);
    break;
  case TransformType::AFFINE:
    transform = runAffine(input_field, corres1, corres2, return_field);
    break;
  case TransformType::RIGID:
    transform = runRigid_P(input_field, corres1, corres2, return_field);
    break;
  case TransformType::NONE:
    transform = runNone(input_field, corres1, corres2, return_field);
    break;
  }

  printMatrix(*transform,"Transform");

  AlgorithmOutput output;
  output[Variables::OutputField] = return_field;
  output[TransformMatrix] = transform;
  return output;
}

DenseMatrixHandle RegisterWithCorrespondencesAlgo::runMorph(FieldHandle input, FieldHandle Cors1, FieldHandle Cors2, FieldHandle& output)  const
{
  double sumx2;
  double sumy2;
  double sumz2;
  double sumx;
  double sumy;
  double sumz;

  if (!input) {
    error("No input field");
    return nullptr;
  }
  if (!Cors1) {
    error("No Correspondence1 input field");
    return nullptr;
  }
  if (!Cors2) {
    error("No Correspondence2 input field");
    return nullptr;
  }

  FieldInformation fi(input);

  output.reset(input->deep_clone());

  FieldHandle input_cp, Cors1_cp, Cors2_cp;
  input_cp.reset(input->deep_clone());
  Cors1_cp.reset(Cors1->deep_clone());
  Cors2_cp.reset(Cors2->deep_clone());

  if (!output)
  {
    error("Could not allocate output field");
    return nullptr;
  }

  VMesh* imesh = input_cp->vmesh();
  VMesh* omesh = output->vmesh();
  VMesh* icors1 = Cors1_cp->vmesh();
  VMesh* icors2 = Cors2_cp->vmesh();

  //get the number of nodes in input field
  //VMesh::size_type num_nodes = imesh->num_nodes();

  VMesh::Node::size_type num_cors1, num_cors2, num_pts;
  icors1->size(num_cors1);
  icors2->size(num_cors2);
  imesh->size(num_pts);

  std::vector<double> coefs;//(3*num_cors1+9);
  std::vector<double> rside;//(3*num_cors1+9);
  if (num_cors1 != num_cors2)
  {
    error("Number of correspondence points does not match");
    return nullptr;
  }

  // Request that it generates the node matrix
  imesh->synchronize(SCIRun::Mesh::NODES_E);

  //get centroids of both point clouds
  sumx = 0.0;
  sumy = 0.0;
  sumz = 0.0;
  SCIRun::Core::Geometry::Point mp;

  VMesh::size_type num_nodes = icors1->num_nodes();

  for (VMesh::Node::index_type idx = 0; idx < num_nodes; idx++)
  {
    icors1->get_center(mp, idx);
    sumx = mp.x() + sumx;
    sumy = mp.y() + sumy;
    sumz = mp.z() + sumz;
  }
  sumx = sumx / (double)num_nodes;
  sumy = sumy / (double)num_nodes;
  sumz = sumz / (double)num_nodes;

  sumx2 = 0.0;
  sumy2 = 0.0;
  sumz2 = 0.0;
  Point np;

  VMesh::size_type num_nodes2 = icors2->num_nodes();

  for (VMesh::Node::index_type idx = 0; idx < num_nodes2; idx++)
  {
    icors2->get_center(np, idx);
    sumx2 = np.x() + sumx2;
    sumy2 = np.y() + sumy2;
    sumz2 = np.z() + sumz2;
  }
  sumx2 = sumx2 / (double)num_nodes2;
  sumy2 = sumy2 / (double)num_nodes2;
  sumz2 = sumz2 / (double)num_nodes2;

  //center fields
  SCIRun::Core::Geometry::Point mypoint;

  for (VMesh::Node::index_type idx = 0; idx < num_nodes; idx++)
  {
    icors1->get_center(mp, idx);
    mypoint.x(mp.x() - sumx);
    mypoint.y(mp.y() - sumy);
    mypoint.z(mp.z() - sumz);
    icors1->set_point(mypoint, idx);
  }

  for (VMesh::Node::index_type idx = 0; idx < num_nodes2; idx++)
  {
    icors2->get_center(mp, idx);
    mypoint.x(mp.x() - sumx2);
    mypoint.y(mp.y() - sumy2);
    mypoint.z(mp.z() - sumz2);
    icors2->set_point(mypoint, idx);
  }
  VMesh::size_type num_nodes_mesh = imesh->num_nodes();
  for (VMesh::Node::index_type idx = 0; idx < num_nodes_mesh; idx++)
  {
    imesh->get_center(mp, idx);
    mypoint.x(mp.x() - sumx2);
    mypoint.y(mp.y() - sumy2);
    mypoint.z(mp.z() - sumz2);
    imesh->set_point(mypoint, idx);
  }

  //create B for big matrix//
  DenseMatrix Bm(num_cors1 + 4, num_cors1 + 4);
  VMesh::Node::iterator it;
  SCIRun::Core::Geometry::Point P;

  for (int L1 = 0; L1 < num_cors1; ++L1)
  {
    it = L1;
    icors2->get_point(P, *(it));
    //horizontal x,y,z
    Bm(0, L1) = P.x();
    Bm(1, L1) = P.y();
    Bm(2, L1) = P.z();
    Bm(3, L1) = 1;

    //vertical x,y,z
    Bm(L1 + 4, num_cors1) = P.x();
    Bm(L1 + 4, num_cors1 + 1) = P.y();
    Bm(L1 + 4, num_cors1 + 2) = P.z();
    Bm(L1 + 4, num_cors1 + 3) = 1;
  }

  for (int L1 = num_cors1; L1 < num_cors1 + 4; ++L1)
  {
    Bm(0, L1) = 0;
    Bm(1, L1) = 0;
    Bm(2, L1) = 0;
    Bm(3, L1) = 0;
  }

  //put in sigmas
  DenseMatrixHandle SMat;
  radial_basis_func(icors2, icors2, SMat);

  for (int i = 0; i < num_cors1; ++i)
  {
    for (int j = 0; j < num_cors1; ++j)
    {
      Bm(4 + i, j) = SMat->get(i, j);
    }
  }

  //Create big matrix //
  DenseMatrix BigMat(3 * num_cors1 + 12, 3 * num_cors1 + 12);

  for (int i = 0; i < (3 * num_cors1 + 12); ++i)
  {
    for (int j = 0; j < (3 * num_cors1 + 12); ++j)
    {
      BigMat(i, j) = 0;
    }
  }
  for (int i = 0; i < (num_cors1 + 4); ++i)
  {
    for (int j = 0; j < (num_cors1 + 4); ++j)
    {
      BigMat(i, j) = Bm(i, j);
    }
  }
  for (int i = 0; i < (num_cors1 + 4); ++i)
  {
    for (int j = 0; j < (num_cors1 + 4); ++j)
    {
      BigMat(num_cors1 + 4 + i, num_cors1 + 4 + j) = Bm(i, j);
    }
  }
  for (int i = 0; i < (num_cors1 + 4); ++i)
  {
    for (int j = 0; j < (num_cors1 + 4); ++j)
    {
      BigMat(2 * num_cors1 + 8 + i, 2 * num_cors1 + 8 + j) = Bm(i, j);
    }
  }

  //create right side of equation//
  for (int i = 0; i < 4; ++i){ rside.push_back(0); }
  for (int i = 0; i < num_cors1; ++i)
  {
    it = i;
    icors1->get_point(P, *(it));
    rside.push_back(P.x());
  }
  for (int i = 0; i < 4; ++i){ rside.push_back(0); }
  for (int i = 0; i < num_cors1; ++i)
  {
    it = i;
    icors1->get_point(P, *(it));
    rside.push_back(P.y());
  }
  for (int i = 0; i < 4; ++i){ rside.push_back(0); }
  for (int i = 0; i < num_cors1; ++i)
  {
    it = i;
    icors1->get_point(P, *(it));
    rside.push_back(P.z());
  }

  //Solve system of equations//
  // Bigm->solve(rside,coefs,0);
  //Use svd linear least squares

  //Create sparse matrix for sigmas of svd

  int m = 3 * num_cors1 + 12;
  int n = 3 * num_cors1 + 12;

  //create the U and V matrix
  Eigen::JacobiSVD<DenseMatrix::EigenBase> svd_mat(BigMat, Eigen::ComputeFullU | Eigen::ComputeFullV);
  DenseMatrix UMat = svd_mat.matrixU();
  DenseMatrix VMat = svd_mat.matrixV();
  DenseMatrix matSingularValues = svd_mat.singularValues();

  //Make more storage for the solving the linear least squares
  DenseMatrix RsideMat(m, 1);
  DenseMatrix CMat(n, 1);
  DenseMatrix YMat(n, 1);
  DenseMatrix CoefMat(n, 1);

  for (int loop = 0; loop < n; ++loop)
  {
    RsideMat(loop, 0) = rside[loop];
  }

  //c=trans(Um)*rside;
  CMat = UMat.transpose() * RsideMat;

  for (int k = 0; k<n; k++)
  {
    YMat(k, 0) = CMat(k, 0) / matSingularValues(k, 0);
  }

  CoefMat = VMat * YMat;

  for (int p = 0; p < n; p++)
  {
    coefs.push_back(CoefMat(p, 0));
  }
  DenseMatrixHandle transform(new DenseMatrix(CoefMat.matrix()));

  //done with solve, make the new field

  make_new_points(imesh, icors2, coefs, *omesh, sumx, sumy, sumz);

  return transform;
}

DenseMatrixHandle RegisterWithCorrespondencesAlgo::runAffine(FieldHandle input, FieldHandle Cors1, FieldHandle Cors2, FieldHandle& output) const
{
  double sumx2;
  double sumy2;
  double sumz2;
  double sumx;
  double sumy;
  double sumz;
  if (!input)
  {
    error("No input field");
    return nullptr;
  }
  if (!Cors1)
  {
    error("No Correspondence1 input field");
    return nullptr;
  }
  if (!Cors2)
  {
    error("No Correspndence2 input field");
    return nullptr;
  }

  FieldInformation fi(input);

  FieldHandle input_cp, Cors1_cp, Cors2_cp;

  output.reset(input->deep_clone());
  input_cp.reset(input->deep_clone());
  Cors1_cp.reset(Cors1->deep_clone());
  Cors2_cp.reset(Cors2->deep_clone());

  if (!output)
  {
    error("Could not allocate output field");
    return nullptr;
  }

  VMesh* imesh = input_cp->vmesh();
  VMesh* omesh = output->vmesh();
  VMesh* icors1 = Cors1_cp->vmesh();
  VMesh* icors2 = Cors2_cp->vmesh();

  //get the number of nodes in input field
  //VMesh::size_type num_nodes = imesh->num_nodes();

  VMesh::Node::size_type num_cors1, num_cors2, num_pts;
  icors1->size(num_cors1);
  icors2->size(num_cors2);
  imesh->size(num_pts);

  std::vector<double> coefs;//(3*num_cors1+9);
  std::vector<double> rside;//(3*num_cors1+9);

  if (num_cors1 != num_cors2)
  {
    error("Number of correspondence points does not match");
    return nullptr;
  }

  // Request that it generates the node matrix
  imesh->synchronize(SCIRun::Mesh::NODES_E);

  //get centroids of both point clouds
  sumx = 0.0;
  sumy = 0.0;
  sumz = 0.0;
  Point mp;

  VMesh::size_type num_nodes = icors1->num_nodes();

  for (VMesh::Node::index_type idx = 0; idx < num_nodes; idx++)
  {
    icors1->get_center(mp, idx);
    sumx = mp.x() + sumx;
    sumy = mp.y() + sumy;
    sumz = mp.z() + sumz;
  }
  sumx = sumx / (double)num_nodes;
  sumy = sumy / (double)num_nodes;
  sumz = sumz / (double)num_nodes;

  sumx2 = 0.0;
  sumy2 = 0.0;
  sumz2 = 0.0;
  Point np;

  VMesh::size_type num_nodes2 = icors2->num_nodes();

  for (VMesh::Node::index_type idx = 0; idx < num_nodes2; idx++)
  {
    icors2->get_center(np, idx);
    sumx2 = np.x() + sumx2;
    sumy2 = np.y() + sumy2;
    sumz2 = np.z() + sumz2;
  }
  sumx2 = sumx2 / (double)num_nodes;
  sumy2 = sumy2 / (double)num_nodes;
  sumz2 = sumz2 / (double)num_nodes;

  //center fields
  Point mypoint;

  for (VMesh::Node::index_type idx = 0; idx < num_nodes; idx++)
  {
    icors1->get_center(mp, idx);
    mypoint.x(mp.x() - sumx);
    mypoint.y(mp.y() - sumy);
    mypoint.z(mp.z() - sumz);
    icors1->set_point(mypoint, idx);
  }

  for (VMesh::Node::index_type idx = 0; idx < num_nodes2; idx++)
  {
    icors2->get_center(mp, idx);
    mypoint.x(mp.x() - sumx2);
    mypoint.y(mp.y() - sumy2);
    mypoint.z(mp.z() - sumz2);
    icors2->set_point(mypoint, idx);
  }

  VMesh::size_type num_nodes_mesh = imesh->num_nodes();
  for (VMesh::Node::index_type idx = 0; idx < num_nodes_mesh; idx++)
  {
    imesh->get_center(mp, idx);
    mypoint.x(mp.x() - sumx2);
    mypoint.y(mp.y() - sumy2);
    mypoint.z(mp.z() - sumz2);
    imesh->set_point(mypoint, idx);
  }

  //create B for big matrix//
  DenseMatrix BMat(num_cors1, 4);
  VMesh::Node::iterator it;
  Point P;

  for (int L1 = 0; L1 < num_cors1; ++L1)
  {
    it = L1;
    icors2->get_point(P, *(it));

    //horizontal x,y,z
    BMat(L1, 0) = P.x();
    BMat(L1, 1) = P.y();
    BMat(L1, 2) = P.z();
    BMat(L1, 3) = 1;
  }

    printMatrix(BMat, "BMat");

  //create right side of equation//
  for (int i = 0; i < num_cors1; ++i)
  {
    it = i;
    icors1->get_point(P, *(it));
    rside.push_back(P.x());
  }
  for (int i = 0; i < num_cors1; ++i)
  {
    it = i;
    icors1->get_point(P, *(it));
    rside.push_back(P.y());
  }
  for (int i = 0; i < num_cors1; ++i)
  {
    it = i;
    icors1->get_point(P, *(it));
    rside.push_back(P.z());
  }

  //Create sparse matrix for sigmas of svd
  int m = num_cors1;
  int n = 4;

  //create the U and V matrix

  printMatrix(BMat, "BMat");
  Eigen::JacobiSVD<DenseMatrix::EigenBase> svd_mat(BMat, Eigen::ComputeFullU | Eigen::ComputeFullV);
  DenseMatrix UMat = svd_mat.matrixU();
  DenseMatrix VMat = svd_mat.matrixV();
  DenseMatrix matSingularValues(svd_mat.singularValues());

  printMatrix(UMat, "UMat");
  printMatrix(VMat, "VMat");
  printMatrix(matSingularValues, "matS");
  //std::cout << "entire matS: \n" << matSingularValues << std::endl;
  //std::cout << "entire UMat: \n" << UMat << std::endl;
  //std::cout << "entire VMat: \n" << VMat << std::endl;

  //Make more storage for the solving the linear least squares
  DenseMatrix RsideMat(m, 1);
  DenseMatrix CMat(m, 1);
  DenseMatrix YMat(n, 1);
  DenseMatrix CoefMat(n, 1);

  for (int xyz = 0; xyz < 3; xyz++)
  {
    //std::cout << "\n-----XYZ " << xyz << "\n" << std::endl;
    for (int loop = 0; loop < m; ++loop)
    {
      RsideMat(loop, 0) = rside[xyz*m + loop];
    }
    printMatrix(RsideMat, "RsideMat");
    //c=trans(Um)*rside;
    CMat = UMat.transpose() * RsideMat;
    printMatrix(CMat, "CMat");

    YMat = CMat.cwiseQuotient(matSingularValues);
    printMatrix(YMat, "YMat");

    CoefMat = VMat * YMat;

    printMatrix(CoefMat, "CoefMat");
    for (int p = 0; p < n; p++)
    {
      coefs.push_back(CoefMat(p, 0));
    }
  }

  DenseMatrix rotation(Eigen::MatrixXd::Identity(4,4));
  DenseMatrix trans1(Eigen::MatrixXd::Identity(4,4));
  DenseMatrix trans2(Eigen::MatrixXd::Identity(4,4));
  double *r_data = rotation.data();

  std::copy(coefs.begin(), coefs.end(), r_data);

  // recreate previous translation
  trans1(0,3) = -sumx2;
  trans1(1,3) = -sumy2;
  trans1(2,3) = -sumz2;

  trans2(0,3) = sumx;
  trans2(1,3) = sumy;
  trans2(2,3) = sumz;

  auto trans_out = makeShared<DenseMatrix>(trans2*rotation*trans1);

  //done with solve, make the new field
  make_new_pointsA(imesh, icors2, coefs, *omesh, sumx, sumy, sumz);

#if 0
  {
    const BBox bbox = omesh->get_bounding_box();
    if (bbox.valid())
    {
      std::cout << "output size: " << bbox.diagonal() << " center: " << bbox.center() << std::endl;
    }
    else
    {
      std::cout << "output bbox invalid" << std::endl;
    }
  }
#endif

  return trans_out;
}

DenseMatrixHandle RegisterWithCorrespondencesAlgo::runRigid_P(FieldHandle input, FieldHandle Cors1, FieldHandle Cors2, FieldHandle& output) const
{
    double sumx2;
    double sumy2;
    double sumz2;
    double sumx;
    double sumy;
    double sumz;
    if (!input)
    {
        error("No input field");
        return nullptr;
    }
    if (!Cors1)
    {
        error("No Correspondence1 input field");
        return nullptr;
    }
    if (!Cors2)
    {
        error("No Correspndence2 input field");
        return nullptr;
    }

    FieldInformation fi(input);

    FieldHandle input_cp, Cors1_cp, Cors2_cp;

    output.reset(input->deep_clone());
    input_cp.reset(input->deep_clone());
    Cors1_cp.reset(Cors1->deep_clone());
    Cors2_cp.reset(Cors2->deep_clone());

    if (!output)
    {
        error("Could not allocate output field");
        return nullptr;
    }

    VMesh* imesh = input_cp->vmesh();
    VMesh* omesh = output->vmesh();
    VMesh* icors1 = Cors1_cp->vmesh();
    VMesh* icors2 = Cors2_cp->vmesh();

    //get the number of nodes in input field
    //VMesh::size_type num_nodes = imesh->num_nodes();

    VMesh::Node::size_type num_cors1, num_cors2, num_pts;
    icors1->size(num_cors1);
    icors2->size(num_cors2);
    imesh->size(num_pts);

    if (num_cors1 != num_cors2)
    {
        error("Number of correspondence points does not match");
        return nullptr;
    }

    // Request that it generates the node matrix
    imesh->synchronize(SCIRun::Mesh::NODES_E);

    //get centroids of both point clouds
    sumx = 0.0;
    sumy = 0.0;
    sumz = 0.0;
    Point mp;

    VMesh::size_type num_nodes = icors1->num_nodes();

    for (VMesh::Node::index_type idx = 0; idx < num_nodes; idx++)
    {
        icors1->get_center(mp, idx);
        sumx = mp.x() + sumx;
        sumy = mp.y() + sumy;
        sumz = mp.z() + sumz;
    }
    sumx = sumx / (double)num_nodes;
    sumy = sumy / (double)num_nodes;
    sumz = sumz / (double)num_nodes;

    sumx2 = 0.0;
    sumy2 = 0.0;
    sumz2 = 0.0;
    Point np;

    VMesh::size_type num_nodes2 = icors2->num_nodes();

    for (VMesh::Node::index_type idx = 0; idx < num_nodes2; idx++)
    {
        icors2->get_center(np, idx);
        sumx2 = np.x() + sumx2;
        sumy2 = np.y() + sumy2;
        sumz2 = np.z() + sumz2;
    }
    sumx2 = sumx2 / (double)num_nodes;
    sumy2 = sumy2 / (double)num_nodes;
    sumz2 = sumz2 / (double)num_nodes;

    //center fields
    Point mypoint;

    for (VMesh::Node::index_type idx = 0; idx < num_nodes; idx++)
    {
        icors1->get_center(mp, idx);
        mypoint.x(mp.x() - sumx);
        mypoint.y(mp.y() - sumy);
        mypoint.z(mp.z() - sumz);
        icors1->set_point(mypoint, idx);
    }

    for (VMesh::Node::index_type idx = 0; idx < num_nodes2; idx++)
    {
        icors2->get_center(mp, idx);
        mypoint.x(mp.x() - sumx2);
        mypoint.y(mp.y() - sumy2);
        mypoint.z(mp.z() - sumz2);
        icors2->set_point(mypoint, idx);
    }

    //normalize the point matrices

    double norm1;
    double norm2;
    double ssq1x=0,ssq1y=0,ssq1z=0;
    double ssq2x=0,ssq2y=0,ssq2z=0;
    Point sqp1,sqp2,normp1,normp2;

    for (VMesh::Node::index_type idx = 0; idx < num_nodes; idx++)
    {
        icors1->get_center(sqp1, idx);
        ssq1x = sqp1.x()*sqp1.x() + ssq1x;
        ssq1y = sqp1.y()*sqp1.y() + ssq1y;
        ssq1z = sqp1.z()*sqp1.z() + ssq1z;

        icors2->get_center(sqp2, idx);
        ssq2x = sqp2.x()*sqp2.x() + ssq2x;
        ssq2y = sqp2.y()*sqp2.y() + ssq2y;
        ssq2z = sqp2.z()*sqp2.z() + ssq2z;
    }

    norm1=sqrt(ssq1x+ssq1y+ssq1z);
    norm2=sqrt(ssq2x+ssq2y+ssq2z);

    for (VMesh::Node::index_type idx = 0; idx < num_nodes; idx++)
    {
        icors1->get_center(sqp1, idx);
        normp1.x(sqp1.x()/norm1);
        normp1.y(sqp1.y()/norm1);
        normp1.z(sqp1.z()/norm1);
        icors1->set_point(normp1, idx);

        icors2->get_center(sqp2, idx);
        normp2.x(sqp2.x()/norm2);
        normp2.y(sqp2.y()/norm2);
        normp2.z(sqp2.z()/norm2);
        icors2->set_point(normp2, idx);
    }

    VMesh::size_type num_nodes_mesh = imesh->num_nodes();
    for (VMesh::Node::index_type idx = 0; idx < num_nodes_mesh; idx++)
    {
        imesh->get_center(mp, idx);
        normp2.x((mp.x() - sumx2)/norm2);
        normp2.y((mp.y() - sumy2)/norm2);
        normp2.z((mp.z() - sumz2)/norm2);
        imesh->set_point(normp2, idx);
    }

    DenseMatrix Amat(3,3);
    Point p1,p2;
    VMesh::Node::index_type idx;
    DenseMatrix P1(num_cors1,3),P2(num_cors1,3);

    //This is to fine the matrix A where A=P1'*P2
    // P1 and P2 are the normalized, centered point sets of size nx3.
    for (int L1 = 0; L1 < num_cors1; L1++)
    {
        idx=L1;
        icors1->get_point(p1, idx);
        icors2->get_point(p2, idx);

        P1(L1,0)=p1.x();
        P1(L1,1)=p1.y();
        P1(L1,2)=p1.z();
        P2(L1,0)=p2.x();
        P2(L1,1)=p2.y();
        P2(L1,2)=p2.z();
    }

    Amat=P1.transpose()*P2;

    /*
    std::cout<<"P1 " << P1 <<std::endl;
    std::cout<<"P2 " << P2 <<std::endl;
    std::cout<<"Amat " << Amat <<std::endl;
    */
    Eigen::JacobiSVD<DenseMatrix::EigenBase> svd_mat(Amat, Eigen::ComputeFullU | Eigen::ComputeFullV);

    //create the U and V matrix
    DenseMatrix UMat = svd_mat.matrixU();
    DenseMatrix VMat = svd_mat.matrixV();
    DenseMatrix SVMat(svd_mat.singularValues());

    printMatrix(UMat, "UMat");
    printMatrix(VMat, "VMat");
    printMatrix(SVMat, "matS");

    //rotation matrix
    DenseMatrix Tmat(3,3);
    Tmat=UMat*VMat.transpose();

    // check for negative determinant and fix.
    // this prevents reflection in the registration
    if (Tmat.determinant()<0)
    {
        VMat(0,2)=-1*VMat(0,2);
        VMat(1,2)=-1*VMat(1,2);
        VMat(2,2)=-1*VMat(2,2);
        Tmat=UMat*VMat.transpose();
    }

    double traceA;
    traceA=SVMat(0,0)+SVMat(1,0)+SVMat(2,0);

    std::vector<double> coefs = {
      Tmat(0, 0)*traceA*norm1,
      Tmat(0, 1)*traceA*norm1,
      Tmat(0, 2)*traceA*norm1,
      0.0,
      Tmat(1, 0)*traceA*norm1,
      Tmat(1, 1)*traceA*norm1,
      Tmat(1, 2)*traceA*norm1,
      0.0,
      Tmat(2, 0)*traceA*norm1,
      Tmat(2, 1)*traceA*norm1,
      Tmat(2, 2)*traceA*norm1,
      0.0 };

  DenseMatrix rotation(Eigen::MatrixXd::Identity(4,4));
  DenseMatrix trans1(Eigen::MatrixXd::Identity(4,4));
  DenseMatrix trans2(Eigen::MatrixXd::Identity(4,4));
  DenseMatrix scale1(Eigen::MatrixXd::Identity(4,4));
  DenseMatrix scale2(Eigen::MatrixXd::Identity(4,4));
  double *r_data = rotation.data();

  std::copy(coefs.begin(), coefs.end(), r_data);

  // recreate previous moves
  trans1(0,3) = -sumx2;
  trans1(1,3) = -sumy2;
  trans1(2,3) = -sumz2;

  trans2(0,3) = sumx;
  trans2(1,3) = sumy;
  trans2(2,3) = sumz;

  scale1(0,0) = 1/norm2;
  scale1(1,1) = 1/norm2;
  scale1(2,2) = 1/norm2;

  scale2(0,0) = norm1;
  scale2(1,1) = norm1;
  scale2(2,2) = norm1;

  printMatrix(trans1,"trans1");
  printMatrix(trans2,"trans2");
  printMatrix(scale1,"scale1");
  printMatrix(scale2,"scale2");

  auto trans_out = makeShared<DenseMatrix>(trans2*rotation*scale1*trans1);

      //done with solve, make the new field
    printMatrix(*trans_out,"transform");

    make_new_pointsA(imesh, icors2, coefs, *omesh, sumx, sumy, sumz);

#if 0
    {
        const BBox bbox = omesh->get_bounding_box();
        if (bbox.valid())
        {
            std::cout << "output size: " << bbox.diagonal() << " center: " << bbox.center() << std::endl;
        }
        else
        {
            std::cout << "output bbox invalid" << std::endl;
        }
    }
#endif
    return trans_out;
}

DenseMatrixHandle RegisterWithCorrespondencesAlgo::runNone(FieldHandle input, FieldHandle Cors1, FieldHandle Cors2, FieldHandle& output) const
{
  if (!input)
  {
    error("No input field");
    return nullptr;
  }
  if (!Cors1)
  {
    error("No Correspondence1 input field");
    return nullptr;
  }
  if (!Cors2)
  {
    error("No Correspndence2 input field");
    return nullptr;
  }

  FieldHandle input_cp;

  output.reset(input->deep_clone());

  if (!output)
  {
    error("Could not allocate output field");
    return nullptr;
  }

  VMesh* imesh = input->vmesh();
  VMesh* omesh = output->vmesh();

  omesh = imesh;

  return makeShared<DenseMatrix>(Eigen::MatrixXd::Identity(4,4));

}

bool RegisterWithCorrespondencesAlgo::radial_basis_func(VMesh* Cors, VMesh* points, DenseMatrixHandle& Sigma) const
{
  VMesh::Node::size_type num_cors, num_pts;
  VMesh::Node::iterator iti, itj;
  Point Pc, Pp;

  Cors->size(num_cors);
  points->size(num_pts);

  double xcomp = 0.0, ycomp = 0.0, zcomp = 0.0, mag = 0.0;

  Sigma.reset(new DenseMatrix(num_pts, num_cors));

  for (int i = 0; i < num_pts; ++i)
  {
    for (int j = 0; j < num_cors; ++j)
    {
      iti = i;
      itj = j;

      Cors->get_point(Pc, *(itj));
      points->get_point(Pp, *(iti));

      xcomp = Pc.x() - Pp.x();
      ycomp = Pc.y() - Pp.y();
      zcomp = Pc.z() - Pp.z();

      mag = sqrt(pow(xcomp, 2.0) + pow(ycomp, 2.0) + pow(zcomp, 2.0));
      if (mag == 0)
      {
        (*Sigma)(i, j) = 0;
      }
      else
      {
        double temp = pow(mag, 2.0)*log(mag);
        (*Sigma)(i, j) = temp;
      }
    }
  }
  return true;
}

bool RegisterWithCorrespondencesAlgo::make_new_points(VMesh* points, VMesh* Cors, const std::vector<double>& coefs, VMesh& omesh, double sumx, double sumy, double sumz) const
{
  VMesh::Node::size_type num_cors, num_pts;
  VMesh::Node::iterator it, itp;
  Point P, Pp;

  Cors->size(num_cors);
  points->size(num_pts);

  int sz = 0;
  double sumerx = 0, sumery = 0, sumerz = 0;
  double sigma = 0.0;
  DenseMatrixHandle SMat;
  radial_basis_func(Cors, points, SMat);

  for (int i = 0; i < num_pts; ++i)
  {
    sumerx = 0; sumery = 0; sumerz = 0;
    for (int j = 0; j < num_cors; ++j)
    {
      sigma = SMat->get(i, j);
      sumerx += coefs[j] * sigma;
      sumery += coefs[j + 4 + num_cors] * sigma;
      sumerz += coefs[j + 8 + 2 * num_cors] * sigma;
    }

    itp = i;
    points->get_point(Pp, *(itp));
    sz = (int)num_cors;

    P.x(sumx + sumerx + (Pp.x()) * (coefs[sz]) + (Pp.y()) * (coefs[sz + 1]) + (Pp.z()) * (coefs[sz + 2]) + coefs[sz + 3]);
    P.y(sumy + sumery + (Pp.x()) * coefs[2 * sz + 4] + (Pp.y())*coefs[2 * sz + 5] + (Pp.z())*coefs[2 * sz + 6] + coefs[2 * sz + 7]);
    P.z(sumz + sumerz + (Pp.x()) * coefs[3 * sz + 8] + (Pp.y())*coefs[3 * sz + 9] + (Pp.z())*coefs[3 * sz + 10] + coefs[3 * sz + 11]);

    omesh.set_point(P, *(itp));
  }
  return true;
}

bool RegisterWithCorrespondencesAlgo::make_new_pointsA(VMesh* points, VMesh*, const std::vector<double>& coefs, VMesh& omesh, double sumx, double sumy, double sumz) const
{
  VMesh::Node::size_type num_cors, num_pts;
  VMesh::Node::iterator it, itp;
  Point P, Pp;

  points->size(num_pts);

#if 0
  std::cout << "make new points A inputs" <<
    "\ncoefs: " << coefs.size()
    << "\nsumx: " << sumx
    << "\nsumy: " << sumy
    << "\nsumz: " << sumz << "\n" << std::endl;
  std::copy(coefs.begin(), coefs.end(), std::ostream_iterator<double>(std::cout, "\n"));
#endif

  for (int i = 0; i < num_pts; ++i)
  {
    itp = i;
    points->get_point(Pp, *(itp));

    P.x(sumx + (Pp.x()) * (coefs[0]) + (Pp.y()) * (coefs[1]) + (Pp.z()) * (coefs[2]) + coefs[3]);
    P.y(sumy + (Pp.x()) * coefs[4] + (Pp.y())*coefs[5] + (Pp.z())*coefs[6] + coefs[7]);
    P.z(sumz + (Pp.x()) * coefs[8] + (Pp.y())*coefs[9] + (Pp.z())*coefs[10] + coefs[11]);
    omesh.set_point(P, *(itp));
  }
  return true;
}

const AlgorithmInputName RegisterWithCorrespondencesAlgo::Correspondences1("Correspondences1");
const AlgorithmInputName RegisterWithCorrespondencesAlgo::Correspondences2("Correspondences2");
const AlgorithmOutputName RegisterWithCorrespondencesAlgo::TransformMatrix("TransformMatrix");


/* Copyright John Reid 2007
*/

#include "bio-pch.h"

#include "bio/defs.h"

#include "bio/pssm_likelihood.h"

#include <boost/numeric/interval.hpp>

#include <iostream>
#include <sstream>

BIO_NS_START
/** Calculates the likelihoods of certain biobase scores given sequences that match the pssm. */
void
pssm_likelihood(
    const Pssm & pssm,
    const size_t max_map_size,
    BiobaseLikelihoods & result,
    float_t pseudo_count,
    bool verbose)
{
    using namespace boost::numeric;

    typedef double prob_t; //using float loses accuracy

    /** Maps scores to probabilities. */
    typedef std::map<prob_t, prob_t> probs_t;
    boost::shared_ptr<probs_t> probs1(new probs_t);
    boost::shared_ptr<probs_t> probs2(new probs_t);
    probs_t * last_probs = probs1.get(); //these alternate
    probs_t * new_probs = probs2.get();
    (*last_probs)[0.0] = std::log(1.0); //initialise with score of 0 has likelihood 1.0 at the start of the algorithm

    //for each row
    size_t row_idx = 0;
    float_t min_score = 0.0;
    float_t max_score = 0.0;
    for (Pssm::const_iterator entry = pssm.begin();
        pssm.end() != entry;
        ++entry)
    {
        max_score += entry->get_max();
        min_score += entry->get_min();

        new_probs->clear(); //start afresh

        //go to the next nucleotide if no observations
        if (0 == entry->get_num_observations())
        {
            continue;
        }

        //if we have too many scores we need to quantise them
        while (last_probs->size() > max_map_size)
        {
            if (verbose)
            {
                std::cout << "Reducing map size from " <<  last_probs->size();
            }

            const prob_t min_score = last_probs->begin()->first;
            const prob_t max_score = last_probs->rbegin()->first;
            const prob_t quantum = (max_score - min_score) / max_map_size;

            //for each pair of consecutive scores
            for (probs_t::iterator p1 = last_probs->begin(); last_probs->end() != p1; )
            {
                //get the next score
                probs_t::iterator p2 = p1; ++p2;
                if (last_probs->end() == p2)
                {
                    break;
                }

                const prob_t score1 = p1->first;
                const prob_t score2 = p2->first;

                //are the two scores close?
                BOOST_ASSERT(score2 > score1);
                if (score2 - score1 < quantum)
                {
                    //we remove them both and insert a replacement for the average score that is as likely
                    //as both together
                    probs_t::iterator to_erase1 = p1;
                    probs_t::iterator to_erase2 = p2;

                    //insert the new value
                    const prob_t prob1 = std::exp(p1->second);
                    const prob_t prob2 = std::exp(p2->second);
                    const prob_t prob_sum = prob1 + prob2;
                    //const prob_t relative_prob1 = prob1 / prob_sum;
                    const prob_t relative_prob2 = prob2 / prob_sum;
                    const prob_t avg_score = score1 + (score2 - score1) * relative_prob2; //had to be careful here with floating point underflow
                    BOOST_ASSERT(score1 <= avg_score);
                    BOOST_ASSERT(avg_score <= score2);

                    //move on before we insert the new value
                    p1 = p2; ++p1;

                    //erase the old values
                    last_probs->erase(to_erase1);
                    last_probs->erase(to_erase2);

                    //is the average score in the map already?
                    probs_t::iterator already_in_map = last_probs->find( avg_score );
                    const prob_t p_already_seen = last_probs->end() == already_in_map ? 0.0 : std::exp( already_in_map->second );

                    //insert the new value
                    const prob_t log_prob_sum = std::log( prob_sum + p_already_seen );
                    //std::pair<probs_t::iterator, bool> insert_result =
                        last_probs->insert( probs_t::value_type( avg_score, log_prob_sum ) );
                }
                else
                {
                    //move on
                    p1 = p2; ++p1;
                }
            }

            if (verbose)
            {
                std::cout << " to " <<  last_probs->size() << std::endl;
            }

        } //while map is too large

        //for each nucleotide
        for (const char * n = "acgt"; 0 != *n; ++n)
        {
            //likelihood in the pssm for this nucleotide in this position? Use a psuedo count of 1
            const prob_t prob_this_nucleo = entry->get_freq( *n, pseudo_count );

            //if none, cannot contribute to the total probabilities
            if (0.0 == prob_this_nucleo)
            {
                continue;
            }
            //BOOST_ASSERT(0 != entry->get_count(*n)); - this was only true before pseudo counts used

            const prob_t log_prob_this_nucleo = std::log(prob_this_nucleo);
            const prob_t score_this_nucleo = entry->get_score(*n);

            //for each score we have already achieved
            for (probs_t::const_iterator p = last_probs->begin(); last_probs->end() != p; ++p)
            {
                const prob_t new_score = p->first + score_this_nucleo;
                const prob_t new_log_likelihood = p->second + log_prob_this_nucleo;

                //update the new probability map

                //first try to insert
                std::pair<probs_t::iterator, bool> insert_result =
                    new_probs->insert(probs_t::value_type(new_score, new_log_likelihood));

                //did it actually insert?
                if (! insert_result.second)
                {
                    //no - so update the existing element
                    insert_result.first->second =
                        std::log(std::exp(insert_result.first->second) + std::exp(new_log_likelihood));
                }
            }

        } //for each nucleotide

        if (verbose)
        {
            //for debugging
            prob_t total_prob = 0.0;
            for (probs_t::const_iterator p = new_probs->begin(); new_probs->end() != p; ++p)
            {
                total_prob += std::exp(p->second);
            }
            std::cout
                << "Row " << row_idx << " has prob map size " << new_probs->size()
                << " and total probability: " << total_prob
                << std::endl;

        }

        std::swap(new_probs, last_probs);

    } //for each row

    //initialise the result vector
    std::fill(result.begin(), result.end(), float_t(0.0));

    //for each score in the last_probs map, place in the likelihoods vector
    for (probs_t::const_iterator p = last_probs->begin(); last_probs->end() != p; ++p)
    {
        //normalise the score
        const prob_t score = p->first;
        prob_t normalised_score = (score - min_score) / (max_score - min_score);
        BOOST_ASSERT( in( normalised_score, interval< prob_t >( -0.0001, 1.0001 ) ) );
        normalised_score = std::max( prob_t( 0.0 ), normalised_score );
        normalised_score = std::min( prob_t( 1.0 ), normalised_score );

        //what is the probability
        const prob_t log_likelihood = p->second;
        const prob_t prob = std::exp(log_likelihood);

        result[ get_biobase_score_index( result.size(), float_t( normalised_score ) ) ] += float_t( prob );
    }

    //normalise result vector
    for (BiobaseLikelihoods::iterator i = result.begin();
        result.end() != i;
        ++i)
    {
        BOOST_ASSERT( in( *i, interval< float_t >( -0.0001f, 1.0001f ) ) );
        *i = std::max( float_t( 0.0 ), *i );
        *i = std::min( float_t( 1.0 ), *i );
    }
}

BIO_NS_END


#include <AES.h>
#include <AESSubFunctions.h>
#include <DES.h>
#include <NTL/ZZ.h>
#include <RSA.h>
#include <X917.h>
#include <fstream>
#include <iostream>
#include <string>
#include <ui.h>
using namespace std;
using namespace NTL;
namespace AES {

unsigned int const Scale = 40000;

ByteBlock operator^(ByteBlock const &A, ByteBlock const &B) {
	ByteBlock C;
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
			C.content[i][j] = A.content[i][j] ^ B.content[i][j];
	return C;
}
struct FileHead {
	Byte ckey[256]; // 加密后的密钥
	ByteBlock iv;	// 16Byte
};					// 272Byte

ByteBlock decryptOneBlock(ByteBlock const &dataIn, ByteBlock const &key) {
	ByteBlock keys[11];
	getSubKeys(keys, key);
	ByteBlock out = finalRound(dataIn, keys[10], 1);
	for (int i = 9; i >= 1; i--)
		out = commonRound(out, keys[i], 1);
	out = initialRound(out, keys[0]);
	return out;
}
ByteBlock encryptOneBlock(ByteBlock const &dataIn, ByteBlock const &key) {
	ByteBlock keys[11];
	getSubKeys(keys, key);
	ByteBlock out = initialRound(dataIn, keys[0]);
	for (int i = 1; i < 10; i++)
		out = commonRound(out, keys[i]);
	out = finalRound(out, keys[10]);
	return out;
}
unsigned long long getFileSize(ifstream &file) {
	file.seekg(0, ios::end);
	unsigned long long l = (unsigned long long)file.tellg();
	file.seekg(0, ios::beg);
	return l;
}
void encipher() {
	// filename
	string ifname, ofname, keyFName;
	// file stream
	ifstream iFile;
	ofstream oFile;
	// keys
	RSA::publicKey rsaKey;
	ZZ zzkey;
	ByteBlock bbkey;
	// 准备文件头
	FileHead fileHead;
	// iv
	ByteBlock bbiv;

	// begin
	ui::printTitle("  文件加密  ");

	// 打开文件
	cout << "请输入待加密文件文件名：" << endl;
	ui::getLine(ifname);
	iFile.open(ifname, ios::in | ios::binary);
	if (!iFile.is_open()) {
		ui::pause("\n待加密文件无法打开\n任意键继续……");
		return;
	}
	cout << "请输入加密后文件文件名（默认 " << ifname << ".aes ）：" << endl;
	ui::getLine(ofname, ifname + ".aes");
	// 添加后缀
	if (ofname.rfind(".aes") != ofname.length() - 4)
		ofname += ".aes";
	oFile.open(ofname, ios::out | ios::binary);
	if (!oFile.is_open()) {
		iFile.close(); // 关闭之前的文件
		ui::pause("\n加密后文件无法打开\n任意键继续……");
		return;
	}
	cout << "请输入公钥文件文件名（默认 rsa.pub ）：" << endl;
	ui::getLine(keyFName, "rsa.pub");
	if (RSA::loadPublicKey(rsaKey, keyFName)) {
		iFile.close(); // 关闭之前的文件
		oFile.close(); // 关闭之前的文件
		ui::pause("\n公钥文件无法打开\n任意键继续……");
		return;
	}

	// 随机一个 128bit key
	Byte bKey128[16];
	zzkey = X917rand(bitset<64>(rand()), 2);
	BytesFromZZ(bKey128, zzkey, 16);
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
			bbkey.content[j][i] = bKey128[i * 4 + j]; // [j][i]因为先填列
	// 准备文件头
	// 加密 128bitkey
	BytesFromZZ(fileHead.ckey, RSA::encipher(zzkey, rsaKey),
				sizeof(fileHead.ckey));
	// fileHead.iv
	Byte biv[16];
	BytesFromZZ(biv, X917rand(bitset<64>(rand()), 2), 16);
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
			// 应该先填列，想想区别不大，算了
			bbiv.content[i][j] = biv[i * 4 + j];

	fileHead.iv = bbiv;
	oFile.write((char const *)&fileHead, sizeof(fileHead));

	// 准备写入
	unsigned long long wroted = 0, fileSize = getFileSize(iFile);
	ui::progressBar(wroted, fileSize);
	ByteBlock bbC, bbP;
	unsigned int count = 0;
	// 检查一个 byteblock 可以被填满
	while (wroted + sizeof(ByteBlock) <= fileSize) {
		iFile.read((char *)&bbP, sizeof(ByteBlock));
		bbP = bbP ^ bbiv;
		bbC = encryptOneBlock(bbP, bbkey);
		// 更新iv
		bbiv = bbC;
		oFile.write((char const *)&bbC, sizeof(ByteBlock));
		wroted += sizeof(ByteBlock);
		count++;
		if (count == Scale) {
			count = 1;
			ui::progressBar(wroted);
		}
	}
	// 最后一个 byteblock
	Byte x = wroted + sizeof(ByteBlock) - fileSize;
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++) {
			bbP.content[i][j] = x;
		}

	if (x != sizeof(ByteBlock)) // 还有多的数据
		iFile.read((char *)&bbP, 16 - x);
	bbP = bbP ^ bbiv;
	bbC = encryptOneBlock(bbP, bbkey);
	oFile.write((char const *)&bbC, sizeof(ByteBlock));
	// 关闭文件
	iFile.close();
	oFile.close();
	ui::progressBar(fileSize);
	ui::pause("\n\n加密完成。任意键继续……");
	return;
}
void decipher() {
	// filename
	string ifname, ofname, keyFName;
	// file stream
	ifstream iFile;
	ofstream oFile;
	// keys
	RSA::privateKey rsaKey;
	ZZ zzkey;
	ByteBlock bbkey;
	// 准备文件头
	FileHead fileHead;
	// iv
	ByteBlock bbiv;

	// begin
	ui::printTitle("  文件解密  ");

	// 打开文件
	cout << "请输入待解密文件文件名：" << endl;
	ui::getLine(ifname);
	// 检查后缀
	if (ifname.length() > 4 && ifname.rfind(".aes") != ifname.length() - 4) {
		ui::pause("\n待解密文件应以 .aes 为后缀\n任意键继续……");
		return;
	}
	iFile.open(ifname, ios::in | ios::binary);
	if (!iFile.is_open()) {
		ui::pause("\n待加密文件无法打开\n任意键继续……");
		return;
	}
	cout << "请输入加密后文件文件名（默认 "
		 << ifname.substr(0, ifname.length() - 4) << " ）：" << endl;
	ui::getLine(ofname, ifname.substr(0, ifname.length() - 4));
	oFile.open(ofname, ios::out | ios::binary);
	if (!oFile.is_open()) {
		iFile.close(); // 关闭之前的文件
		ui::pause("\n加密后文件无法打开\n任意键继续……");
		return;
	}
	cout << "请输入公钥文件文件名（默认 rsa.pri ）：" << endl;
	ui::getLine(keyFName, "rsa.pri");
	if (RSA::loadPrivateKey(rsaKey, keyFName)) {
		iFile.close(); // 关闭之前的文件
		oFile.close(); // 关闭之前的文件
		ui::pause("\n私钥文件无法打开\n任意键继续……");
		return;
	}

	// 准备写入
	unsigned long long wroted = 0,
					   fileSize = getFileSize(iFile) - sizeof(fileHead);
	ByteBlock bbC, bbP;
	// 读取文件头
	iFile.read((char *)&fileHead, sizeof(FileHead));

	// 获得 128bit key
	Byte bKey128[16] = {0};
	zzkey = RSA::decipher(ZZFromBytes(fileHead.ckey, 256), rsaKey);
	BytesFromZZ(bKey128, zzkey, 16);
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
			bbkey.content[j][i] = bKey128[i * 4 + j]; // [j][i]因为先填列

	// iv
	bbiv = fileHead.iv;
	ui::progressBar(wroted, fileSize);
	unsigned int count=0;
	// 检查一个 byteblock 可以被填满
	while (wroted + sizeof(ByteBlock) < fileSize) {
		iFile.read((char *)&bbC, sizeof(ByteBlock));
		bbP = decryptOneBlock(bbC, bbkey);
		bbP = bbP ^ bbiv;
		// 更新iv
		bbiv = bbC;
		oFile.write((char const *)&bbP, sizeof(ByteBlock));
		wroted += sizeof(ByteBlock);
		count++;
		if (count == Scale) {
			count = 1;
			ui::progressBar(wroted);
		}
	}

	// 最后一个 byteblock
	iFile.read((char *)&bbC, sizeof(ByteBlock));
	bbP = decryptOneBlock(bbC, bbkey);
	bbP = bbP ^ bbiv;
	// 更新iv
	bbiv = bbC;
	Byte x = 16 - ((Byte const *)&bbP)[15];
	if (x != 0)
		oFile.write((char const *)&bbP, x);
	ui::progressBar(fileSize);

	// 关闭文件
	iFile.close();
	oFile.close();
	ui::progressBar(fileSize);
	ui::pause("\n\n解密完成。任意键继续……");
	return;
}
} // namespace AES


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

#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include "SimpleMatrixFriends.hpp"
#include "SiconosMatrix.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "SiconosAlgebraTools.hpp" // for isComparableTo
#include "SiconosException.hpp"

using Siconos::Algebra::isComparableTo;

void scal(double a, const SiconosMatrix& A, SiconosMatrix& B, bool init)
{
  // To compute B = a * A (init = true) or B += a*A (init = false).
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");
  if(!B.isBlock())
    B.resetFactorizationFlags();

  if(&A == &B)
  {
    if(init) B *= a;
    else B *= (1.0 + a);
  }
  else
  {
    Siconos::UBLAS_TYPE numA = A.num();
    Siconos::UBLAS_TYPE numB = B.num();

    if(numB == Siconos::ZERO || numB == Siconos::IDENTITY)  // B = 0 or identity.
      THROW_EXCEPTION("forbidden for B being a zero or identity matrix.");

    if(numA == Siconos::ZERO)
    {
      if(init) B.zero();  // else nothing
    }
    else if(numA == Siconos::IDENTITY)
    {
      if(init)
      {
        B.eye();
        B *= a;
      }
      else
      {
        // Assuming B is square ...
        for(unsigned int i = 0; i < B.size(0); ++i)
          B(i, i) += a;
      }
    }
    else
    {
      if(numA == numB)  // if A and B are of the same type ...
      {
        switch(numA)
        {

        case Siconos::BLOCK: // A and B are block
          if(isComparableTo(A, B))
          {
            const BlockMatrix& Aref = static_cast<const BlockMatrix&>(A);
            BlockMatrix& Bref = static_cast<BlockMatrix&>(B);
            BlocksIterator1 itB1;
            BlocksIterator2 itB2;
            ConstBlocksIterator1 itA1 = Aref._mat->begin1();
            ConstBlocksIterator2 itA2;
            for(itB1 = Bref._mat->begin1(); itB1 != Bref._mat->end1(); ++itB1)
            {
              itA2 = itA1.begin();
              for(itB2 = itB1.begin(); itB2 != itB1.end(); ++itB2)
              {
                scal(a, **itA2++, **itB2, init);
              }
              itA1++;
            }
          }
          else // if A and B are not "block-consistent"
          {
            if(init)
            {
              for(unsigned int i = 0; i < A.size(0); ++i)
                for(unsigned int j = 0; j < A.size(1); ++j)
                  B(i, j) = a * A(i, j);
            }
            else
            {
              for(unsigned int i = 0; i < A.size(0); ++i)
                for(unsigned int j = 0; j < A.size(1); ++j)
                  B(i, j) += a * A(i, j);
            }
          }
          break;

        case Siconos::DENSE: // if both are dense
          if(init)
            noalias(*B.dense()) = a ** A.dense();
          else
            noalias(*B.dense()) += a ** A.dense();
          break;
        case Siconos::TRIANGULAR:
          if(init)
            noalias(*B.triang()) = a ** A.triang();
          else
            noalias(*B.triang()) += a ** A.triang();
          break;
        case Siconos::SYMMETRIC:
          if(init)
            noalias(*B.sym()) = a ** A.sym();
          else
            noalias(*B.sym()) += a ** A.sym();
          break;
        case Siconos::SPARSE:
          if(init)
            noalias(*B.sparse()) = a ** A.sparse();
          else
            noalias(*B.sparse()) += a ** A.sparse();
          break;
        case Siconos::BANDED:
          if(init)
            noalias(*B.banded()) = a ** A.banded();
          else
            noalias(*B.banded()) += a ** A.banded();
          break;
        default:
          THROW_EXCEPTION("Not implemented for A/B type.");
        }
      }
      else // if A and B are of different types.
      {
        if(numA == Siconos::BLOCK || numB == Siconos::BLOCK)  // if A or B is block
        {
          if(init)
          {
            B = A;
            B *= a;
          }
          else
          {
            SimpleMatrix tmp(A);
            tmp *= a;
            B += tmp; // bof bof ...
          }
        }
        else
        {
          if(numB != Siconos::DENSE)
            THROW_EXCEPTION("Inconsistent types between A and B (must be dense?)");

          if(init)
          {
            switch(numA)
            {
            case Siconos::DENSE:
              noalias(*B.dense()) = a ** A.dense();
              break;
            case Siconos::TRIANGULAR:
              noalias(*B.dense()) = a ** A.triang();
              break;
            case Siconos::SYMMETRIC:
              noalias(*B.dense()) = a ** A.sym();
              break;
            case Siconos::SPARSE:
              noalias(*B.dense()) = a ** A.sparse();
              break;
            case Siconos::BANDED:
              noalias(*B.dense()) = a ** A.banded();
              break;
            default:
              THROW_EXCEPTION("Not implemented for A type.");
            }
          }
          else

          {
            switch(numA)
            {
            case Siconos::DENSE:
              noalias(*B.dense()) += a ** A.dense();
              break;
            case Siconos::TRIANGULAR:
              noalias(*B.dense()) += a ** A.triang();
              break;
            case Siconos::SYMMETRIC:
              noalias(*B.dense()) += a ** A.sym();
              break;
            case Siconos::SPARSE:
              noalias(*B.dense()) += a ** A.sparse();
              break;
            case Siconos::BANDED:
              noalias(*B.dense()) += a ** A.banded();
              break;
            default:
              THROW_EXCEPTION("Not implemented for A type.");
            }
          }
        }
      }
    }
  }
}


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

#pragma once

#include <cmath>
#include <iostream>
#include <string>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "icp_localization/transform/math.hpp"
#include "icp_localization/common/time.hpp"
#include "icp_localization/common/output.hpp"
#include "icp_localization/common/math.hpp"
namespace icp_loco {

template <typename FloatType>
class Rigid3 {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Vector = Eigen::Matrix<FloatType, 3, 1>;
  using Quaternion = Eigen::Quaternion<FloatType>;
  using AngleAxis = Eigen::AngleAxis<FloatType>;

  Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}
  Rigid3(const Vector& translation, const Quaternion& rotation)
      : translation_(translation), rotation_(rotation) {}
  Rigid3(const Vector& translation, const AngleAxis& rotation)
      : translation_(translation), rotation_(rotation) {}

  static Rigid3 Rotation(const AngleAxis& angle_axis) {
    return Rigid3(Vector::Zero(), Quaternion(angle_axis));
  }

  static Rigid3 Rotation(const Quaternion& rotation) {
    return Rigid3(Vector::Zero(), rotation);
  }

  static Rigid3 Translation(const Vector& vector) {
    return Rigid3(vector, Quaternion::Identity());
  }

  static Rigid3 FromArrays(const std::array<FloatType, 4>& rotation,
                           const std::array<FloatType, 3>& translation) {
    return Rigid3(Eigen::Map<const Vector>(translation.data()),
                  Eigen::Quaternion<FloatType>(rotation[0], rotation[1],
                                               rotation[2], rotation[3]));
  }

  static Rigid3<FloatType> Identity() { return Rigid3<FloatType>(); }

  template <typename OtherType>
  Rigid3<OtherType> cast() const {
    return Rigid3<OtherType>(translation_.template cast<OtherType>(),
                             rotation_.template cast<OtherType>());
  }

  const Vector& translation() const { return translation_; }
  const Quaternion& rotation() const { return rotation_; }

  Vector& translation() { return translation_; }
  Quaternion& rotation() { return rotation_; }

  Rigid3 inverse() const {
    const Quaternion rotation = rotation_.conjugate();
    const Vector translation = -(rotation * translation_);
    return Rigid3(translation, rotation);
  }

  std::string asString() const {
    const double kRadToDeg = 180.0 / M_PI;
    const auto &t = translation();
    const auto &q = rotation();
    const std::string trans  =  string_format("t:[%f, %f, %f]", t.x(), t.y(), t.z());
    const std::string rot = string_format("q:[%f, %f, %f, %f]",q.x(), q.y(), q.z(), q.w());
    const auto rpy = toRPY(q) * kRadToDeg;
    const std::string rpyString = string_format("rpy (deg):[%f, %f, %f]",rpy.x(), rpy.y(),rpy.z());
    return trans + " ; " + rot + " ; " + rpyString;
  }

  bool isValid() const {
    return !std::isnan(translation_.x()) && !std::isnan(translation_.y()) &&
           !std::isnan(translation_.z()) &&
           std::abs(FloatType(1) - rotation_.norm()) < FloatType(1e-3);
  }

 private:
  Vector translation_;
  Quaternion rotation_;
};

template <typename FloatType>
Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs,
                            const Rigid3<FloatType>& rhs) {
  return Rigid3<FloatType>(
      lhs.rotation() * rhs.translation() + lhs.translation(),
      (lhs.rotation() * rhs.rotation()).normalized());
}

template <typename FloatType>
typename Rigid3<FloatType>::Vector operator*(
    const Rigid3<FloatType>& rigid,
    const typename Rigid3<FloatType>::Vector& point) {
  return rigid.rotation() * point + rigid.translation();
}

using Rigid3d = Rigid3<double>;
using Rigid3f = Rigid3<float>;

// Converts (roll, pitch, yaw) to a unit length quaternion. Based on the URDF
// specification http://wiki.ros.org/urdf/XML/joint.

struct TimestampedTransform {
  Time time_;
  Rigid3d transform_;
};

TimestampedTransform interpolate(const TimestampedTransform& start,
                                 const TimestampedTransform& end,
                                 const Time &time);

}  // namespace icp_loco


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

#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/exponential/functions/scalar/impl/expo/f_expo_reduction.hpp>
#include <nt2/exponential/functions/scalar/impl/expo/d_expo_reduction.hpp>
#include <nt2/exponential/functions/scalar/impl/expo/expo_scale.hpp>
#include <boost/simd/sdk/config.hpp>
#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/is_nan.hpp>
#endif

namespace nt2 { namespace details
{
  template < class A0, class Tag,  class Speed_Tag >
  struct exponential < A0, Tag, tag::simd_type, Speed_Tag, double>
  {
    typedef exp_reduction<A0,Tag>                        reduc_t;
    // compute exp(ax)
    static inline A0 expa(const A0& a0)
    {
      A0 hi, lo, x;
      A0 k = reduc_t::reduce(a0, hi, lo, x);
      A0 c = reduc_t::approx(x);
      c = reduc_t::finalize(x, c, hi, lo);
      c = nt2::if_zero_else(reduc_t::isleminlog(a0), scale(c, k));
      c = nt2::if_else(reduc_t::isgemaxlog(a0), nt2::Inf<A0>(), c);
      #ifdef BOOST_SIMD_NO_INVALIDS
        return c;
      #else
       return if_nan_else(is_nan(a0), c);
      #endif
    }
  };

  template < class A0, class Tag,  class Speed_Tag >
  struct exponential < A0, Tag, tag::simd_type, Speed_Tag, float>
  {
    typedef exp_reduction<A0,Tag>                        reduc_t;
    // compute exp(ax)
    static inline A0 expa(const A0& a0)
    {
      A0 x;
      A0 k = reduc_t::reduce(a0, x);
      x = reduc_t::approx(x);
      x = nt2::if_zero_else(reduc_t::isleminlog(a0), scale(x, k));
      x = nt2::if_else(reduc_t::isgemaxlog(a0), nt2::Inf<A0>(), x);
      #ifdef BOOST_SIMD_NO_INVALIDS
        return x;
      #else
        return if_nan_else(is_nan(a0), x);
      #endif
    }
  };

} }

#endif


#pragma once

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/optimisationProblem/kinematicallyRedundantMachines.hpp"

namespace mant {
  namespace krm {
    class ParallelKinematicMachine3PRPR : public KinematicallyRedundantMachines {
     public:
      explicit ParallelKinematicMachine3PRPR();

     protected:
      const arma::mat::fixed<2, 3> redundantJointsPosition_;
      const arma::mat::fixed<2, 3> redundantJointsAngles_;
      const arma::rowvec::fixed<3> middleJointsMinimalLength_;
      const arma::rowvec::fixed<3> middleJointsMaximalLength_;
      const arma::mat::fixed<2, 3> endEffectorJointsRelativePosition_;
    };
  }
}


/** trilateration.cpp
 *
 * Copyright (c) 2021 Navigine.
 *
 */
#include "trilateration.h"
#include <Eigen/Dense>

namespace navigine {
namespace navigation_core {

RadioMeasurementsData getIntersectedMeasurements(const Level &level, const RadioMeasurementsData& radioMsr)
{
    RadioMeasurementsData newMsr = radioMsr;
    RadioMeasurementsData badMsr;

    // Sorting distances in ascending order
    std::sort(newMsr.begin(), newMsr.end(),
              [](const RadioMeasurementData& lhs, const RadioMeasurementData& rhs)
              {
                return lhs.distance < rhs.distance;
              });

    // Method for determining the Access Point that fails
    for (const auto& msr1 : newMsr)
    {
      if (std::find_if(badMsr.begin(), badMsr.end(),
                      [msr1](const RadioMeasurementData& msr)
                      {
                        return msr1.id == msr.id;
                      }) == badMsr.end())
      {
        Transmitter<XYZPoint> tx1 = level.transmitter(msr1.id);
        const double x1 = tx1.point.x;
        const double y1 = tx1.point.y;
        const double z1 = tx1.point.z;
        for (const auto& msr2 : newMsr)
        {
          Transmitter<XYZPoint> tx2 = level.transmitter(msr2.id);
          const double x2 = tx2.point.x;
          const double y2 = tx2.point.y;
          const double z2 = tx2.point.z;
          const double dx = x2 - x1;
          const double dy = y2 - y1;
          const double dz = z2 - z1;
          const double delta = std::sqrt(dx * dx + dy * dy + dz * dz);
          if (delta > (msr1.distance + msr2.distance))
            badMsr.push_back(msr2);
        }
      }
    }

    // Update measurement vector with bad Access Points
    for (const auto& msr : badMsr)
    {
      newMsr.erase(std::remove_if(newMsr.begin(), newMsr.end(),
                                [msr](const RadioMeasurementData& elem)
                                {
                                  return msr.id == elem.id;
                                } ),newMsr.end());
    }

    return newMsr;
}

boost::optional<double> Trilateration::calculateAltitude(const Level& level, const RadioMeasurementsData& radioMeasurements)
{
    const RadioMeasurementsData levelMsrs = getLevelRadioMeasurements(level, radioMeasurements);
    const RadioMeasurementsData radioMsrs = getIntersectedMeasurements(level, levelMsrs);

    if (radioMsrs.size() < 4)
        return boost::none;

    // Define the matrix that we are going to use
    size_t count = radioMsrs.size();
    size_t rows = count - 1;
    Eigen::MatrixXd m(rows, 3);
    Eigen::VectorXd b(rows);

    // Fill in matrix according to the equations
    size_t row = 0;
    double x1, x2, y1, y2, z1, z2, r1, r2;

    Transmitter<XYZPoint> tx1 = level.transmitter(radioMsrs[0].id);
    x1 = tx1.point.x, y1 = tx1.point.y, z1 = tx1.point.z;
    r1 = radioMsrs[0].distance;
    for (size_t i = 1; i < count; ++i)
    {
      Transmitter<XYZPoint> tx2 = level.transmitter(radioMsrs[i].id);
      x2 = tx2.point.x, y2 = tx2.point.y, z2 = tx2.point.z;
      r2 = radioMsrs[i].distance;
      m(row, 0) = x2 - x1;
      m(row, 1) = y2 - y1;
      m(row, 2) = z2 - z1;
      b(row) = (pow(r1, 2) - pow(r2, 2) + (pow((x2 - x1), 2) + pow((y2 - y1), 2) + pow((z2 - z1), 2))) / 2;
      row++;
    }
    Eigen::Vector3d rawLocation = m.bdcSvd(Eigen::ComputeThinU|Eigen::ComputeThinV).solve(b);
    double x = rawLocation[0] + x1;
    double y = rawLocation[1] + y1;
    double sum1 = 0.0;
    // double sum2 = 0.0;
    bool nanFlag = true;
    for (const auto& msr : radioMsrs)
    {
      Transmitter<XYZPoint> tx = level.transmitter(msr.id);
      double deltaZ = std::sqrt(pow(msr.distance, 2) - pow((x - tx.point.x), 2 - pow((y - tx.point.y), 2)));
      if (!std::isnan(deltaZ))
      {
        nanFlag = false;
        sum1 += (tx.point.z + deltaZ) / msr.distance; //if the tracked position is below all beacons, then use the sign '-', else: '+'
        //in theory it should also be calculated: sum2 += 1.0 / msr.distance, but in practice gives worse results
      }
    }

    if (nanFlag)
      return boost::none;
    else
      return sum1; //sum1 / sum2;
}

} } // namespace navigine::navigation_core


///////////////////////////////////////////////////////////////////////////////
// pot_quantile.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_POT_QUANTILE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_POT_QUANTILE_HPP_DE_01_01_2006

#include <vector>
#include <limits>
#include <numeric>
#include <functional>
#include <boost/parameter/keyword.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/peaks_over_threshold.hpp>
#include <boost/accumulators/statistics/weighted_peaks_over_threshold.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // pot_quantile_impl
    //
    /**
        @brief Quantile Estimation based on Peaks over Threshold Method (for both left and right tails)

        Computes an estimate
        \f[
            \hat{q}_{\alpha} = \bar{u} + \frac{\bar{\beta}}{\xi}\left[(1-\alpha)^{-\xi}-1\right]
        \f]
        for a right or left extreme quantile, \f$\bar[u]\f$, \f$\bar{\beta}\f$ and \f$\xi\f$ being the parameters of the
        generalized Pareto distribution that approximates the right tail of the distribution (or the mirrored left tail,
        in case the left tail is used). In the latter case, the result is mirrored back, yielding the correct result.
    */
    template<typename Sample, typename Impl, typename LeftRight>
    struct pot_quantile_impl
      : accumulator_base
    {
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type float_type;
        // for boost::result_of
        typedef float_type result_type;

        pot_quantile_impl(dont_care)
          : sign_((is_same<LeftRight, left>::value) ? -1 : 1)
        {
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            typedef
                typename mpl::if_<
                    is_same<Impl, weighted>
                  , tag::weighted_peaks_over_threshold<LeftRight>
                  , tag::peaks_over_threshold<LeftRight>
                >::type
            peaks_over_threshold_tag;

            extractor<peaks_over_threshold_tag> const some_peaks_over_threshold = {};

            float_type u_bar    = some_peaks_over_threshold(args).template get<0>();
            float_type beta_bar = some_peaks_over_threshold(args).template get<1>();
            float_type xi_hat   = some_peaks_over_threshold(args).template get<2>();

            return this->sign_ * (u_bar + beta_bar/xi_hat * ( std::pow(
                    is_same<LeftRight, left>::value ? args[quantile_probability] : 1. - args[quantile_probability]
                , -xi_hat
              ) - 1.));
        }

        // make this accumulator serializeable
        template<class Archive>
        void serialize(Archive & ar, const unsigned int file_version)
        {
            ar & sign_;
        }

    private:
        short sign_; // if the fit parameters from the mirrored left tail extreme values are used, mirror back the result
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::pot_quantile<>
// tag::pot_quantile_prob<>
// tag::weighted_pot_quantile<>
// tag::weighted_pot_quantile_prob<>
//
namespace tag
{
    template<typename LeftRight>
    struct pot_quantile
      : depends_on<peaks_over_threshold<LeftRight> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::pot_quantile_impl<mpl::_1, unweighted, LeftRight> impl;
    };
    template<typename LeftRight>
    struct pot_quantile_prob
      : depends_on<peaks_over_threshold_prob<LeftRight> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::pot_quantile_impl<mpl::_1, unweighted, LeftRight> impl;
    };
    template<typename LeftRight>
    struct weighted_pot_quantile
      : depends_on<weighted_peaks_over_threshold<LeftRight> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::pot_quantile_impl<mpl::_1, weighted, LeftRight> impl;
    };
    template<typename LeftRight>
    struct weighted_pot_quantile_prob
      : depends_on<weighted_peaks_over_threshold_prob<LeftRight> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::pot_quantile_impl<mpl::_1, weighted, LeftRight> impl;
    };
}

// pot_quantile<LeftRight>(with_threshold_value) -> pot_quantile<LeftRight>
template<typename LeftRight>
struct as_feature<tag::pot_quantile<LeftRight>(with_threshold_value)>
{
    typedef tag::pot_quantile<LeftRight> type;
};

// pot_quantile<LeftRight>(with_threshold_probability) -> pot_quantile_prob<LeftRight>
template<typename LeftRight>
struct as_feature<tag::pot_quantile<LeftRight>(with_threshold_probability)>
{
    typedef tag::pot_quantile_prob<LeftRight> type;
};

// weighted_pot_quantile<LeftRight>(with_threshold_value) -> weighted_pot_quantile<LeftRight>
template<typename LeftRight>
struct as_feature<tag::weighted_pot_quantile<LeftRight>(with_threshold_value)>
{
    typedef tag::weighted_pot_quantile<LeftRight> type;
};

// weighted_pot_quantile<LeftRight>(with_threshold_probability) -> weighted_pot_quantile_prob<LeftRight>
template<typename LeftRight>
struct as_feature<tag::weighted_pot_quantile<LeftRight>(with_threshold_probability)>
{
    typedef tag::weighted_pot_quantile_prob<LeftRight> type;
};

// for the purposes of feature-based dependency resolution,
// pot_quantile<LeftRight> and pot_quantile_prob<LeftRight> provide
// the same feature as quantile
template<typename LeftRight>
struct feature_of<tag::pot_quantile<LeftRight> >
  : feature_of<tag::quantile>
{
};

template<typename LeftRight>
struct feature_of<tag::pot_quantile_prob<LeftRight> >
  : feature_of<tag::quantile>
{
};

// So that pot_quantile can be automatically substituted
// with weighted_pot_quantile when the weight parameter is non-void.
template<typename LeftRight>
struct as_weighted_feature<tag::pot_quantile<LeftRight> >
{
    typedef tag::weighted_pot_quantile<LeftRight> type;
};

template<typename LeftRight>
struct feature_of<tag::weighted_pot_quantile<LeftRight> >
  : feature_of<tag::pot_quantile<LeftRight> >
{
};

// So that pot_quantile_prob can be automatically substituted
// with weighted_pot_quantile_prob when the weight parameter is non-void.
template<typename LeftRight>
struct as_weighted_feature<tag::pot_quantile_prob<LeftRight> >
{
    typedef tag::weighted_pot_quantile_prob<LeftRight> type;
};

template<typename LeftRight>
struct feature_of<tag::weighted_pot_quantile_prob<LeftRight> >
  : feature_of<tag::pot_quantile_prob<LeftRight> >
{
};

}} // namespace boost::accumulators

#endif


#pragma once
#include <Fastor/Fastor.h>

#include <OpenVolumeMesh/Mesh/TetrahedralMesh.hh>
#include <boost/container/static_vector.hpp>
#include <boost/range/algorithm/find.hpp>
#include <boost/range/iterator.hpp>

namespace FeltElements
{
using Mesh = OpenVolumeMesh::GeometricTetrahedralMeshV3d;
using Vtxh = OpenVolumeMesh::VertexHandle;
using Vtx = Mesh::PointT;
template <std::size_t dim>
using Vtxhs = std::array<Vtxh, dim>;
using Cellh = OpenVolumeMesh::CellHandle;
using Scalar = Mesh::PointT::value_type;

namespace Tensor
{
enum
{
	i,
	j,
	k,
	l,
	m,
	n,
	a,
	b
};
namespace Func = Fastor;
using Index = Fastor::FASTOR_INDEX;
template <Index... indices>
using Indices = Fastor::Index<indices...>;
template <Index... indices>
using Idxs = Indices<indices...>;
template <Index... indices>
using Order = Fastor::OIndex<indices...>;

template <typename value_type, Index... indices>
using Base = Fastor::Tensor<value_type, indices...>;

template <Index rows = 3, Index cols = rows>
using Matrix = Base<Scalar, rows, cols>;
template <Index dim = 3>
using Vector = Base<Scalar, dim>;
template <Index... indices>
using Multi = Base<Scalar, indices...>;

template <typename value_type, Index... indices>
using BaseMap = Fastor::TensorMap<value_type, indices...>;
template <Index... indices>
using Map = BaseMap<Scalar, indices...>;
template <Index... indices>
using ConstMap = BaseMap<Scalar const, indices...>;
}  // namespace Tensor

namespace Node
{
static constexpr Tensor::Index dim = 3;

using Pos = Tensor::Vector<dim>;
using Force = Tensor::Vector<dim>;
}  // namespace Node

namespace Element
{
static constexpr Tensor::Index num_nodes = 4;
static constexpr Tensor::Index num_faces = 4;

using Vtxhs = FeltElements::Vtxhs<num_nodes>;
using IsoCoordDerivative = Tensor::Matrix<Node::dim, num_nodes>;
using ShapeDerivative = Tensor::Matrix<num_nodes, Node::dim>;
using SurfaceShapeDerivative = Tensor::Matrix<num_nodes - 1, Node::dim - 1>;
using ShapeDerivativeDeterminant = Tensor::Multi<num_nodes, num_nodes, num_nodes>;
using Elasticity = Tensor::Multi<Node::dim, Node::dim, Node::dim, Node::dim>;
using ShapeCartesianTransform = Tensor::Matrix<4, 4>;
using CartesianDerivative = Tensor::Matrix<Node::dim, num_nodes>;
using Gradient = Tensor::Matrix<Node::dim, Node::dim>;
using SurfaceGradient = Tensor::Matrix<Node::dim, Node::dim - 1>;
using Stress = Gradient;
using Stiffness = Tensor::Multi<num_nodes, Node::dim, num_nodes, Node::dim>;
using NodePositions = Fastor::Tensor<Scalar, num_nodes, Node::dim>;
using Forces = Fastor::Tensor<Scalar, num_nodes, Node::dim>;
using StiffnessResidual = std::tuple<Stiffness, Forces>;
}  // namespace Element

namespace BoundaryElement
{
static constexpr Tensor::Index num_nodes = Element::num_nodes - 1;

using Vtxhs = FeltElements::Vtxhs<num_nodes>;
using VtxhIdxs = std::array<Tensor::Index, BoundaryElement::num_nodes>;
using NodePositions = Tensor::Matrix<num_nodes, Node::dim>;
using Stiffness = Tensor::Multi<num_nodes, Node::dim, num_nodes, Node::dim>;
}  // namespace BoundaryElement

namespace Element
{
template <class T>
using PerFace = boost::container::static_vector<T, Element::num_faces>;
using BoundaryVtxhIdxs = PerFace<BoundaryElement::VtxhIdxs>;
using BoundaryNodePositions = PerFace<BoundaryElement::NodePositions>;
}  // namespace Element

template <typename Index = std::size_t, class Haystack, typename Needle>
constexpr Index index_of(Haystack && haystack, Needle && needle)
{
	auto const & it = boost::range::find(
		std::forward<decltype(haystack)>(haystack), std::forward<decltype(needle)>(needle));
	return static_cast<Index>(std::distance(haystack.begin(), it));
}
}  // namespace FeltElements


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_DISTANCE_CARTESIAN_HPP
#define BOOST_GEOMETRY_STRATEGIES_DISTANCE_CARTESIAN_HPP

//#include <boost/geometry/strategies/cartesian/azimuth.hpp>

#include <boost/geometry/strategies/cartesian/distance_projected_point.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras_box_box.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras_point_box.hpp>
#include <boost/geometry/strategies/cartesian/distance_segment_box.hpp>

#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/distance/comparable.hpp>
#include <boost/geometry/strategies/distance/detail.hpp>
#include <boost/geometry/strategies/distance/services.hpp>

//#include <boost/geometry/strategies/normalize.hpp>
#include <boost/geometry/strategies/relate/cartesian.hpp>

#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace distance
{

template <typename CalculationType = void>
struct cartesian
    : public strategies::relate::cartesian<CalculationType>
{
    template <typename Geometry1, typename Geometry2>
    static auto distance(Geometry1 const&, Geometry2 const&,
                         detail::enable_if_pp_t<Geometry1, Geometry2> * = nullptr)
    {
        return strategy::distance::pythagoras<CalculationType>();
    }

    template <typename Geometry1, typename Geometry2>
    static auto distance(Geometry1 const&, Geometry2 const&,
                         detail::enable_if_ps_t<Geometry1, Geometry2> * = nullptr)
    {
        return strategy::distance::projected_point
            <
                CalculationType,
                strategy::distance::pythagoras<CalculationType>
            >();
    }

    template <typename Geometry1, typename Geometry2>
    static auto distance(Geometry1 const&, Geometry2 const&,
                         detail::enable_if_pb_t<Geometry1, Geometry2> * = nullptr)
    {
        return strategy::distance::pythagoras_point_box<CalculationType>();
    }

    template <typename Geometry1, typename Geometry2>
    static auto distance(Geometry1 const&, Geometry2 const&,
                         detail::enable_if_sb_t<Geometry1, Geometry2> * = nullptr)
    {
        return strategy::distance::cartesian_segment_box
            <
                CalculationType,
                strategy::distance::pythagoras<CalculationType>
            >();
    }

    template <typename Geometry1, typename Geometry2>
    static auto distance(Geometry1 const&, Geometry2 const&,
                         detail::enable_if_bb_t<Geometry1, Geometry2> * = nullptr)
    {
        return strategy::distance::pythagoras_box_box<CalculationType>();
    }
};

namespace services
{

template <typename Geometry1, typename Geometry2>
struct default_strategy<Geometry1, Geometry2, cartesian_tag, cartesian_tag>
{
    using type = strategies::distance::cartesian<>;
};

template <typename CT>
struct strategy_converter<strategy::distance::pythagoras<CT> >
{
    template <typename S>
    static auto get(S const&)
    {
        return strategies::distance::cartesian<CT>();
    }
};

template <typename CT, typename PPS>
struct strategy_converter<strategy::distance::projected_point<CT, PPS> >
    : strategy_converter<PPS>
{};

template <typename CT>
struct strategy_converter<strategy::distance::pythagoras_point_box<CT> >
{
    static auto get(strategy::distance::pythagoras_point_box<CT> const&)
    {
        return strategies::distance::cartesian<CT>();
    }
};

template <typename CT, typename PPS>
struct strategy_converter<strategy::distance::cartesian_segment_box<CT, PPS> >
    : strategy_converter<PPS>
{};

template <typename CT>
struct strategy_converter<strategy::distance::pythagoras_box_box<CT> >
{
    static auto get(strategy::distance::pythagoras_box_box<CT> const&)
    {
        return strategies::distance::cartesian<CT>();
    }
};

template <typename CT>
struct strategy_converter<strategy::distance::comparable::pythagoras<CT> >
{
    template <typename S>
    static auto get(S const&)
    {
        return strategies::distance::detail::make_comparable(
                    strategies::distance::cartesian<CT>());
    }
};

template <typename CT>
struct strategy_converter<strategy::distance::comparable::pythagoras_point_box<CT> >
{
    static auto get(strategy::distance::comparable::pythagoras_point_box<CT> const&)
    {
        return strategies::distance::detail::make_comparable(
                    strategies::distance::cartesian<CT>());
    }
};

template <typename CT>
struct strategy_converter<strategy::distance::comparable::pythagoras_box_box<CT> >
{
    static auto get(strategy::distance::comparable::pythagoras_box_box<CT> const&)
    {
        return strategies::distance::detail::make_comparable(
                    strategies::distance::cartesian<CT>());
    }
};

} // namespace services

}} // namespace strategies::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_DISTANCE_CARTESIAN_HPP