//
// Created by fabian on 03.03.17.
//
#ifndef LEPP3_SURFACEEVALUATOR_HPP
#define LEPP3_SURFACEEVALUATOR_HPP
#include <boost/filesystem.hpp>
#include <iostream>
#include <sstream>
#include "lepp3/FrameData.hpp"
#include "lepp3/util/util.h"
class SurfaceEvaluator: public FrameDataObserver {
public:
    SurfaceEvaluator();
    /**
   * SurfaceAggregator interface implementation: processes the current models.
   */
    void updateFrame(FrameDataPtr frameData);
private:
    void init();
    float evaluate(SurfaceModelPtr const& model);
    double getAngle(const pcl::ModelCoefficients &coeffs);
    std::string file_path_;
    std::vector<SurfaceModelPtr> surfaces;
    std::vector<size_t> indeces;
};
SurfaceEvaluator::SurfaceEvaluator() {
    init();
}
float SurfaceEvaluator::evaluate(SurfaceModelPtr const &model) {
    //model->get_hull()->points[0].x
    float tmp = 0;
    for(int i = 0; i < model->get_hull()->points.size(); ++i) {
        if (i != model->get_hull()->points.size() - 1) {
            float mul_a = model->get_hull()->points[i].x * model->get_hull()->points[i+1].y;
            float mul_b = model->get_hull()->points[i+1].x * model->get_hull()->points[i].y;
            tmp = tmp + (mul_a - mul_b);
        } else {
            float mul_a = model->get_hull()->points[i].x * model->get_hull()->points[0].y;
            float mul_b = model->get_hull()->points[0].x * model->get_hull()->points[i].y;
            tmp = tmp + (mul_a - mul_b);
        }
    }
    tmp *= 0.5;
    float area = std::abs(tmp);
//    std::stringstream ss;
//    ss << model->id() << ","
//       << area << ","
//       << std::endl;
//    std::ofstream tf_fout;
//    // open the file and add the current model evaluation to the end of it
//    tf_fout.open(file_path_.c_str(), std::ofstream::app);
//    tf_fout << ss.str();
//    tf_fout.close();
    return area;
}
double SurfaceEvaluator::getAngle(const pcl::ModelCoefficients &coeffs) {
    float dot_product = (coeffs.values[0] * 0) + (coeffs.values[1] * 0) + (coeffs.values[2] * 1);
    float coeff_length = std::cbrt(coeffs.values[0] * coeffs.values[0] + coeffs.values[1] * coeffs.values[1] + coeffs.values[2] * coeffs.values[2]);
    double angle = acos(dot_product/coeff_length) * 180 / M_PI;
    return angle;
}
void SurfaceEvaluator::init() {
    namespace bfs = boost::filesystem;
    std::stringstream ss;
    // Create the evaluation directory
    ss << "../surfaceEvaluation/";
    std::string dir = ss.str();
    if ( !bfs::exists(bfs::path(dir)) )
        bfs::create_directory(bfs::path(dir));
    // Create the sub directory based on current timestamp
    ss << lepp::get_current_timestamp();
    dir = ss.str();
    bfs::create_directory(bfs::path(dir));
    // Prepare the csv file path
    ss  << "/surfeval.csv";
    file_path_ = ss.str();
    std::cout << "file_path: " << file_path_ << std::endl;
    // Create the file header
    std::ofstream tf_fout;
    tf_fout.open(file_path_.c_str(), std::ofstream::app);
    tf_fout << "model id,"
            << "est. surface,"
            << "total surface,"
            << "angle,"
            << std::endl;
    tf_fout.close();
}
void SurfaceEvaluator::updateFrame(FrameDataPtr frameData) {
    size_t sz = frameData->surfaces.size();
    cout << sz << endl;
    if (sz > 0) {
        float total_area = 0;
        /* Use this implementation if point of view creates multiple surfaces for one object
        if (sz > 2) {
            for (size_t i = 0; i < sz; ++i) {
                SurfaceModelPtr current = frameData->surfaces[i];
                float ref_x = current->get_planeCoefficients().values[0];
                float ref_y = current->get_planeCoefficients().values[1];
                float ref_z = current->get_planeCoefficients().values[2];
                bool surface_flag = false;
                for (size_t j = i+1; j < sz; ++j) {
                    SurfaceModelPtr comp = frameData->surfaces[j];
                    float comp_x = comp->get_planeCoefficients().values[0];
                    float comp_y = comp->get_planeCoefficients().values[1];
                    float comp_z = comp->get_planeCoefficients().values[2];
                    if (ref_x == comp_x && ref_y == comp_y && ref_z == comp_z) {
                        surface_flag = true;
                        indeces.push_back(j);
                    }
                }
                if (surface_flag) {
                    indeces.push_back(i);
                }
            }
            for (size_t i = 0; i < sz; ++i) {
                bool contain = true;
                for (size_t j = 0; j < indeces.size(); ++j) {
                    if (indeces[j] == i) {
                        contain = false;
                    }
                }
                if (contain) {
                    surfaces.push_back(frameData->surfaces[i]);
                }
            }
            if (surfaces.size() == 2) {
                double ref_z = surfaces[0]->centerpoint().z;
                double comp_z = surfaces[1]->centerpoint().z;
                if (ref_z > comp_z)
                    surfaces.erase(surfaces.begin() + 1);
                else
                    surfaces.erase(surfaces.begin());
            }
            for (size_t i = 0; i < surfaces.size(); ++i) {
            float area = evaluate(surfaces[i]);
            total_area += area;
            std::stringstream ss;
            ss << surfaces[i]->id() << ","
               << area << ","
               << total_area
               << std::endl;
            std::ofstream tf_fout;
            // open the file and add the current model evaluation to the end of it
            tf_fout.open(file_path_.c_str(), std::ofstream::app);
            tf_fout << ss.str();
            tf_fout.close();
            }
            surfaces.clear();
        } else {
            float area = evaluate(frameData->surfaces[0]);
            total_area += area;
            std::stringstream ss;
            ss << frameData->surfaces[0]->id() << ","
               << area << ","
               << total_area
               << std::endl;
            std::ofstream tf_fout;
            // open the file and add the current model evaluation to the end of it
            tf_fout.open(file_path_.c_str(), std::ofstream::app);
            tf_fout << ss.str();
            tf_fout.close();
        }
         */
        for (size_t i = 0; i < sz; ++i) {
            double angle = getAngle(frameData->surfaces[i]->get_planeCoefficients());
            float area = evaluate(frameData->surfaces[i]);
            total_area += area;
            std::stringstream ss;
            ss << frameData->surfaces[i]->id() << ","
               << area << ","
               << total_area << ","
               << angle << std::endl;
            std::ofstream tf_fout;
            tf_fout.open(file_path_.c_str(), std::ofstream::app);
            tf_fout << ss.str();
            tf_fout.close();
        }
    }
}
#endif //LEPP3_SURFACEEVALUATOR_HPP


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Musegaas, P. (2012). Optimization of Space Trajectories Including Multiple Gravity Assists
 *          and Deep Space Maneuvers. MSc Thesis, Delft University of Technology, Delft,
 *          The Netherlands.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include <tudat/astro/basic_astro/physicalConstants.h>
#include <tudat/basics/testMacros.h>

#include "tudat/astro/ephemerides/constantEphemeris.h"
#include "tudat/astro/mission_segments/transferNode.h"
#include "tudat/astro/mission_segments/transferLeg.h"

namespace tudat
{
namespace unit_tests
{

//! Test implementation of departure leg within MGA trajectory model.
BOOST_AUTO_TEST_SUITE( test_departure_leg_mga )

//! Test delta-V computation.
BOOST_AUTO_TEST_CASE( testVelocities )
{
    // Set tolerance. Due to iterative nature in the process, (much) higher accuracy cannot be
    // achieved.
    const double tolerance = 1.0e-6;

    // Expected test result based on the first leg of the ideal Cassini 1 trajectory as modelled by
    // GTOP software distributed and downloadable from the ESA website, or within the PaGMO
    // Astrotoolbox.
    const double expectedDeltaV = 2754.63598732926;
    const Eigen::Vector3d expectedVelocity ( 34216.4827530912, -15170.1440677825,
                                             395.792122152361 );

    // Specify the required parameters.
    // Set the planetary positions and velocities.
    const Eigen::Vector6d planet1State =
            ( Eigen::Vector6d( ) <<
              113191651440.549, 95992973233.5064, 0.0,
              -19752.2624404406, 22607.9064746733, 0.0 ).finished( );
    std::shared_ptr< ephemerides::Ephemeris > constantEphemeris1 =
            std::make_shared< ephemerides::ConstantEphemeris >( planet1State );

    const Eigen::Vector6d planet2State =
            ( Eigen::Vector6d( ) <<
              -35554348960.8278, -102574987127.178, 648696819.780156,
              TUDAT_NAN, TUDAT_NAN, TUDAT_NAN ).finished( );
    std::shared_ptr< ephemerides::Ephemeris > constantEphemeris2 =
            std::make_shared< ephemerides::ConstantEphemeris >( planet2State );

    // Set the time of flight, which has to be converted from JD (in GTOP) to seconds (in Tudat).
    const double timeOfFlight = 158.302027105278 * physical_constants::JULIAN_DAY;

    // Set the Sun gravitational parameters.
    const double sunGravitationalParameter = 1.32712428e20;

    using namespace mission_segments;
    UnpoweredUnperturbedTransferLeg transferLeg(
                constantEphemeris1, constantEphemeris2,
                sunGravitationalParameter );
    transferLeg.updateLegParameters( ( Eigen::VectorXd( 2 )<<0.0, timeOfFlight ).finished( ) );

    // Set the Earth gravitational parameters.
    const double earthGravitationalParameter = 3.9860119e14;

    // Set the departure parking orbit (at inifinity)
    const double semiMajorAxis = std::numeric_limits< double >::infinity( );
    const double eccentricity = 0.;

    Eigen::Vector3d departureVelocity =
            transferLeg.getDepartureVelocity( );
    DepartureWithFixedOutgoingVelocityNode departureNode(
                constantEphemeris1,
                earthGravitationalParameter, semiMajorAxis, eccentricity,
                [=]( ){ return departureVelocity; } );
    departureNode.updateNodeParameters( ( Eigen::VectorXd( 1 )<< 0.0 ).finished( ) );

    // Prepare the variables for the results.
    Eigen::Vector3d resultingVelocity = transferLeg.getArrivalVelocity( );
    double resultingDeltaV = departureNode.getNodeDeltaV( );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance and if the computed velocity before target planet matches the expected velocity
    // within the specified tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaV, resultingDeltaV, tolerance );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedVelocity, resultingVelocity, tolerance );

    // Get data on 10 equispace points on trajectory
    std::map< double, Eigen::Vector6d > statesAlongTrajectory;
    transferLeg.getStatesAlongTrajectory( statesAlongTrajectory, 10 );

    // Check initial and final time on output list
    BOOST_CHECK_SMALL( statesAlongTrajectory.begin( )->first, 1.0E-14 );
    BOOST_CHECK_CLOSE_FRACTION( statesAlongTrajectory.rbegin( )->first, timeOfFlight, 1.0E-14 );

    // Check if Keplerian state (slow elements) is the same for each output point
    Eigen::Vector6d previousKeplerianState = Eigen::Vector6d::Constant( TUDAT_NAN );
    for( auto it : statesAlongTrajectory )
    {
        Eigen::Vector6d currentCartesianState = it.second;
        Eigen::Vector6d currentKeplerianState = tudat::orbital_element_conversions::convertCartesianToKeplerianElements(
                    currentCartesianState, sunGravitationalParameter );
        if( previousKeplerianState == previousKeplerianState )
        {
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        ( previousKeplerianState.segment( 0, 5 ) ),
                        ( currentKeplerianState.segment( 0, 5 ) ),
                        1.0E-14 );

        }
        previousKeplerianState = currentKeplerianState;
    }

    // Check if output meets boundary conditions
    for( int i = 0; i < 3; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( statesAlongTrajectory.begin( )->second( i ) - planet1State( i ) ), 1.0E-2 );
        BOOST_CHECK_SMALL( std::fabs( statesAlongTrajectory.rbegin( )->second( i ) - planet2State( i ) ), 1.0E-2 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Copyright (c) 2020 fortiss GmbH
//
// Authors: Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef BARK_GEOMETRY_COMMONS_HPP_
#define BARK_GEOMETRY_COMMONS_HPP_

#include <Eigen/Core>
#include <algorithm>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <cmath>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
#include "bark/commons/util/util.hpp"

namespace bark {
namespace geometry {

//! using boost geometry
namespace bg = boost::geometry;

//! points
template <typename T>
using Point2d_t = bg::model::point<T, 2, bg::cs::cartesian>;
using Point2d = Point2d_t<double>;

//! Point operators
inline bool operator==(const Point2d& lhs, const Point2d& rhs) {
  return (bg::get<0>(lhs) == bg::get<0>(rhs) &&
          bg::get<1>(lhs) == bg::get<1>(rhs));
}
inline bool operator!=(const Point2d& lhs, const Point2d& rhs) {
  return !(lhs == rhs);
}
inline Point2d operator+(const Point2d& lhs, const Point2d& rhs) {
  return Point2d(bg::get<0>(lhs) + bg::get<0>(rhs),
                 bg::get<1>(lhs) + bg::get<1>(rhs));
}
inline Point2d operator+(const Point2d& lhs, const double& rhs) {
  return Point2d(bg::get<0>(lhs) + rhs, bg::get<1>(lhs) + rhs);
}
inline Point2d operator-(const Point2d& lhs, const Point2d& rhs) {
  return Point2d(bg::get<0>(lhs) - bg::get<0>(rhs),
                 bg::get<1>(lhs) - bg::get<1>(rhs));
}
inline Point2d operator-(const Point2d& lhs, const double& rhs) {
  return Point2d(bg::get<0>(lhs) - rhs, bg::get<1>(lhs) - rhs);
}

inline Point2d operator*(const Point2d& point, const double& factor) {
  return Point2d(bg::get<0>(point) * factor, bg::get<1>(point) * factor);
}
inline Point2d operator/(const Point2d& point, const double& divisor) {
  return Point2d(bg::get<0>(point) / divisor, bg::get<1>(point) / divisor);
}

using Pose = Eigen::Vector3d;

inline std::string print(const Point2d& p) {
  std::stringstream ss;
  ss << "Point2d: x: " << bg::get<0>(p) << ", y: " << bg::get<1>(p)
     << std::endl;
  return ss.str();
}

inline double Distance(const Point2d& p1, const Point2d& p2) {
  double dx = bg::get<0>(p1) - bg::get<0>(p2);
  double dy = bg::get<1>(p1) - bg::get<1>(p2);
  return sqrt(dx * dx + dy * dy);
}

template <typename G, typename T>
struct Shape {
  Shape(const Pose& center, std::vector<T> points, int32_t id)
      : obj_(), id_(id), center_(center) {
    for (auto it = points.begin(); it != points.end(); ++it) AddPoint(*it);
  }

  Shape(const Pose& center,
        const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& points,
        int32_t id)
      : obj_(), id_(id), center_(center) {
    auto row_num = points.rows();
    for (auto rowIter = 0; rowIter < row_num; ++rowIter) {
      AddPoint(T(points.coeff(rowIter, 0), points.coeff(rowIter, 1)));
    }
  }

  Shape(const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& points,
        int32_t id)
      : obj_(), id_(id) {
    auto row_num = points.rows();
    for (auto rowIter = 0; rowIter < row_num; ++rowIter) {
      AddPoint(T(points.coeff(rowIter, 0), points.coeff(rowIter, 1)));
    }
  }

  virtual ~Shape() {}
  virtual std::shared_ptr<Shape> Clone() const = 0;
  virtual std::string ShapeToString() const;

  // translates, scales, and rotates object
  std::shared_ptr<Shape<G, T>> ScalingTransform(const double& scaling_factor,
                                                const Pose& pose) const;

  // rotates object
  std::shared_ptr<Shape<G, T>> Rotate(const double& a) const;

  // translates object
  std::shared_ptr<Shape<G, T>> Translate(const Point2d& point) const;

  // return object transform
  std::shared_ptr<Shape<G, T>> Transform(const Pose& pose) const;

  // Scales in x- and y-direction separately
  virtual std::shared_ptr<Shape<G, T>> Scale(const double& x_dir, const double& y_) const;

  // Inflates in x- and y-direction separately
  virtual std::shared_ptr<Shape<G, T>> Inflate(const double& x_dir, const double& y_dir) const;

  virtual bool Valid() const;

  virtual Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> ToArray()
      const = 0;

  bool AddPoint(const T& p) {
    bg::append(obj_, p);
    return true;
  }

  std::vector<T> GetPoints() const {
      std::vector<T> points;
      std::copy(Shape<G, T>::obj_.begin(),
          Shape<G, T>::obj_.end(),
          std::back_inserter(points));
        return points;
  }

  std::pair<T, T> BoundingBox() const {
    boost::geometry::model::box<T> box;
    boost::geometry::envelope(obj_, box);
    boost::geometry::correct(box);
    return std::make_pair(
        T(bg::get<bg::min_corner, 0>(box), bg::get<bg::min_corner, 1>(box)),
        T(bg::get<bg::max_corner, 0>(box), bg::get<bg::max_corner, 1>(box)));
  }

  G obj_;
  int32_t id_;
  Pose center_;  // fixed center pose of shape
};

template <typename G, typename T>
inline bool Shape<G, T>::Valid() const {
  std::string message;
  bool valid = boost::geometry::is_valid(obj_, message);
  if (!valid) {
    LOG(ERROR) << "Polygon not valid. Why not valid? " << message;
  }
  return valid;
}

template <typename G, typename T>
inline std::shared_ptr<Shape<G, T>> Shape<G, T>::Rotate(const double& a) const {
  namespace trans = boost::geometry::strategy::transform;
  // move shape relative to coordinate center
  trans::translate_transformer<double, 2, 2> translate_rel_to_center(
      -center_[0], -center_[1]);
  G obj_rel_translated;
  boost::geometry::transform(obj_, obj_rel_translated, translate_rel_to_center);

  // rotate (counterclockwise)
  trans::rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(-a);
  G obj_rotated;
  boost::geometry::transform(obj_rel_translated, obj_rotated, rotate);

  // move object backwards plus translation component
  trans::translate_transformer<double, 2, 2> translate_backwards(center_[0],
                                                                 center_[1]);
  G obj_transformed;
  boost::geometry::transform(obj_rotated, obj_transformed, translate_backwards);

  std::shared_ptr<Shape<G, T>> shape_transformed = this->Clone();
  shape_transformed->obj_ = obj_transformed;
  shape_transformed->center_[2] += a;
  return shape_transformed;
}

template <typename G, typename T>
inline std::shared_ptr<Shape<G, T>> Shape<G, T>::ScalingTransform(
    const double& scaling_factor, const Pose& pose) const {
  namespace trans = boost::geometry::strategy::transform;
  // move shape relative to coordinate center
  trans::translate_transformer<double, 2, 2> translate_rel_to_center(
      -center_[0], -center_[1]);
  G obj_rel_translated;
  boost::geometry::transform(obj_, obj_rel_translated, translate_rel_to_center);

  trans::scale_transformer<double, 2, 2> scale(scaling_factor);
  G obj_scaled;
  boost::geometry::transform(obj_rel_translated, obj_scaled, scale);

  // rotate (counterclockwise)
  trans::rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(
      -pose[2]);
  G obj_rotated;
  boost::geometry::transform(obj_scaled, obj_rotated, rotate);

  // move object backwards plus translation component
  trans::translate_transformer<double, 2, 2> translate_backwards(
      center_[0] + pose[0], center_[1] + pose[1]);
  G obj_transformed;
  boost::geometry::transform(obj_rotated, obj_transformed, translate_backwards);

  std::shared_ptr<Shape<G, T>> shape_transformed = this->Clone();
  shape_transformed->obj_ = obj_transformed;
  shape_transformed->center_[0] += pose[0];
  shape_transformed->center_[1] += pose[1];
  shape_transformed->center_[2] += pose[2];
  return shape_transformed;
}

template <typename G, typename T>
inline std::shared_ptr<Shape<G, T>> Shape<G, T>::Translate(
    const Point2d& point) const {
  namespace trans = boost::geometry::strategy::transform;
  trans::translate_transformer<double, 2, 2> translate_backwards(
      bg::get<0>(point), bg::get<1>(point));
  G obj_transformed;
  boost::geometry::transform(obj_, obj_transformed, translate_backwards);

  std::shared_ptr<Shape<G, T>> shape_transformed = this->Clone();
  shape_transformed->obj_ = obj_transformed;
  shape_transformed->center_[0] += bg::get<0>(point);
  shape_transformed->center_[1] += bg::get<1>(point);
  return shape_transformed;
}

template <typename G, typename T>
inline std::shared_ptr<Shape<G, T>> Shape<G, T>::Transform(
    const Pose& pose) const {
  namespace trans = boost::geometry::strategy::transform;
  // move shape relative to coordinate center
  trans::translate_transformer<double, 2, 2> translate_rel_to_center(
      -center_[0], -center_[1]);
  G obj_rel_translated;
  boost::geometry::transform(obj_, obj_rel_translated, translate_rel_to_center);

  // rotate (counterclockwise)
  trans::rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(
      -pose[2]);
  G obj_rotated;
  boost::geometry::transform(obj_rel_translated, obj_rotated, rotate);

  // move object backwards plus translation component
  trans::translate_transformer<double, 2, 2> translate_backwards(
      center_[0] + pose[0], center_[1] + pose[1]);
  G obj_transformed;
  boost::geometry::transform(obj_rotated, obj_transformed, translate_backwards);

  std::shared_ptr<Shape<G, T>> shape_transformed = this->Clone();
  shape_transformed->obj_ = obj_transformed;
  shape_transformed->center_[0] += pose[0];
  shape_transformed->center_[1] += pose[1];
  shape_transformed->center_[2] += pose[2];
  return shape_transformed;
}

template <typename G, typename T>
inline std::shared_ptr<Shape<G, T>> Shape<G, T>::Scale(const double& x_dir, const double& y_dir) const {
  namespace trans = boost::geometry::strategy::transform;
  // move shape relative to coordinate center
  trans::translate_transformer<double, 2, 2> translate_rel_to_center(
      -center_[0], -center_[1]);
  G obj_rel_translated;
  boost::geometry::transform(obj_, obj_rel_translated, translate_rel_to_center);

  // rotate so that orientation is aligned with x-axis
  trans::rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(
      center_[2]);
  G obj_rotated;
  boost::geometry::transform(obj_rel_translated, obj_rotated, rotate);

  // Do longitudinal and lateral scaling
  const auto x_m_scale = x_dir == 0.0 ? 1.0 : x_dir;
  const auto y_m_scale = y_dir == 0.0 ? 1.0 : y_dir;
  trans::matrix_transformer<double, 2, 2> scale_transform(x_m_scale, 0.0, 0.0, 0.0, y_m_scale, 0.0, 0.0, 0.0, 0.0);
  G obj_scaled;
  boost::geometry::transform(obj_rotated, obj_scaled, scale_transform);

  // rotate back according to center specification
  trans::rotate_transformer<boost::geometry::radian, double, 2, 2> back_rotate(
      -center_[2]);
  G obj_back_rotated;
  boost::geometry::transform(obj_scaled, obj_back_rotated, back_rotate);

  // move object according to center specification
  trans::translate_transformer<double, 2, 2> translate_backwards(
      center_[0], center_[1]);
  G obj_transformed;
  boost::geometry::transform(obj_back_rotated, obj_transformed, translate_backwards);

  std::shared_ptr<Shape<G, T>> shape_transformed = this->Clone();
  shape_transformed->obj_ = obj_transformed;
  return shape_transformed;
}

template <typename G, typename T>
inline std::shared_ptr<Shape<G, T>> Shape<G, T>::Inflate(const double& x_dir, const double& y_dir) const {
  namespace trans = boost::geometry::strategy::transform;
  // move shape relative to coordinate center
  trans::translate_transformer<double, 2, 2> translate_rel_to_center(
      -center_[0], -center_[1]);
  G obj_rel_translated;
  boost::geometry::transform(obj_, obj_rel_translated, translate_rel_to_center);

  // rotate so that orientation is aligned with x-axis
  trans::rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(
      center_[2]);
  G obj_rotated;
  boost::geometry::transform(obj_rel_translated, obj_rotated, rotate);

  // Do x and y inflating
  const auto get_inflated = [&](const T& p) {
    T p_inflated;
    if (bg::get<0>(p) >= 0.0 && bg::get<1>(p) >= 0.0)
      p_inflated = T(bg::get<0>(p) + x_dir, bg::get<1>(p) + y_dir);
    else if (bg::get<0>(p) >= 0.0 && bg::get<1>(p) < 0.0)
      p_inflated = T(bg::get<0>(p) + x_dir, bg::get<1>(p) - y_dir);
    else if (bg::get<0>(p) < 0.0 && bg::get<1>(p) < 0.0)
      p_inflated = T(bg::get<0>(p) - x_dir, bg::get<1>(p) -y_dir);
    else
      p_inflated = T(bg::get<0>(p) - x_dir, bg::get<1>(p) + y_dir);
    return p_inflated;
  };
  G inflated_object;
  if constexpr (std::is_same<G, bg::model::polygon<T>>::value) {
    const auto& points = obj_rotated.outer();
    for (const auto& p : points) {
      bg::append(inflated_object, get_inflated(p));
    }
  }
  else if constexpr (std::is_same<G, bg::model::linestring<T>>::value) {
    for (const auto& p : obj_rotated) {
      bg::append(inflated_object, get_inflated(p));
    }
  }

  // rotate back according to center specification
  trans::rotate_transformer<boost::geometry::radian, double, 2, 2> back_rotate(
      -center_[2]);
  G obj_back_rotated;
  boost::geometry::transform(inflated_object, obj_back_rotated, back_rotate);

  // move object according to center specification
  trans::translate_transformer<double, 2, 2> translate_backwards(
      center_[0], center_[1]);
  G obj_transformed;
  boost::geometry::transform(obj_back_rotated, obj_transformed, translate_backwards);

  std::shared_ptr<Shape<G, T>> shape_transformed = this->Clone();
  shape_transformed->obj_ = obj_transformed;
  return shape_transformed;
}

template <typename G, typename T>
inline std::string Shape<G, T>::ShapeToString() const {
  std::stringstream ss;
  Eigen::IOFormat OctaveFmt(Eigen::FullPrecision, 0, ", ", ";\n", "", "", "[",
                            "]");
  ss << ToArray().format(OctaveFmt);
  return ss.str();
}

template <typename G, typename T>
std::vector<Point2d> Intersection(const G& g1, const T& g2) {
  std::vector<Point2d> intersecting_points;
  bg::intersection(g1.obj_, g2.obj_, intersecting_points);
  return intersecting_points;
}

}  // namespace geometry
}  // namespace bark

#endif  // BARK_GEOMETRY_COMMONS_HPP_


#define BOOST_TEST_MODULE "test_read_lennard_jones_wall_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/input/read_external_potential.hpp>
#include <tuple>

using test_types = std::tuple<double, float>;

constexpr inline float  tolerance_value(float)  noexcept {return 1e-4;}
constexpr inline double tolerance_value(double) noexcept {return 1e-8;}

template<typename Real>
decltype(boost::test_tools::tolerance(std::declval<Real>()))
tolerance() {return boost::test_tools::tolerance(tolerance_value(Real()));}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_lennard_jones_wall_noenv, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_lennard_jones_wall.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction    = "Distance"
            potential      = "LennardJonesWall"
            shape.name     = "AxisAlignedPlane"
            shape.axis     = "+X"
            shape.position = 1.0
            shape.margin   = 0.5
            parameters     = [
                {index =  0, sigma = 2.0, epsilon = 1.5},
                {index =  2, "σ"   = 2.0, epsilon = 1.5},
                {index =  9, sigma = 2.0, "ε"     = 1.5},
                {index = 10, "σ"   = 2.0, "ε"     = 1.5},
            ]
        )"_toml;

        const auto g = mjolnir::read_lennard_jones_wall_potential<real_type>(v);

        BOOST_TEST(g.participants().size() ==  4u);
        BOOST_TEST(g.participants().at(0)  ==  0u);
        BOOST_TEST(g.participants().at(1)  ==  2u);
        BOOST_TEST(g.participants().at(2)  ==  9u);
        BOOST_TEST(g.participants().at(3)  == 10u);

        BOOST_TEST(g.parameters().at( 0).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 0).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.cutoff() == real_type(2.5), tolerance<real_type>());
    }

    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction    = "Distance"
            potential      = "LennardJonesWall"
            shape.name     = "AxisAlignedPlane"
            shape.axis     = "+X"
            shape.position = 1.0
            shape.margin   = 0.5
            cutoff = 3.14
            parameters     = [
                {index =  0, sigma = 2.0, epsilon = 1.5},
                {index =  2, "σ"   = 2.0, epsilon = 1.5},
                {index =  9, sigma = 2.0, "ε"     = 1.5},
                {index = 10, "σ"   = 2.0, "ε"     = 1.5},
            ]
        )"_toml;

        const auto g = mjolnir::read_lennard_jones_wall_potential<real_type>(v);

        BOOST_TEST(g.participants().size() ==  4u);
        BOOST_TEST(g.participants().at(0)  ==  0u);
        BOOST_TEST(g.participants().at(1)  ==  2u);
        BOOST_TEST(g.participants().at(2)  ==  9u);
        BOOST_TEST(g.participants().at(3)  == 10u);

        BOOST_TEST(g.parameters().at( 0).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 0).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.cutoff() == real_type(3.14), tolerance<real_type>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_lennard_jones_wall_env, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_lennard_jones_wall.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction    = "Distance"
            potential      = "LennardJonesWall"
            shape.name     = "AxisAlignedPlane"
            shape.axis     = "+X"
            shape.position = 1.0
            shape.margin   = 0.5
            env.large  = 10.0
            env.small  = 0.01
            env.strong = 100.0
            env.weak   = 0.001
            cutoff = 3.14
            parameters     = [
                {index =  0, sigma = "large", epsilon = "strong"},
                {index =  2, "σ"   = "large", epsilon = "weak"},
                {index =  9, sigma = "small", "ε"     = "strong"},
                {index = 10, "σ"   = "small", "ε"     = "weak"},
            ]
        )"_toml;

        const auto g = mjolnir::read_lennard_jones_wall_potential<real_type>(v);

        BOOST_TEST(g.participants().size() ==  4u);
        BOOST_TEST(g.participants().at(0)  ==  0u);
        BOOST_TEST(g.participants().at(1)  ==  2u);
        BOOST_TEST(g.participants().at(2)  ==  9u);
        BOOST_TEST(g.participants().at(3)  == 10u);

        BOOST_TEST(g.parameters().at( 0).first  == real_type(10.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).first  == real_type(10.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).first  == real_type(0.01), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).first  == real_type(0.01), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 0).second == real_type(100.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).second == real_type(0.001), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).second == real_type(100.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).second == real_type(0.001), tolerance<real_type>());
        BOOST_TEST(g.cutoff() == real_type(3.14), tolerance<real_type>());
    }
}


#include <frovedis.hpp>
#include <frovedis/ml/graph/graph.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

template <class T>
void call_pagerank(const std::string& data_p,
                   const std::string& out_p,
                   bool if_prep,
                   double df,
                   double epsilon,
                   size_t niter,
                   double thr) {
  graph<T> gr;
  time_spent t(INFO);
  if(if_prep) gr = read_edgelist<T>(data_p);
  else {
    auto mat = make_crs_matrix_load<T>(data_p);
    gr = graph<T>(mat);
  }
  t.show("data loading time: ");
  //gr.debug_print();
  auto res = gr.pagerank(df, epsilon, niter, thr);
  t.show("pagerank computation time: ");
  make_dvector_scatter(res).saveline(out_p);
  std::cout << "ranks: "; debug_print_vector(res, 5);
}

int main(int argc, char* argv[]){
    frovedis::use_frovedis use(argc, argv);
    using namespace boost::program_options;

    options_description opt("option");
    opt.add_options()
        ("help,h", "produce help message")
        ("input,i" , value<std::string>(), "input data path containing either edgelist or adjacency matrix data")
        ("dtype,t" , value<std::string>(), "input data type (int, float or double) [default: int]")
        ("output,o" , value<std::string>(), "output data path to save ranking scores")
        ("dfactor,d", value<double>(), "damping factor (default: 0.15)")
        ("epsilon,e", value<double>(), "convergence threshold (default: 1E-4)")
        ("threshold,l", value<double>(), "threshold forn shrink version (default: 0.4)")
        ("max_iter,k", value<size_t>(), "maximum no. of iterations (default: 100)")
        ("verbose", "set loglevel to DEBUG")
        ("verbose2", "set loglevel to TRACE")
        ("prepare,p" , "whether to generate the CRS matrix from original edgelist file ");

    variables_map argmap;
    store(command_line_parser(argc,argv).options(opt).allow_unregistered().
          run(), argmap);
    notify(argmap);

    bool if_prep = 0; // true if prepare data from raw dataset
    std::string data_p, out_p, dtype = "int";
    size_t niter = 100;
    double epsilon = 1e-4;
    double df = 0.15;
    double thr = 0.4;

    if(argmap.count("help")){
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("input")){
      data_p = argmap["input"].as<std::string>();
    } else {
      std::cerr << "input path is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("output")){
      out_p = argmap["output"].as<std::string>();
    } else {
      std::cerr << "output path is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("prepare")){
      if_prep = true;
    }
    if(argmap.count("dtype")){
      dtype = argmap["dtype"].as<std::string>();
    }
    if(argmap.count("dfactor")){
       df = argmap["dfactor"].as<double>();
    }
    if(argmap.count("epsilon")){
       epsilon = argmap["epsilon"].as<double>();
    }
    if(argmap.count("threshold")){
       thr = argmap["threshold"].as<double>();
    }
    if(argmap.count("max_iter")){
       niter = argmap["max_iter"].as<size_t>();
    }
    if(argmap.count("verbose")){
      set_loglevel(DEBUG);
    }
    if(argmap.count("verbose2")){
      set_loglevel(TRACE);
    }

    try {
      if (dtype == "int") {
        call_pagerank<int>(data_p, out_p, if_prep, df, epsilon, niter, thr);
      }
      else if (dtype == "float") {
        call_pagerank<float>(data_p, out_p, if_prep, df, epsilon, niter, thr);
      }
      else if (dtype == "double") {
        call_pagerank<double>(data_p, out_p, if_prep, df, epsilon, niter, thr);
      }
      else {
        std::cerr << "Supported dtypes are only int, float and double!\n";
        std::cerr << opt << std::endl;
        exit(1);
      }
    }
    catch (std::exception& e) {
      std::cout << "exception caught: " << e.what() << std::endl;
    }
    return 0;
}


#include "ukf.h"
#include <Eigen/Dense>
#include <cmath>
using Eigen::MatrixXd;
using Eigen::VectorXd;

/**
 * Initializes Unscented Kalman filter
 */
UKF::UKF() {
  // if this is false, laser measurements will be ignored (except during init)
  use_laser_ = true;

  // if this is false, radar measurements will be ignored (except during init)
  use_radar_ = true;

  // initial state vector
  x_ = VectorXd(5);

  // initial covariance matrix
  P_ = MatrixXd(5, 5);

  // Process noise standard deviation longitudinal acceleration in m/s^2
  std_a_ = 2.0;

  // Process noise standard deviation yaw acceleration in rad/s^2
  std_yawdd_ = 1.0;

  /**
   * DO NOT MODIFY measurement noise values below.
   * These are provided by the sensor manufacturer.
   */

  // Laser measurement noise standard deviation position1 in m
  std_laspx_ = 0.15;

  // Laser measurement noise standard deviation position2 in m
  std_laspy_ = 0.15;

  // Radar measurement noise standard deviation radius in m
  std_radr_ = 0.3;

  // Radar measurement noise standard deviation angle in rad
  std_radphi_ = 0.03;

  // Radar measurement noise standard deviation radius change in m/s
  std_radrd_ = 0.3;

  /**
   * End DO NOT MODIFY section for measurement noise values
   */

  /**
   * TODO: Complete the initialization. See ukf.h for other member properties.
   * Hint: one or more values initialized above might be wildly off...
   */
  is_initialized_ = false;
  n_x_ = 5;
  n_aug_ = 7;
  lambda_ = 3 - n_x_;

  weights_ = VectorXd(2 * n_aug_ + 1);
  weights_.fill(1 / (2 * (lambda_ + n_aug_)));
  weights_(0) = lambda_ / (lambda_ + n_aug_);
}

UKF::~UKF() {}

void UKF::ProcessMeasurement(MeasurementPackage meas_package) {
  /**
   * TODO: Complete this function! Make sure you switch between lidar and radar
   * measurements.
   */
  if (!is_initialized_) {
    // set the state with the initial location and zero velocity
    // Must determine what sensor this is first
    if (meas_package.sensor_type_ == MeasurementPackage::SensorType::LASER) {
      x_ << meas_package.raw_measurements_[0],
          meas_package.raw_measurements_[1], 0, 0, 0;
      P_ << std_laspx_ * std_laspx_, 0, 0, 0, 0,
            0, std_laspy_ * std_laspy_, 0, 0, 0,
            0, 0, 1, 0, 0,
            0, 0, 0, 1, 0,
            0, 0, 0, 0, 1;
    } else if (meas_package.sensor_type_ ==
               MeasurementPackage::SensorType::RADAR) {
      const double rho = meas_package.raw_measurements_(0);
      const double phi = meas_package.raw_measurements_(1);
      const double rhodot = meas_package.raw_measurements_(2);
      const double x = rho * cos(phi);
      const double y = rho * sin(phi);
      const double vx = rhodot * cos(phi);
      const double vy = rhodot * sin(phi);
      const double v = rhodot; // std::sqrt(vx * vx + vy * vy);
      x_ << x, y, v, rho, rhodot;
      P_ << std_radr_* std_radr_, 0, 0, 0, 0,
            0, std_radr_ * std_radr_, 0, 0, 0,
            0, 0, std_radrd_ * std_radrd_, 0, 0,
            0, 0, 0, std_radphi_ * std_radphi_, 0,
            0, 0, 0, 0, std_radphi_ * std_radphi_;
    }

    time_us_ = meas_package.timestamp_;
    is_initialized_ = true;
    return;
  }

  // compute the time elapsed between the current and previous measurements
  // dt - expressed in seconds
  const float dt = (meas_package.timestamp_ - time_us_) / 1000000.0;
  time_us_ = meas_package.timestamp_;

  // Predict the next states and covariance matrix
  Prediction(dt);

  // Update the next states and covariance matrix
  if (meas_package.sensor_type_ == MeasurementPackage::SensorType::LASER) {
    UpdateLidar(meas_package);
  } else if (meas_package.sensor_type_ ==
             MeasurementPackage::SensorType::RADAR) {
    UpdateRadar(meas_package);
  }
}

void UKF::Prediction(double delta_t) {
  /**
   * TODO: Complete this function! Estimate the object's location.
   * Modify the state vector, x_. Predict sigma points, the state,
   * and the state covariance matrix.
   */

  // Step #1 - Create augmented mean vector, augmented state covariance
  VectorXd x_aug = VectorXd(n_aug_);
  x_aug.setZero(n_aug_);
  x_aug.head(5) = x_;

  MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);
  P_aug.setZero(n_aug_, n_aug_);
  P_aug.topLeftCorner(5, 5) = P_;
  P_aug.bottomRightCorner(2, 2) << std_a_ * std_a_, 0, 0,
      std_yawdd_ * std_yawdd_;

  // Step #2 - Create square root matrix
  MatrixXd A = P_aug.llt().matrixL();

  // Step #3 - Create augmented sigma points
  MatrixXd Xsig_aug = MatrixXd(n_aug_, 2 * n_aug_ + 1);
  Xsig_aug.col(0) = x_aug;
  Xsig_aug.middleCols(1, n_aug_) =
      (std::sqrt(lambda_ + n_aug_) * A.array()).colwise() + x_aug.array();
  Xsig_aug.middleCols(n_aug_ + 1, n_aug_) =
      (-std::sqrt(lambda_ + n_aug_) * A.array()).colwise() + x_aug.array();

  // Step #4 - Predict sigma points
  Xsig_pred_ = MatrixXd(n_x_, 2 * n_aug_ + 1);
  for (int i = 0; i < Xsig_aug.cols(); i++) {
    const float px = Xsig_aug(0, i);
    const float py = Xsig_aug(1, i);
    const float v = Xsig_aug(2, i);
    const float yaw = Xsig_aug(3, i);
    const float yaw_rate = Xsig_aug(4, i);
    const float nu_accel = Xsig_aug(5, i);
    const float nu_yaw_accel = Xsig_aug(6, i);
    VectorXd vec(n_x_);
    VectorXd noise_vec(n_x_);
    noise_vec << 0.5 * delta_t * delta_t * std::cos(yaw) * nu_accel,
        0.5 * delta_t * delta_t * std::sin(yaw) * nu_accel, delta_t * nu_accel,
        0.5 * delta_t * delta_t * nu_yaw_accel, delta_t * nu_yaw_accel;
    // avoid division by zero
    if (std::abs(yaw_rate) <= 1e-10) {
      vec << v * std::cos(yaw) * delta_t, v * std::sin(yaw) * delta_t, 0, 0, 0;
    } else {
      vec << (v / yaw_rate) *
                 (std::sin(yaw + yaw_rate * delta_t) - std::sin(yaw)),
          (v / yaw_rate) *
              (-std::cos(yaw + yaw_rate * delta_t) + std::cos(yaw)),
          0, yaw_rate * delta_t, 0;
    }
    // write predicted sigma points into right column
    Xsig_pred_.col(i) = Xsig_aug.col(i).head(n_x_) + vec + noise_vec;
  }

  // Step #5 - Now predict state mean and covariance
  x_ = Xsig_pred_ * weights_;

  P_ = (Xsig_pred_.array().colwise() - x_.array());
  P_ = P_ * weights_.asDiagonal() * P_.transpose();
}

void UKF::UpdateLidar(MeasurementPackage meas_package) {
  /**
   * TODO: Complete this function! Use lidar data to update the belief
   * about the object's position. Modify the state vector, x_, and
   * covariance, P_.
   * You can also calculate the lidar NIS, if desired.
   */
  // Note that the LiDAR noise profile is linear, so we can simply
  // use the standard linear Kalman filter here
  // Taken directly from the previous assignments

  // New - define measurement matrix
  MatrixXd H;
  H.setZero(2, n_x_);
  H(0, 0) = H(1, 1) = 1;  // Select out the position elements only
  VectorXd z_pred = H * x_;
  const VectorXd z = meas_package.raw_measurements_;

  // Calculate residual vector y
  const VectorXd y = z - z_pred;

  // New - define measurement noise matrix
  R_.setZero(2, 2);
  R_ << std_laspx_ * std_laspx_, 0, 0, std_laspy_ * std_laspy_;

  // Create innovation covariance matrix S
  MatrixXd S = H * P_ * H.transpose() + R_;

  // Create Kalman gain matrix K
  MatrixXd K = P_ * H.transpose() * S.inverse();

  // Create new estimate for states and covariance
  x_ = x_ + (K * y);
  MatrixXd I = MatrixXd::Identity(x_.size(), x_.size());
  P_ = (I - K * H) * P_;

  // Calculate NIS for LiDAR
  NIS_lidar_ = y.transpose() * S.inverse() * y;
}

void UKF::UpdateRadar(MeasurementPackage meas_package) {
  /**
   * TODO: Complete this function! Use radar data to update the belief
   * about the object's position. Modify the state vector, x_, and
   * covariance, P_.
   * You can also calculate the radar NIS, if desired.
   */
  // Step #1 - Create matrix for sigma points in measurement space
  MatrixXd Zsig = MatrixXd(3, 2 * n_aug_ + 1);

  // Step #2 - Transform sigma points into measurement space
  Zsig.row(0) = ((Xsig_pred_.row(0).array() * Xsig_pred_.row(0).array()) +
                 (Xsig_pred_.row(1).array() * Xsig_pred_.row(1).array()))
                    .sqrt();
  for (int i = 0; i < Zsig.cols(); i++) {
    Zsig(1, i) = std::atan2(Xsig_pred_(1, i), Xsig_pred_(0, i));
  }
  Zsig.row(2) = ((Xsig_pred_.row(0).array() * Xsig_pred_.row(2).array() *
                  Xsig_pred_.row(3).array().cos()) +
                 (Xsig_pred_.row(1).array() * Xsig_pred_.row(2).array() *
                  Xsig_pred_.row(3).array().sin())) /
                Zsig.row(0).array();

  // Step #3 - Create and run final update method
  // mean predicted measurement
  const int n_z = 3;
  VectorXd z_pred = VectorXd(n_z);

  // measurement covariance matrix S
  MatrixXd S = MatrixXd(n_z, n_z);

  // Step #4 - Calculate mean predicted measurement
  z_pred = Zsig * weights_;

  // Step #5 - Create measurement covariance matrix R and
  // calculate innovation covariance matrix S
  R_.setZero(n_z, n_z);
  R_ << std_radr_ * std_radr_, 0, 0, 0, std_radphi_ * std_radphi_, 0, 0, 0,
      std_radrd_ * std_radrd_;

  S = (Zsig.array().colwise() - z_pred.array());
  S = S * weights_.asDiagonal() * S.transpose() + R_;

  // Step #6 - Calculate Cross Correlation Matrix
  MatrixXd Tc = MatrixXd(n_x_, n_z);
  Tc = (Xsig_pred_.array().colwise() - x_.array()).matrix() *
       weights_.asDiagonal() *
       (Zsig.array().colwise() - z_pred.array()).matrix().transpose();

  // Step #7 - Calculate Kalman gain K
  const MatrixXd K = Tc * S.inverse();

  // Step #8 - Update state mean and covariance matrix
  const VectorXd z = meas_package.raw_measurements_;
  const VectorXd y = z - z_pred;
  x_ = x_ + (K * y);
  P_ = P_ - K * S * K.transpose();

  // Calculate NIS for Radar
  NIS_radar_ = y.transpose() * S.inverse() * y;
}


#pragma once
#include <QImage>
#include "maps/grid/MLSConfig.hpp"
#include "maps/grid/MLSMap.hpp"
#include <boost/archive/polymorphic_binary_oarchive.hpp>
#include <base/Eigen.hpp>
#include "StandaloneVisualizer.hpp"
#include <fstream>
#include <iostream>
#include <boost/lexical_cast.hpp>

using namespace ::maps::grid;

template<class MLSMap>
static void show_MLS(const MLSMap& mls)
{
    std::cout << "update finish" << std::endl;
    StandaloneVisualizer app;
    app.updateData(mls);

    while (app.wait(1000))
    {
    }
}

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

    if(argc <= 3)
    {
        std::cout << "Usage: heightmap_to_mls <input height map> <output file name> <max height>\n input = any grayscale image" << std::endl;
        exit(-1);
    }

    const double maxHeight = boost::lexical_cast<double>(argv[3]);
    QImage img(argv[1]);

    const int width = img.width();
    const int height = img.height();
    std::cout << "Image: width: " << width << ", height: " << height << std::endl;

    const Eigen::Vector2d res(0.1, 0.1);
    const Vector2ui numCells(width, height);

    MLSConfig mls_config;
//     mls_config.updateModel = MLSConfig::SLOPE;
    mls_config.updateModel = MLSConfig::KALMAN;
    mls_config.gapSize = 0.3f;
    mls_config.useNegativeInformation = false;
    MLSMapKalman mls(numCells, res, mls_config);
//
    for(int x = 0; x < width; ++x)
    {
        for(int y = 0; y < height; ++y)
        {
            const QRgb pixel = img.pixel(x, y);
            const int brightness = qGray(pixel);
            const double topHeight = maxHeight * brightness / 255.0;

            for(double height = topHeight; height > -0.3; height -= 0.2)
            {
                base::Vector3d point(res.x() / 2.0 +  x * res.x(), res.y() / 2.0 + y * res.y(), height);
                mls.mergePoint(point);
            }
        }
    }

    std::ofstream of(argv[2], std::ios::binary);
    boost::archive::binary_oarchive oa(of);
    oa << mls;
    of.flush();

    show_MLS(mls);

    return 0;
}


/*
 *  Created on: Aug, 2015
 *      Author: Thomas Faeulhammer
 *
 */

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <fstream>

namespace po = boost::program_options;

Eigen::Matrix4f readMatrixFromFile(const std::string &file, int padding = 0);
Eigen::Matrix4f RotTrans2Mat4f(const Eigen::Quaternionf &q, const Eigen::Vector3f &trans);
Eigen::Matrix4f RotTrans2Mat4f(const Eigen::Quaternionf &q, const Eigen::Vector4f &trans);

Eigen::Matrix4f
readMatrixFromFile(const std::string &file, int padding)
{

    // check if file exists
    boost::filesystem::path path = file;
    if ( ! (boost::filesystem::exists ( path ) && boost::filesystem::is_regular_file(path)) )
        throw std::runtime_error ("Given file path to read Matrix does not exist!");

    std::ifstream in (file.c_str (), std::ifstream::in);

    char linebuf[1024];
    in.getline (linebuf, 1024);
    std::string line (linebuf);
    std::vector < std::string > strs_2;
    boost::split (strs_2, line, boost::is_any_of (" "));

    Eigen::Matrix4f matrix;
    for (int i = 0; i < 16; i++)
        matrix (i / 4, i % 4) = static_cast<float> (atof (strs_2[padding+i].c_str ()));

    return matrix;
}

Eigen::Matrix4f
RotTrans2Mat4f(const Eigen::Quaternionf &q, const Eigen::Vector4f &trans)
{
    Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();
    tf.block<3,3>(0,0) = q.normalized().toRotationMatrix();
    tf.block<4,1>(0,3) = trans;
    return tf;
}

int main (int argc, char ** argv)
{
    typedef pcl::PointXYZRGB PointT;

    std::string path_s, path_p, path_m;
    po::options_description desc("Visualize scene with object hypotheses in a given pose\n======================================\n**Required options");
    desc.add_options()
            ("help,h", "produce help message")
            ("scene_path,s", po::value<std::string>(&path_s)->required(), "Filename of the point cloud of the scene")
            ("model_path,m", po::value<std::string>(&path_m)->required(), "Filename of the point cloud of the model (e.g. 3D_model.pcd)")
            ("model_pose,p", po::value<std::string>(&path_p)->required(), "Filename of the 4x4 object pose stored in row-major order")
            ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) { std::cout << desc << std::endl; return false; }
    try  { po::notify(vm); }
    catch(std::exception& e)  {  std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl; return false; }

    pcl::visualization::PCLVisualizer vis;

    pcl::PointCloud<PointT>::Ptr scene_cloud (new pcl::PointCloud<PointT>);
    pcl::io::loadPCDFile (path_s, *scene_cloud);

    pcl::PointCloud<PointT>::Ptr model_cloud (new pcl::PointCloud<PointT>);
    pcl::io::loadPCDFile (path_m, *model_cloud);

    const Eigen::Matrix4f transform = readMatrixFromFile(path_p);
    const Eigen::Matrix4f scene_tf = RotTrans2Mat4f(scene_cloud->sensor_orientation_, scene_cloud->sensor_origin_);
    pcl::transformPointCloud(*model_cloud, *model_cloud,  scene_tf * transform);

    vis.addPointCloud (scene_cloud, "original_cloud");
    vis.addPointCloud (model_cloud, "model_cloud");
    vis.spin();
    return true;
}


/*
 * A c++ version of (significant pieces of) the QPController.m mimoOutput method.
 *
 * Todo:
 *   switch to spatial accelerations in motion constraints
 *   use fixed-size matrices (or at least pre-allocated)
 *       for instance: #define nq
 *       set MaxRowsAtCompileTime (http://eigen.tuxfamily.org/dox/TutorialMatrixClass.html)
 *   some matrices might be better off using RowMajor
 */

#include "QPCommon.h"
#include <Eigen/StdVector>
#include <limits>
#include <cmath>
#include "drakeMexUtil.h"
#include "controlMexUtil.h"

//#define TEST_FAST_QP
//#define USE_MATRIX_INVERSION_LEMMA

using namespace std;
using namespace Eigen;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  int error;
  if (nrhs<1) mexErrMsgTxt("usage: ptr = QPControllermex(0,control_obj,robot_obj,...); alpha=QPControllermex(ptr,...,...)");
  if (nlhs<1) mexErrMsgTxt("take at least one output... please.");

  struct QPControllerData* pdata;
  mxArray* pm;
  double* pr;
  int i,j;

  if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
    pdata = new struct QPControllerData;

    // get control object properties
    const mxArray* pobj = prhs[1];

    pm = myGetProperty(pobj,"slack_limit");
    pdata->slack_limit = mxGetScalar(pm);

    pm = myGetProperty(pobj,"W_kdot");
    assert(mxGetM(pm)==3); assert(mxGetN(pm)==3);
    pdata->W_kdot.resize(mxGetM(pm),mxGetN(pm));
    memcpy(pdata->W_kdot.data(),mxGetPrSafe(pm),sizeof(double)*mxGetM(pm)*mxGetN(pm));

    pm= myGetProperty(pobj,"w_grf");
    pdata->w_grf = mxGetScalar(pm);

    pm= myGetProperty(pobj,"w_slack");
    pdata->w_slack = mxGetScalar(pm);

    pm = myGetProperty(pobj,"Kp_ang");
    pdata->Kp_ang = mxGetScalar(pm);

    pm = myGetProperty(pobj,"Kp_accel");
    pdata->Kp_accel = mxGetScalar(pm);

    pm= myGetProperty(pobj,"n_body_accel_inputs");
    pdata->n_body_accel_inputs = (int) mxGetScalar(pm);

    pm= myGetProperty(pobj,"n_body_accel_bounds");
    pdata->n_body_accel_bounds = (int) mxGetScalar(pm);

    mxArray* body_accel_bounds = myGetProperty(pobj,"body_accel_bounds");
    Vector6d vecbound;
    for (int i=0; i<pdata->n_body_accel_bounds; i++) {
      pdata->accel_bound_body_idx.push_back((int) mxGetScalar(mxGetField(body_accel_bounds,i,"body_idx"))-1);
      pm = mxGetField(body_accel_bounds,i,"min_acceleration");

      assert(mxGetM(pm)==6); assert(mxGetN(pm)==1);
      memcpy(vecbound.data(),mxGetPrSafe(pm),sizeof(double)*6);
      pdata->min_body_acceleration.push_back(vecbound);
      pm = mxGetField(body_accel_bounds,i,"max_acceleration");
      assert(mxGetM(pm)==6); assert(mxGetN(pm)==1);
      memcpy(vecbound.data(),mxGetPrSafe(pm),sizeof(double)*6);
      pdata->max_body_acceleration.push_back(vecbound);
    }

    pm = myGetProperty(pobj,"body_accel_input_weights");
    pdata->body_accel_input_weights.resize(pdata->n_body_accel_inputs);
    memcpy(pdata->body_accel_input_weights.data(),mxGetPrSafe(pm),sizeof(double)*pdata->n_body_accel_inputs);

    pdata->n_body_accel_eq_constraints = 0;
    for (int i=0; i<pdata->n_body_accel_inputs; i++) {
      if (pdata->body_accel_input_weights(i) < 0)
        pdata->n_body_accel_eq_constraints++;
    }

    // get robot mex model ptr
    if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
      mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the third argument should be the robot mex ptr");
    memcpy(&(pdata->r),mxGetData(prhs[2]),sizeof(pdata->r));

    pdata->B.resize(mxGetM(prhs[3]),mxGetN(prhs[3]));
    memcpy(pdata->B.data(),mxGetPrSafe(prhs[3]),sizeof(double)*mxGetM(prhs[3])*mxGetN(prhs[3]));

    int nq = pdata->r->num_positions, nu = pdata->B.cols();

    pm = myGetProperty(pobj,"w_qdd");
    pdata->w_qdd.resize(nq);
    memcpy(pdata->w_qdd.data(),mxGetPrSafe(pm),sizeof(double)*nq);

    pdata->umin.resize(nu);
    pdata->umax.resize(nu);
    memcpy(pdata->umin.data(),mxGetPrSafe(prhs[4]),sizeof(double)*nu);
    memcpy(pdata->umax.data(),mxGetPrSafe(prhs[5]),sizeof(double)*nu);

    pdata->B_act.resize(nu,nu);
    pdata->B_act = pdata->B.bottomRows(nu);

     // get the map ptr back from matlab
     if (!mxIsNumeric(prhs[6]) || mxGetNumberOfElements(prhs[6])!=1)
     mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the seventh argument should be the map ptr");
     memcpy(&pdata->map_ptr,mxGetPrSafe(prhs[6]),sizeof(pdata->map_ptr));

//    pdata->map_ptr = NULL;
    if (!pdata->map_ptr)
      mexWarnMsgTxt("Map ptr is NULL.  Assuming flat terrain at z=0");

    // create gurobi environment
    error = GRBloadenv(&(pdata->env),NULL);

    // set solver params (http://www.gurobi.com/documentation/5.5/reference-manual/node798#sec:Parameters)
    mxArray* psolveropts = myGetProperty(pobj,"gurobi_options");
    int method = (int) mxGetScalar(myGetField(psolveropts,"method"));
    CGE ( GRBsetintparam(pdata->env,"outputflag",0), pdata->env );
    CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
    // CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
    CGE ( GRBsetintparam(pdata->env,"presolve",0), pdata->env );
    if (method==2) {
      CGE ( GRBsetintparam(pdata->env,"bariterlimit",20), pdata->env );
      CGE ( GRBsetintparam(pdata->env,"barhomogeneous",0), pdata->env );
      CGE ( GRBsetdblparam(pdata->env,"barconvtol",0.0005), pdata->env );
    }

    mxClassID cid;
    if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
    else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
    else mexErrMsgIdAndTxt("Drake:constructModelmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");

    plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
    memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));

    // preallocate some memory
    pdata->H.resize(nq,nq);
    pdata->H_float.resize(6,nq);
    pdata->H_act.resize(nu,nq);

    pdata->C.resize(nq);
    pdata->C_float.resize(6);
    pdata->C_act.resize(nu);

    pdata->J.resize(3,nq);
    pdata->J_xy.resize(2,nq);
    pdata->Hqp.resize(nq,nq);
    pdata->fqp.resize(nq);
    pdata->Ag.resize(6,nq);
    pdata->Ak.resize(3,nq);

    pdata->vbasis_len = 0;
    pdata->cbasis_len = 0;
    pdata->vbasis = NULL;
    pdata->cbasis = NULL;
    return;
  }

  // first get the ptr back from matlab
  if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
    mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the first argument should be the ptr");
  memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));

//  for (i=0; i<pdata->r->num_bodies; i++)
//    mexPrintf("body %d (%s) has %d contact points\n", i, pdata->r->bodies[i].linkname.c_str(), pdata->r->bodies[i].contact_pts.cols());

  int nu = pdata->B.cols(), nq = pdata->r->num_positions, nv = pdata->r->num_velocities;
  const int dim = 3, // 3D
  nd = 2*m_surface_tangents; // for friction cone approx, hard coded for now

  assert(nu+6 == nq);

  int narg=1;

  int use_fast_qp = (int) mxGetScalar(prhs[narg++]);

  Map< VectorXd > qddot_des(mxGetPrSafe(prhs[narg++]),nq);

  Map<VectorXd> q(mxGetPrSafe(prhs[narg]), nq);
  Map<VectorXd> qd(mxGetPrSafe(prhs[narg++]) + nq, nv);

  vector<VectorXd,aligned_allocator<VectorXd>> body_accel_inputs;
  for (int i=0; i<pdata->n_body_accel_inputs; i++) {
    assert(mxGetM(prhs[narg])==7); assert(mxGetN(prhs[narg])==1);
    VectorXd v = VectorXd::Zero(7,1);
    memcpy(v.data(),mxGetPrSafe(prhs[narg++]),sizeof(double)*7);
    body_accel_inputs.push_back(v);
  }

  int num_condof;
  VectorXd condof;
  if (!mxIsEmpty(prhs[narg])) {
    assert(mxGetN(prhs[narg])==1);
    num_condof=mxGetM(prhs[narg]);
    condof = VectorXd::Zero(num_condof);
    memcpy(condof.data(),mxGetPrSafe(prhs[narg++]),sizeof(double)*num_condof);
  }
  else {
    num_condof=0;
    narg++; // skip over empty vector
  }

  int desired_support_argid = narg++;

  Map<MatrixXd> A_ls(mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
  Map<MatrixXd> B_ls(mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
  Map<MatrixXd> Qy  (mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
  Map<MatrixXd> R_ls(mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
  Map<MatrixXd> C_ls(mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
  Map<MatrixXd> D_ls(mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
  Map<MatrixXd> S   (mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;

  Map<VectorXd> s1(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
  Map<VectorXd> s1dot(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
  double s2dot = mxGetScalar(prhs[narg++]);
  Map<VectorXd> x0(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
  Map<VectorXd> u0(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
  Map<VectorXd> y0(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
  Map<VectorXd> qdd_lb(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
  Map<VectorXd> qdd_ub(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
  memcpy(pdata->w_qdd.data(),mxGetPrSafe(prhs[narg++]),sizeof(double)*nq);

  double mu = mxGetScalar(prhs[narg++]);

  MatrixXd R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls;

  KinematicsCache<double> cache = pdata->r->doKinematics(q, qd);

  //---------------------------------------------------------------------
  // Compute active support from desired supports -----------------------
  MatrixXd all_body_contact_pts;

  vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>> active_supports;
  set<int> contact_bodies; // redundant, clean up later
  int num_active_contact_pts=0;
  if (!mxIsEmpty(prhs[desired_support_argid])) {
    VectorXd phi;
    mxArray* mxBodies = myGetField(prhs[desired_support_argid],"bodies");
    if (!mxBodies) mexErrMsgTxt("couldn't get bodies");
    double* pBodies = mxGetPrSafe(mxBodies);
    mxArray* mxContactPts = myGetField(prhs[desired_support_argid],"contact_pts");
    if (!mxContactPts) mexErrMsgTxt("couldn't get contact points");

    for (i=0; i<mxGetNumberOfElements(mxBodies);i++) {
      mxArray* mxBodyContactPts = mxGetCell(mxContactPts,i);
      assert(mxGetM(mxBodyContactPts) == 3);
      int nc = static_cast<int>(mxGetN(mxBodyContactPts));
      if (nc<1) continue;

      all_body_contact_pts.resize(mxGetM(mxBodyContactPts),mxGetN(mxBodyContactPts));
      pr = mxGetPrSafe(mxBodyContactPts);
      memcpy(all_body_contact_pts.data(),pr,sizeof(double)*mxGetM(mxBodyContactPts)*mxGetN(mxBodyContactPts));

      SupportStateElement se;
      se.body_idx = (int) pBodies[i]-1;
      for (j=0; j<nc; j++) {
        se.contact_pts.push_back(all_body_contact_pts.col(j));
      }

      active_supports.push_back(se);
      num_active_contact_pts += nc;
      contact_bodies.insert((int)se.body_idx);
    }
  }

  std::map<int, std::unique_ptr<GradientVar<double, TWIST_SIZE, 1>> > f_ext;
  pdata->H = pdata->r->massMatrix(cache).value();
  pdata->C = pdata->r->inverseDynamics(cache, f_ext).value();

  pdata->H_float = pdata->H.topRows(6);
  pdata->H_act = pdata->H.bottomRows(nu);
  pdata->C_float = pdata->C.head<6>();
  pdata->C_act = pdata->C.tail(nu);

  bool include_angular_momentum = (pdata->W_kdot.array().maxCoeff() > 1e-10);

  if (include_angular_momentum) {
    pdata->Ag = pdata->r->centroidalMomentumMatrix(cache, 0).value();
    pdata->Agdot_times_v = pdata->r->centroidalMomentumMatrixDotTimesV(cache, 0).value();
    pdata->Ak = pdata->Ag.topRows<3>();
    pdata->Akdot_times_v = pdata->Agdot_times_v.topRows<3>();
  }

  // consider making all J's into row-major

  auto com_gradientvar = pdata->r->centerOfMass(cache, 1);
  Vector3d xcom = com_gradientvar.value();
  pdata->J = com_gradientvar.gradient().value();
  pdata->Jdotv = pdata->r->centerOfMassJacobianDotTimesV(cache, 0).value();
  pdata->J_xy = pdata->J.topRows(2);
  pdata->Jdotv_xy = pdata->Jdotv.head<2>();

  MatrixXd Jcom;
  VectorXd Jcomdotv;

  if (x0.size()==6) {
    Jcom = pdata->J;
    Jcomdotv = pdata->Jdotv;
  }
  else {
    Jcom = pdata->J_xy;
    Jcomdotv = pdata->Jdotv_xy;
  }

  MatrixXd B,JB,Jp,normals;
  VectorXd Jpdotv;
  std::vector<double> support_mus(active_supports.size(), mu);
  int nc = contactConstraintsBV(pdata->r, cache, num_active_contact_pts, support_mus, active_supports, B, JB, Jp, Jpdotv, normals);
  int neps = nc*dim;

  VectorXd x_bar,xlimp;
  MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols());
  if (nc>0) {
    if (x0.size()==6) {
      // x,y,z com
      xlimp.resize(6);
      xlimp.topRows(3) = xcom;
      xlimp.bottomRows(3) = Jcom*qd;
    }
    else {
      xlimp.resize(4);
      xlimp.topRows(2) = xcom.topRows(2);
      xlimp.bottomRows(2) = Jcom*qd;
    }
    x_bar = xlimp-x0;

    D_float = JB.topRows(6);
    D_act = JB.bottomRows(nu);
  }

  int nf = nc*nd; // number of contact force variables
  int nparams = nq+nf+neps;

  Vector3d kdot_des;
  if (include_angular_momentum) {
    VectorXd k = pdata->Ak*qd;
    kdot_des = -pdata->Kp_ang*k; // TODO: parameterize
  }

  //----------------------------------------------------------------------
  // QP cost function ----------------------------------------------------
  //
  //  min: ybar*Qy*ybar + ubar*R*ubar + (2*S*xbar + s1)*(A*x + B*u) +
  //    w_qdd*quad(qddot_ref - qdd) + w_eps*quad(epsilon) +
  //    w_grf*quad(beta) + quad(kdot_des - (A*qdd + Adot*qd))
  VectorXd f(nparams);
  {
    if (nc > 0) {
      // NOTE: moved Hqp calcs below, because I compute the inverse directly for FastQP (and sparse Hqp for gurobi)
      VectorXd tmp = C_ls*xlimp;
      VectorXd tmp1 = Jcomdotv;
      MatrixXd tmp2 = R_DQyD_ls*Jcom;

      pdata->fqp = tmp.transpose()*Qy*D_ls*Jcom;
      pdata->fqp += tmp1.transpose()*tmp2;
      pdata->fqp += (S*x_bar + 0.5*s1).transpose()*B_ls*Jcom;
      pdata->fqp -= u0.transpose()*tmp2;
      pdata->fqp -= y0.transpose()*Qy*D_ls*Jcom;
      pdata->fqp -= (pdata->w_qdd.array()*qddot_des.array()).matrix().transpose();
      if (include_angular_momentum) {
        pdata->fqp += pdata->Akdot_times_v.transpose() * pdata->W_kdot * pdata->Ak;
        pdata->fqp -= kdot_des.transpose()*pdata->W_kdot*pdata->Ak;
      }
      f.head(nq) = pdata->fqp.transpose();
     } else {
      f.head(nq) = -qddot_des;
    }
  }
  f.tail(nf+neps) = VectorXd::Zero(nf+neps);

  int neq = 6+neps+6*pdata->n_body_accel_eq_constraints+num_condof;
  MatrixXd Aeq = MatrixXd::Zero(neq,nparams);
  VectorXd beq = VectorXd::Zero(neq);

  // constrained floating base dynamics
  //  H_float*qdd - J_float'*lambda - Dbar_float*beta = -C_float
  Aeq.topLeftCorner(6,nq) = pdata->H_float;
  beq.topRows(6) = -pdata->C_float;

  if (nc>0) {
    Aeq.block(0,nq,6,nc*nd) = -D_float;
  }

  if (nc > 0) {
    // relative acceleration constraint
    Aeq.block(6,0,neps,nq) = Jp;
    Aeq.block(6,nq,neps,nf) = MatrixXd::Zero(neps,nf);  // note: obvious sparsity here
    Aeq.block(6,nq+nf,neps,neps) = MatrixXd::Identity(neps,neps);             // note: obvious sparsity here
    beq.segment(6,neps) = -Jpdotv -pdata->Kp_accel*Jp*qd;
  }

  // add in body spatial equality constraints
  VectorXd body_vdot;
  int body_idx;
  int equality_ind = 6+neps;
  Vector3d origin = Vector3d::Zero();
  for (int i=0; i<pdata->n_body_accel_inputs; i++) {
    if (pdata->body_accel_input_weights(i) < 0) {
      // negative implies constraint
      body_vdot = body_accel_inputs[i].bottomRows(6);
      body_idx = (int)(body_accel_inputs[i][0])-1;

      if (!inSupport(active_supports,body_idx)) {
        auto Jb = pdata->r->forwardKinJacobian(cache, origin, body_idx, 0, 1, false, 0).value();
        auto Jbdot_times_v = pdata->r->forwardJacDotTimesV(cache, origin, body_idx, 0, 1, 0).value();

        for (int j=0; j<6; j++) {
          if (!std::isnan(body_vdot[j])) {
            Aeq.block(equality_ind,0,1,nq) = Jb.row(j);
            beq[equality_ind++] = -Jbdot_times_v(j) + body_vdot[j];
          }
        }
      }
    }
  }

  if (num_condof>0) {
    // add joint acceleration constraints
    for (int i=0; i<num_condof; i++) {
      Aeq(equality_ind,(int)condof[i]-1) = 1;
      beq[equality_ind++] = qddot_des[(int)condof[i]-1];
    }
  }

  int n_ineq = 2*nu+2*6*pdata->n_body_accel_bounds;
  MatrixXd Ain = MatrixXd::Zero(n_ineq,nparams);  // note: obvious sparsity here
  VectorXd bin = VectorXd::Zero(n_ineq);

  // linear input saturation constraints
  // u=B_act'*(H_act*qdd + C_act - Jz_act'*z - Dbar_act*beta)
  // using transpose instead of inverse because B is orthogonal
  Ain.topLeftCorner(nu,nq) = pdata->B_act.transpose()*pdata->H_act;
  Ain.block(0,nq,nu,nc*nd) = -pdata->B_act.transpose()*D_act;
  bin.head(nu) = -pdata->B_act.transpose()*pdata->C_act + pdata->umax;

  Ain.block(nu,0,nu,nparams) = -1*Ain.block(0,0,nu,nparams);
  bin.segment(nu,nu) = pdata->B_act.transpose()*pdata->C_act - pdata->umin;

  int body_index;
  int constraint_start_index = 2*nu;
  for (int i=0; i<pdata->n_body_accel_bounds; i++) {
    body_index = pdata->accel_bound_body_idx[i];
    auto Jb = pdata->r->forwardKinJacobian(cache, origin, body_index, 0, 1, false, 0).value();
    auto Jbdot_times_v = pdata->r->forwardJacDotTimesV(cache, origin, body_index, 0, 1, 0).value();

    Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = Jb;
    bin.segment(constraint_start_index,6) = -Jbdot_times_v + pdata->max_body_acceleration[i];
    constraint_start_index += 6;
    Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = -Jb;
    bin.segment(constraint_start_index,6) = Jbdot_times_v - pdata->min_body_acceleration[i];
    constraint_start_index += 6;
  }

  for (int i=0; i<n_ineq; i++) {
    // remove inf constraints---needed by gurobi
    if (std::isinf(double(bin(i)))) {
      Ain.row(i) = 0*Ain.row(i);
      bin(i)=0;
    }
  }

  GRBmodel * model = NULL;
  int info=-1;

  // set obj,lb,up
  VectorXd lb(nparams), ub(nparams);
  lb.head(nq) = qdd_lb;
  ub.head(nq) = qdd_ub;
  lb.segment(nq,nf) = VectorXd::Zero(nf);
  ub.segment(nq,nf) = 1e3*VectorXd::Ones(nf);
  lb.tail(neps) = -pdata->slack_limit*VectorXd::Ones(neps);
  ub.tail(neps) = pdata->slack_limit*VectorXd::Ones(neps);

  VectorXd alpha(nparams);

  MatrixXd Qnfdiag(nf,1), Qneps(neps,1);
  vector<MatrixXd*> QBlkDiag( nc>0 ? 3 : 1 );  // nq, nf, neps   // this one is for gurobi

  VectorXd w = (pdata->w_qdd.array() + REG).matrix();
  #ifdef USE_MATRIX_INVERSION_LEMMA
  bool include_body_accel_cost_terms = pdata->n_body_accel_inputs > 0 && pdata->body_accel_input_weights.array().maxCoeff() > 1e-10;
  if (use_fast_qp > 0 && !include_angular_momentum && !include_body_accel_cost_terms)
  {
    // TODO: update to include angular momentum, body accel objectives.

  	//    We want Hqp inverse, which I can compute efficiently using the
  	//    matrix inversion lemma (see wikipedia):
  	//    inv(A + U'CV) = inv(A) - inv(A)*U* inv([ inv(C)+ V*inv(A)*U ]) V inv(A)
  	if (nc>0) {
      MatrixXd Wi = ((1/(pdata->w_qdd.array() + REG)).matrix()).asDiagonal();
  		if (R_DQyD_ls.trace()>1e-15) { // R_DQyD_ls is not zero
  			pdata->Hqp = Wi - Wi*Jcom.transpose()*(R_DQyD_ls.inverse() + Jcom*Wi*Jcom.transpose()).inverse()*Jcom*Wi;
      }
  	}
    else {
    	pdata->Hqp = MatrixXd::Constant(nq,1,1/(1+REG));
  	}

	  #ifdef TEST_FAST_QP
  	  if (nc>0) {
        MatrixXd Hqp_test(nq,nq);
        MatrixXd W = w.asDiagonal();
        Hqp_test = (Jcom.transpose()*R_DQyD_ls*Jcom + W).inverse();
    	  if (((Hqp_test-pdata->Hqp).array().abs()).maxCoeff() > 1e-6) {
    		  mexErrMsgTxt("Q submatrix inverse from matrix inversion lemma does not match direct Q inverse.");
        }
      }
	  #endif

    Qnfdiag = MatrixXd::Constant(nf,1,1/REG);
    Qneps = MatrixXd::Constant(neps,1,1/(.001+REG));

    QBlkDiag[0] = &pdata->Hqp;
    if (nc>0) {
    	QBlkDiag[1] = &Qnfdiag;
    	QBlkDiag[2] = &Qneps;     // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
    }

    MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
    VectorXd bin_lb_ub(n_ineq+2*nparams);
    Ain_lb_ub << Ain, 			     // note: obvious sparsity here
    		-MatrixXd::Identity(nparams,nparams),
    		MatrixXd::Identity(nparams,nparams);
    bin_lb_ub << bin, -lb, ub;

    info = fastQPThatTakesQinv(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->active, alpha);

    //if (info<0)  	mexPrintf("fastQP info = %d.  Calling gurobi.\n", info);
  }
  else {
  #endif

    if (nc>0) {
      pdata->Hqp = Jcom.transpose()*R_DQyD_ls*Jcom;
      if (include_angular_momentum) {
        pdata->Hqp += pdata->Ak.transpose()*pdata->W_kdot*pdata->Ak;
      }
      pdata->Hqp += pdata->w_qdd.asDiagonal();
      pdata->Hqp += REG*MatrixXd::Identity(nq,nq);
    } else {
      pdata->Hqp = (1+REG)*MatrixXd::Identity(nq,nq);
    }

    // add in body spatial acceleration cost terms
    double w_i;
    for (int i=0; i<pdata->n_body_accel_inputs; i++) {
      w_i=pdata->body_accel_input_weights(i);
      if (w_i > 0) {
        body_vdot = body_accel_inputs[i].bottomRows(6);
        body_idx = (int)(body_accel_inputs[i][0])-1;

        if (!inSupport(active_supports,body_idx)) {
          auto Jb = pdata->r->forwardKinJacobian(cache, origin, body_idx, 0, 1, false, 0).value();
          auto Jbdot_times_v = pdata->r->forwardJacDotTimesV(cache, origin, body_idx, 0, 1, 0).value();

          for (int j=0; j<6; j++) {
            if (!std::isnan(body_vdot[j])) {
              pdata->Hqp += w_i*(Jb.row(j)).transpose()*Jb.row(j);
              f.head(nq) += w_i*(Jbdot_times_v(j) - body_vdot[j])*Jb.row(j).transpose();
            }
          }
        }
      }
    }

    Qnfdiag = MatrixXd::Constant(nf,1,pdata->w_grf+REG);
    Qneps = MatrixXd::Constant(neps,1,pdata->w_slack+REG);

    QBlkDiag[0] = &pdata->Hqp;
    if (nc>0) {
      QBlkDiag[1] = &Qnfdiag;
      QBlkDiag[2] = &Qneps;     // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
    }

    MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
    VectorXd bin_lb_ub(n_ineq+2*nparams);
    Ain_lb_ub << Ain,            // note: obvious sparsity here
        -MatrixXd::Identity(nparams,nparams),
        MatrixXd::Identity(nparams,nparams);
    bin_lb_ub << bin, -lb, ub;

    if (use_fast_qp > 0)
    { // set up and call fastqp
      info = fastQP(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->active, alpha);
      //if (info<0)    mexPrintf("fastQP info=%d... calling Gurobi.\n", info);
    }
    else {
      // use gurobi active set
      model = gurobiActiveSetQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->vbasis,pdata->vbasis_len,pdata->cbasis,pdata->cbasis_len,alpha);
      CGE(GRBgetintattr(model,"NumVars",&pdata->vbasis_len), pdata->env);
      CGE(GRBgetintattr(model,"NumConstrs",&pdata->cbasis_len), pdata->env);
      info=66;
      //info = -1;
    }

    if (info<0) {
      model = gurobiQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->active,alpha);
      int status; CGE(GRBgetintattr(model, "Status", &status), pdata->env);
      //if (status!=2) mexPrintf("Gurobi reports non-optimal status = %d\n", status);
    }
  #ifdef USE_MATRIX_INVERSION_LEMMA
  }
  #endif

  //----------------------------------------------------------------------
  // Solve for inputs ----------------------------------------------------
  VectorXd y(nu);
  VectorXd qdd = alpha.head(nq);
  VectorXd beta = alpha.segment(nq,nc*nd);

  // use transpose because B_act is orthogonal
  y = pdata->B_act.transpose()*(pdata->H_act*qdd + pdata->C_act - D_act*beta);
  //y = pdata->B_act.jacobiSvd(ComputeThinU|ComputeThinV).solve(pdata->H_act*qdd + pdata->C_act - Jz_act.transpose()*lambda - D_act*beta);

  if (nlhs>0) {
    plhs[0] = eigenToMatlab(y);
  }

  if (nlhs>1) {
    plhs[1] = eigenToMatlab(qdd);
  }

  if (nlhs>2) {
    plhs[2] = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
    memcpy(mxGetData(plhs[2]),&info,sizeof(int));
  }

  if (nlhs>3) {
      plhs[3] = mxCreateDoubleMatrix(1,active_supports.size(),mxREAL);
      pr = mxGetPrSafe(plhs[3]);
      int i=0;
      for (vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
          pr[i++] = (double) (iter->body_idx + 1);
      }
  }

  if (nlhs>4) {
    plhs[4] = eigenToMatlab(alpha);
  }

  if (nlhs>5) {
    plhs[5] = eigenToMatlab(pdata->Hqp);
  }

  if (nlhs>6) {
    plhs[6] = eigenToMatlab(f);
  }

  if (nlhs>7) {
    plhs[7] = eigenToMatlab(Aeq);
  }

  if (nlhs>8) {
    plhs[8] = eigenToMatlab(beq);
  }

  if (nlhs>9) {
    plhs[9] = eigenToMatlab(Ain_lb_ub);
  }

  if (nlhs>10) {
    plhs[10] = eigenToMatlab(bin_lb_ub);
  }

  if (nlhs>11) {
    plhs[11] = eigenToMatlab(Qnfdiag);
  }

  if (nlhs>12) {
    plhs[12] = eigenToMatlab(Qneps);
  }

  if (nlhs>13) {
    double Vdot;
    if (nc>0)
      // note: Sdot is 0 for ZMP/double integrator dynamics, so we omit that term here
      Vdot = ((2*x_bar.transpose()*S + s1.transpose())*(A_ls*x_bar + B_ls*(Jcomdotv + Jcom*qdd)) + s1dot.transpose()*x_bar)(0) + s2dot;
    else
      Vdot = 0;
    plhs[13] = mxCreateDoubleScalar(Vdot);
  }

  if (nlhs>14) {
    RigidBodyManipulator* r = pdata->r;

    VectorXd individual_cops = individualSupportCOPs(r, cache, active_supports, normals, B, beta);
    plhs[14] = eigenToMatlab(individual_cops);
  }

  if (model) {
    GRBfreemodel(model);
  }
  //  GRBfreeenv(env);
}


/**
 * \ file AttackReleaseFilter.cpp
 */

#include <ATK/Dynamic/AttackReleaseFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 1024*64;

BOOST_AUTO_TEST_CASE( AttackRelease_attack_test )
{
  ATK::AttackReleaseFilter<double> filter;
  filter.set_attack(0.5);
  BOOST_CHECK_EQUAL(filter.get_attack(), 0.5);
}

BOOST_AUTO_TEST_CASE( AttackRelease_attack_range_test )
{
  ATK::AttackReleaseFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_attack(-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackRelease_attack_range2_test )
{
  ATK::AttackReleaseFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_attack(1.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackRelease_release_test )
{
  ATK::AttackReleaseFilter<double> filter;
  filter.set_release(0.5);
  BOOST_CHECK_EQUAL(filter.get_release(), 0.5);
}

BOOST_AUTO_TEST_CASE( AttackRelease_release_range_test )
{
  ATK::AttackReleaseFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_release(-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackRelease_release_range2_test )
{
  ATK::AttackReleaseFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_release(1.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackReleaseFilter_triangle_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[i] = i / 48000;
  }
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[PROCESSSIZE/2 + i] = (PROCESSSIZE/2 - i) / 48000;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::AttackReleaseFilter<double> filter(1);
  filter.set_attack(std::exp(-1./(48000 * 1e-3)));
  filter.set_release(std::exp(-1./(48000 * 100e-3)));
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    BOOST_REQUIRE_GE(data[i], outdata[i]);
  }
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    BOOST_REQUIRE_GE(outdata[PROCESSSIZE/2+i], outdata[PROCESSSIZE/2+i-1]);
  }
}


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

#include <boost/simd/arithmetic/functions/divfix.hpp>
#include <boost/simd/include/functions/scalar/trunc.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( divfix_, tag::cpu_
                                   , (A0)
                                   , (scalar_< signed_<A0> >)
                                     (scalar_< signed_<A0> >)
                                   )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (!a0) return  Zero<result_type>();
      if(a1)
      {
        return a0/a1;
      }
      else
        return ((a0>0) ? Valmax<result_type>() : Valmin<result_type>());

    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( divfix_, tag::cpu_
                                   , (A0)
                                   , (scalar_< unsigned_<A0> >)
                                     (scalar_< unsigned_<A0> >)
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (!a0) return  Zero<result_type>();
      if(a1)
      {
        return a0/a1;
      }
      else
        return Valmax<result_type>();

    }
  };

#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_IMPLEMENT         ( divfix_, tag::cpu_
                                   , (A0)
                                   , (scalar_< floating_<A0> >)
                                     (scalar_< floating_<A0> >)
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return trunc(a0/a1);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif

} } }

#endif


#include <boost/multiprecision/tommath.hpp>


#pragma once
#include <Eigen/Sparse>
#include <array>
#include <numeric>

namespace mandoline {
Eigen::SparseMatrix<double> grid_boundary(int ni, int nj, bool dirichlet_boundary = true);
void fix_axis_aligned_indefiniteness(Eigen::SparseMatrix<double> &M, Eigen::VectorXd &rhs);
Eigen::VectorXd solve_SPSD_system(const Eigen::SparseMatrix<double> &E, const Eigen::VectorXd &rhs);
Eigen::VectorXd grid_edgeratio(int ni, int nj, const Eigen::VectorXd &phi);

}// namespace mandoline


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <stdint.h>
#include <string>
#include <vector>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <unsupported/Eigen/MatrixFunctions>

// CUDA runtime
#include <cuda_runtime.h>
// Utilities and system includes
//#include <mmf/defines.h>
#include <nvidia/helper_cuda.h>

#include <jsCore/clDataGpu.hpp>
#include <jsCore/timer.hpp>

using namespace Eigen;
using namespace std;

extern void robustSquaredAngleCostFctGPU(float *h_cost, float *d_cost,
    float *d_x, float* d_weights, uint32_t *d_z, float *d_mu, float sigma_sq,
    int N);

extern void robustSquaredAngleCostFctAssignmentGPU(float *h_cost, float *d_cost,
  uint32_t *h_W, uint32_t *d_W, float *d_x, float* d_weights,
  uint32_t *d_z, float *d_mu, float sigma_sq, int N);

extern void robustSquaredAngleCostFctJacobianGPU(float *h_J, float *d_J,
    float *d_x, float *d_weights, uint32_t *d_z, float *d_mu, float sigma_sq,
    int N);

extern void meanInTpS2GPU(float *h_p, float *d_p, float *h_mu_karch,
    float *d_mu_karch, float *d_q, uint32_t *d_z, float* d_weights,int N);

extern void sufficientStatisticsOnTpS2GPU(float *h_p, float *d_p, float
    *h_Rnorths, float *d_Rnorths, float *d_q, uint32_t *d_z ,int N, float
    *h_SSs, float *d_SSs);

extern void loadRGBvaluesForMFaxes();

namespace mmf{

class OptSO3
{
protected:
  float *d_cost, *d_J, *d_mu_;
//  float *h_errs_;
  uint32_t *d_N_;
  float t_max_, dt_;

  jsc::ClDataGpu<float> cld_;

public:
//  float *d_errs_;
//  float *d_q_
  float *d_weights_;
  const float sigma_sq_;
//  uint32_t *d_z, *h_z;

  float dtPrep_; // time for preparation (here assignemnts)
  float dtCG_; // time for cunjugate gradient

  uint32_t t_; // timestep
  Matrix3f Rprev_; // previous rotation

  // t_max and dt define the number linesearch steps, and how fine
  // grained to search
  OptSO3(float sigma, float t_max = 1.0f, float dt = 0.1f,
      float *d_weights =NULL)
    : d_cost(NULL), d_J(NULL), d_mu_(NULL), d_N_(NULL),
      t_max_(t_max),dt_(dt), cld_(3,6),
//    d_q_(d_q),
    d_weights_(d_weights),
    sigma_sq_(sigma*sigma), dtPrep_(0.0f), dtCG_(0.0f),
    t_(0), Rprev_(Matrix3f::Identity())
  {init();};

  virtual ~OptSO3();

  double D_KL_axisUnif();
  float *getErrs(int N);

  // uses uncompressed normals
  virtual void updateExternalGpuNormals(float* d_q, uint32_t N,
      uint32_t step, uint32_t offset = 0) {
    cld_.updateData(d_q,N,step,offset); };
  virtual double conjugateGradientCUDA(Matrix3f& R, uint32_t maxIter=100);

  /* return a skew symmetric matrix from A */
  Matrix3f enforceSkewSymmetry(const Matrix3f &A) const
  {return 0.5*(A-A.transpose());};

  float dtPrep(void) const { return dtPrep_;};
  float dtCG(void) const { return dtCG_;};

  const VectorXf& counts() {return this->cld_.counts();};
  const VectorXu& z() {return this->cld_.z();};
  const spMatrixXf& x() {return this->cld_.x();};
  uint32_t N() const {return this->cld_.N();};

protected:
  virtual float conjugateGradientPreparation_impl(Matrix3f& R, uint32_t& N);
  virtual float conjugateGradientCUDA_impl(Matrix3f& R, float res0, uint32_t N, uint32_t maxIter=100);
  virtual void conjugateGradientPostparation_impl(Matrix3f& R){;};
  /* evaluate cost function for a given assignment of npormals to axes */
  virtual float evalCostFunction(Matrix3f& R);
  /* compute Jacobian */
  virtual void computeJacobian(Matrix3f&J, Matrix3f& R, float N);
  /* recompute assignment based on rotation R and return residual as well */
  virtual float computeAssignment(Matrix3f& R, uint32_t& N);
  /* updates G and H from rotation R and jacobian J
   */
  virtual void updateGandH(Matrix3f& G, Matrix3f& G_prev, Matrix3f& H,
      const Matrix3f& R, const Matrix3f& J, const Matrix3f& M_t_min,
      bool resetH);
  /* performs line search starting at R in direction of H
   * returns min of cost function and updates R, and M_t_min
   */
  virtual float linesearch(Matrix3f& R, Matrix3f& M_t_min, const
      Matrix3f& H, float N, float t_max=1.0f, float dt=0.1f);
  /* mainly init GPU arrays */
  virtual void init();
  /* copy rotation to device */
  void Rot2Device(Matrix3f& R);
  /* convert a Rotation matrix R to a MF representaiton of the axes */
  void Rot2M(Matrix3f& R, float *mu);
  //deprecated
  void rectifyRotation(Matrix3f& R);
};
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward 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_TOOLBOX_ARITHMETIC_FUNCTION_SCALAR_FMA_HPP_INCLUDED
#define NT2_TOOLBOX_ARITHMETIC_FUNCTION_SCALAR_FMA_HPP_INCLUDED
#include <boost/fusion/tuple.hpp>

#include <nt2/include/functions/two_add.hpp>
#include <nt2/include/functions/two_prod.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::fma_, tag::cpu_,
                     (A0)(A1)(A2),
                     (arithmetic_<A0>)(arithmetic_<A1>)(arithmetic_<A2>)
                    )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::fma_(tag::arithmetic_,tag::arithmetic_,tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1,class A2>
    struct  result<This(A0,A1,A2)>
          : std::tr1::result_of<meta::arithmetic(A0,A1,A2)>{};

    NT2_FUNCTOR_CALL(3)
    {
      return a0*a1+a2;
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::fma_, tag::cpu_,
                     (A0)(A1)(A2),
                     (double_<A0>)(double_<A1>)(double_<A2>)
                    )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::fma_(tag::double_,tag::double_,tag::double_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1,class A2>
    struct  result<This(A0,A1,A2)>
          : std::tr1::result_of<meta::arithmetic(A0,A1,A2)>{};

    NT2_FUNCTOR_CALL(3)
    {
//       A0 p, rp, s, rs;
//       boost::fusion::tie(p, rp) = two_prod(a0, a1);
//       boost::fusion::tie(s, rs) = two_add(p, a2);
//       return s+(rp+rs);
      //      fma is incorrect this is Bugzilla Bug 3268 for glibc
      return a0*a1+a2;
      //         return ::fma(a0, a1, a2);
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::fma_, tag::cpu_,
                     (A0)(A1)(A2),
                     (float_<A0>)(float_<A1>)(float_<A2>)
                    )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::fma_(tag::float_,tag::float_,tag::float_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1,class A2>
    struct  result<This(A0,A1,A2)>
          : std::tr1::result_of<meta::arithmetic(A0,A1,A2)>{};

    NT2_FUNCTOR_CALL(3)
    {
//       A0 p, rp, s, rs;
//       boost::fusion::tie(p, rp) = two_prod(a0, a1);
//       boost::fusion::tie(s, rs) = two_add(p, a2);
//       return s+(rp+rs);
      //       fma is incorrect this is Bugzilla Bug 3268 for glibc
      return a0*a1+a2;
      //         return ::fma(a0, a1, a2);
    }
  };
} }

#endif
// modified by jt the 26/12/2010


/**
 * @file
 * @author Denise Ratasich
 * @date 05.09.2013
 *
 * @brief Configuration for ROS node.
 */

#include <Eigen/Core>
#include <string>
#include <stdexcept>
#include "estimation/models.h"
#include "configuration/configuration.h"

using namespace estimation;
using namespace Eigen;

// private functions
std::string getMethod();

// models
#if METHOD == EXTENDED_KALMAN_FILTER  ||	\
  METHOD == UNSCENTED_KALMAN_FILTER  ||		\
  METHOD == PARTICLE_FILTER_SIR
void f(VectorXd& x, const VectorXd& u);
void h(VectorXd& z, const VectorXd& x);
#endif
#if METHOD == EXTENDED_KALMAN_FILTER
void df(MatrixXd& A, const VectorXd& x, const VectorXd& u);
void dh(MatrixXd& H, const VectorXd& x);
#endif

// public functions for the ROS node
int getEstimatePeriod(void)
{
  return ESTIMATION_PERIOD;
}

void initEstimatorFactory(EstimatorFactory& factory)
{
  factory.addParam("method", getMethod());

#ifdef WINDOW_SIZE
  factory.addParam("window-size", WINDOW_SIZE);
#endif
#ifdef WEIGHTING_COEFFICIENTS
  VectorXd wc(VECTOR_SIZE(WEIGHTING_COEFFICIENTS));
  CODE_ASSIGN_VALUES_TO_VECTOR(wc, WEIGHTING_COEFFICIENTS);
  factory.addParam("weighting-coefficients", wc);
#endif

#ifdef STATE_TRANSITION_MODEL
  #if METHOD == KALMAN_FILTER
  MatrixXd stm(MATRIX_ROWS(STATE_TRANSITION_MODEL),
	       MATRIX_COLS(STATE_TRANSITION_MODEL));
  CODE_ASSIGN_VALUES_TO_MATRIX(stm, STATE_TRANSITION_MODEL);
  #elif METHOD == EXTENDED_KALMAN_FILTER  ||	\
    METHOD == UNSCENTED_KALMAN_FILTER  ||	\
    METHOD == PARTICLE_FILTER_SIR
  func_f stm = f;
  #endif
  factory.addParam("state-transition-model", stm);
#endif
#ifdef PROCESS_NOISE_COVARIANCE
  MatrixXd pnc(MATRIX_ROWS(PROCESS_NOISE_COVARIANCE),
	       MATRIX_COLS(PROCESS_NOISE_COVARIANCE));
  CODE_ASSIGN_VALUES_TO_MATRIX(pnc, PROCESS_NOISE_COVARIANCE);
  factory.addParam("process-noise-covariance", pnc);
#endif
#ifdef OBSERVATION_MODEL
  #if METHOD == KALMAN_FILTER
  MatrixXd om(MATRIX_ROWS(OBSERVATION_MODEL),
	       MATRIX_COLS(OBSERVATION_MODEL));
  CODE_ASSIGN_VALUES_TO_MATRIX(om, OBSERVATION_MODEL);
  #elif METHOD == EXTENDED_KALMAN_FILTER  ||	\
    METHOD == UNSCENTED_KALMAN_FILTER  ||	\
    METHOD == PARTICLE_FILTER_SIR
  func_h om = h;
  #endif
  factory.addParam("observation-model", om);
#endif
#ifdef MEASUREMENT_NOISE_COVARIANCE
  MatrixXd mnc(MATRIX_ROWS(MEASUREMENT_NOISE_COVARIANCE),
	       MATRIX_COLS(MEASUREMENT_NOISE_COVARIANCE));
  CODE_ASSIGN_VALUES_TO_MATRIX(mnc, MEASUREMENT_NOISE_COVARIANCE);
  factory.addParam("measurement-noise-covariance", mnc);
#endif
#ifdef CONTROL_INPUT_MODEL
  MatrixXd cim(MATRIX_ROWS(CONTROL_INPUT_MODEL),
	       MATRIX_COLS(CONTROL_INPUT_MODEL));
  CODE_ASSIGN_VALUES_TO_MATRIX(cim, CONTROL_INPUT_MODEL);
  factory.addParam("control-input-model", cim);
#endif
#ifdef INITIAL_STATE
  VectorXd is(VECTOR_SIZE(INITIAL_STATE));
  CODE_ASSIGN_VALUES_TO_VECTOR(is, INITIAL_STATE);
  factory.addParam("initial-state", is);
#endif
#ifdef INITIAL_ERROR_COVARIANCE
  MatrixXd iec(MATRIX_ROWS(INITIAL_ERROR_COVARIANCE),
	       MATRIX_COLS(INITIAL_ERROR_COVARIANCE));
  CODE_ASSIGN_VALUES_TO_MATRIX(iec, INITIAL_ERROR_COVARIANCE);
  factory.addParam("initial-error-covariance", iec);
#endif

#ifdef STATE_SIZE
  factory.addParam("state-size", STATE_SIZE);
#endif
#ifdef MEASUREMENT_SIZE
  factory.addParam("measurement-size", MEASUREMENT_SIZE);
#endif
#ifdef CONTROL_SIZE
  factory.addParam("control-input-size", CONTROL_SIZE);
#endif

#ifdef STATE_TRANSITION_MODEL_JACOBIAN
  func_df stmj = df;
  factory.addParam("state-transition-model-jacobian", stmj);
#endif
#ifdef OBSERVATION_MODEL_JACOBIAN
  func_dh omj = dh;
  factory.addParam("observation-model-jacobian", omj);
#endif

// SIR
#ifdef STATE_BOUNDS
  MatrixXd sb(MATRIX_ROWS(STATE_BOUNDS),
	      MATRIX_COLS(STATE_BOUNDS));
  CODE_ASSIGN_VALUES_TO_MATRIX(sb, STATE_BOUNDS);
  factory.addParam("state-bounds", sb);
#endif
#ifdef NUMBER_OF_PARTICLES
  factory.addParam("number-of-particles", NUMBER_OF_PARTICLES);
#endif

// Confidence-Weighted Averaging
#ifdef IGNORE_ZERO_VARIANCE_VALUES
  factory.addParam("ignore-zero-variance-values", 0);
#endif
}

// implementation of private functions
std::string getMethod()
{
#if METHOD == MOVING_MEDIAN
  return "MovingMedian";
#elif METHOD == MOVING_AVERAGE
  return "MovingAverage";
#elif METHOD == KALMAN_FILTER
  return "KalmanFilter";
#elif METHOD == EXTENDED_KALMAN_FILTER
  return "ExtendedKalmanFilter";
#elif METHOD == UNSCENTED_KALMAN_FILTER
  return "UnscentedKalmanFilter";
#elif METHOD == PARTICLE_FILTER_SIR
  return "ParticleFilterSIR";
#elif METHOD == CONFIDENCE_WEIGHTED_AVERAGING
  return "ConfidenceWeightedAveraging";
#endif
}

// implementation of models
#if METHOD == EXTENDED_KALMAN_FILTER  ||	\
  METHOD == UNSCENTED_KALMAN_FILTER  ||		\
  METHOD == PARTICLE_FILTER_SIR
void f(VectorXd& x, const VectorXd& u)
{
  if (x.size() != STATE_SIZE)
    throw std::runtime_error("Applying state transition model failed, state vector has invalid size.");

  // create state vector for result, i.e. the a priori state estimate;
  // copying must be done, because x occurs on the right-hand side,
  // e.g.: x[0] = x[1]; x[1] = x[0]; is different to x_apriori[0] =
  // x[1]; x_apriori[1] = x[0];!!
  VectorXd x_apriori(x.size());

  try
  {
    // assign formulas of the state transition model (from the
    // configuration header) to vector x, i.e. calculate a priori state
    // estimate
    CODE_ASSIGN_FORMULAS_TO_VECTOR(x_apriori, STATE_TRANSITION_MODEL);
  }
  catch (std::exception& e)
  {
    std::string additionalInfo = "Applying state transition model failed. ";
    throw std::runtime_error(additionalInfo + e.what());
  }

  // copy back, x will then represent the a priori state estimate
  x = x_apriori;
}

void h(VectorXd& z, const VectorXd& x)
{
  if (z.size() != MEASUREMENT_SIZE)
    throw std::runtime_error("Applying observation model failed, measurement vector has invalid size.");

  try
  {
    // z doesn't occur on the right-hand side, so no copying necessary
    CODE_ASSIGN_FORMULAS_TO_VECTOR(z, OBSERVATION_MODEL);
  }
  catch (std::exception& e)
  {
    std::string additionalInfo = "Applying observation model failed. ";
    throw std::runtime_error(additionalInfo + e.what());
  }
}
#endif

#if METHOD == EXTENDED_KALMAN_FILTER
void df(MatrixXd& A, const VectorXd& x, const VectorXd& u)
{
  try
  {
    CODE_ASSIGN_FORMULAS_TO_MATRIX(A, STATE_TRANSITION_MODEL_JACOBIAN);
  }
  catch (std::exception& e)
  {
    std::string additionalInfo = "Applying Jacobian of state transition model failed. ";
    throw std::runtime_error(additionalInfo + e.what());
  }
}

void dh(MatrixXd& H, const VectorXd& x)
{
  try
  {
    CODE_ASSIGN_FORMULAS_TO_MATRIX(H, OBSERVATION_MODEL_JACOBIAN);
  }
  catch (std::exception& e)
  {
    std::string additionalInfo = "Applying Jacobian of observation model failed. ";
    throw std::runtime_error(additionalInfo + e.what());
  }
}
#endif