#include "initial_conditions/radial_wave.hpp"
#include "utils.hpp"
#include <boost/test/unit_test.hpp>

#include <boost/numeric/ublas/io.hpp>

using zero_vec = boost::numeric::ublas::zero_vector<double>;

using namespace init_cond;

BOOST_AUTO_TEST_SUITE(radial_init)

    struct Radial2DFixture
    {
        Radial2DFixture() : wave(2)
        {
            InitialConditionConfiguration config;
            config.setParticleCount(100).setEnergyNormalization(false)
                    .setSupport(std::vector<double>{1.0, 1.0})
                    .setOffset(zero_vec(2));

            wave.init(config);
        }

        RadialWave2D wave;
    };

    /*
     * This tests that the RadialWave nad RadialWave3D IC correctly checks its arguments and refuses mismatching dimensions.
     */

    BOOST_FIXTURE_TEST_CASE(parameter_check_2d, Radial2DFixture)
    {
        // need at least 2d for 2d IC
        BOOST_CHECK_THROW(RadialWave2D(1), std::logic_error);

        RadialWave2D wave2d(2);

        BOOST_CHECK_THROW(wave.setOrigin(gen_vect(3)), std::invalid_argument);
        BOOST_CHECK_THROW(wave.setOrigin(gen_vect(1)), std::invalid_argument);
    }

    BOOST_AUTO_TEST_CASE(parameter_check_3d)
    {
        // need at least 3d for 3d IC
        BOOST_CHECK_THROW(RadialWave3D(1), std::logic_error);
        BOOST_CHECK_THROW(RadialWave3D(2), std::logic_error);

        RadialWave3D wave3d(3);

        BOOST_CHECK_THROW(wave3d.setOrigin(gen_vect(2)), std::invalid_argument);
    }

    /*
     * This checks that PlanarWave produces correct ICs in the 2d case.
     */
    BOOST_FIXTURE_TEST_CASE(deterministic_2d, Radial2DFixture)
    {
        gen_vect origin(2);
        origin[0] = 0.3;
        origin[1] = 0.6;
        wave.setOrigin(origin);

        //! \todo we call init before having supplied all data. That should probably not be allowed!

        std::vector<int> counter(100);
        for(auto ic = wave.next(); ic; ++ic)
        {
            auto& state = ic.getState();
            BOOST_REQUIRE_CLOSE(state.getPosition()[0], 0.3, 1e-6);
            BOOST_REQUIRE_CLOSE(state.getPosition()[1], 0.6, 1e-6);

            double angle = std::atan2(state.getVelocity()[1], state.getVelocity()[0]) + pi;
            int angle_int = std::floor(angle * 100 / 2 / pi);
            counter[angle_int] += 1;

            // delta:
            auto& delta = ic.getDelta(0);
            // no change in position
            BOOST_REQUIRE_SMALL(delta.getPosition()[0], 1e-6);
            BOOST_REQUIRE_SMALL(delta.getPosition()[1], 1e-6);

            // and dv _|_ v
            BOOST_REQUIRE_SMALL(dotProduct(delta.getVelocity(), state.getVelocity()), 2e-4);
            BOOST_REQUIRE_CLOSE(dotProduct(delta.getVelocity(), delta.getVelocity()), 4*pi*pi, 2e-4);
        }

        for(auto& count : counter) {
            BOOST_CHECK_EQUAL(count, 1);
        }
    }

    /*
     * Utility used as base class for fixtures for 2D radial tests (both random and deterministic). This registers
     * checks for the distribution of velocities that are run automatically at the end of the test case.
     */

    struct Radial2DTest
    {
        Radial2DTest() : check_phi(true, "phi"),
                         check_vx(true, "vx"),
                         check_vy(true, "vy")
        {
        }

        void checkCondition(InitialCondition& cond) {
            auto& vel = cond.getState().getVelocity();
            double x = vel[0];
            double y = vel[1];
            double phi = std::atan2(y, x);

            double phi_deg = phi * 180 / pi + 180;
            check_phi.put_in_bin(phi_deg / 360);

            check_vx.put_in_bin((x + 1)/2);
            check_vy.put_in_bin((y + 1)/2);

            // delta:
            auto& delta_phi = cond.getDelta(0);

            // and dv/dphi _|_ v, rate of 2pi
            BOOST_REQUIRE_SMALL(dotProduct(delta_phi.getVelocity(), vel), 2e-4);
            // check the length of the vector
            BOOST_REQUIRE_CLOSE(dotProduct(delta_phi.getVelocity(), delta_phi.getVelocity()), 4*pi*pi, 2e-4);

            // no change in position
            BOOST_REQUIRE_SMALL(delta_phi.getPosition()[0], 1e-6);
            BOOST_REQUIRE_SMALL(delta_phi.getPosition()[1], 1e-6);
        }

        std::size_t NUM_PARTICLES = 200000;

        UniformityCheck check_phi;
        UniformityCheck check_vx;
        UniformityCheck check_vy;
    };

    struct DeterministicRadial2D : public Radial2DTest
    {
        DeterministicRadial2D() : wave(2)
        {
            InitialConditionConfiguration config;
            config.setParticleCount(NUM_PARTICLES).setEnergyNormalization(false)
                    .setSupport(std::vector<double>{1.0, 1.0})
                    .setOffset(zero_vec(2));

            wave.init(config);
        }

        RadialWave2D wave;
    };

    struct RandomRadial2D : public Radial2DTest
    {
        RandomRadial2D() : wave(2)
        {
            InitialConditionConfiguration config;
            config.setParticleCount(NUM_PARTICLES).setEnergyNormalization(false)
                    .setSupport(std::vector<double>{1.0, 1.0})
                    .setOffset(zero_vec(2));

            wave.init(config);
        }

        RandomRadial wave;
    };

    BOOST_FIXTURE_TEST_CASE(deterministic_2d_velocity_distribution, DeterministicRadial2D)
    {
        for(InitialCondition cond = wave.next(); cond; ++cond)
        {
            checkCondition(cond);

            auto& delta = cond.getDelta(0);
        }
    }

    BOOST_FIXTURE_TEST_CASE(random_2d_velocity_distribution, RandomRadial2D)
    {
        UniformityCheck check_x(true, "x");
        UniformityCheck check_y(true, "y");
        for(InitialCondition cond = wave.next(); cond; ++cond)
        {
            checkCondition(cond);
            check_x.put_in_bin(cond.getState().getPosition()[0]);
            check_y.put_in_bin(cond.getState().getPosition()[1]);
        }
    }

    /*
     * Utility used as base class for fixtures for 3D radial tests (both random and deterministic). This registers
     * checks for the distribution of velocities that are run automatically at the end of the test case.
     */

    struct Radial3DTest
    {
        Radial3DTest() : check_phi(true, "phi"),
                         check_vx(true, "vx"),
                         check_vy(true, "vy"),
                         check_vz(true, "vz")
        {
        }

        void checkCondition(InitialCondition& cond) {
            auto& vel = cond.getState().getVelocity();
            double x = vel[0];
            double y = vel[1];
            double z = vel[2];
            double c = std::sqrt(1 - z*z);   // cos(theta)
            double phi = std::atan2(y/c, x/c);

            double phi_deg = phi * 180 / pi + 180;
            check_phi.put_in_bin(phi_deg / 360);

            check_vx.put_in_bin((x + 1)/2);
            check_vy.put_in_bin((y + 1)/2);
            check_vz.put_in_bin((z + 1)/2);

            // delta:
            auto& delta_theta = cond.getDelta(0);
            auto& delta_phi = cond.getDelta(1);

            // and dv _|_ v, rate of 2pi
            BOOST_REQUIRE_SMALL(dotProduct(delta_phi.getVelocity(), vel), 2e-4);
            BOOST_REQUIRE_SMALL(delta_phi.getVelocity()[2], 1e-6);
            BOOST_REQUIRE_CLOSE(dotProduct(delta_phi.getVelocity(), delta_phi.getVelocity()), c*c*4*pi*pi, 2e-4);

            // and dv _|_ v
            BOOST_REQUIRE_SMALL(dotProduct(delta_theta.getVelocity(), vel), 2e-4);
            BOOST_REQUIRE_CLOSE(dotProduct(delta_theta.getVelocity(), delta_theta.getVelocity()), pi*pi, 2e-4);

            // no change in position
            BOOST_REQUIRE_SMALL(delta_phi.getPosition()[0], 1e-6);
            BOOST_REQUIRE_SMALL(delta_phi.getPosition()[1], 1e-6);
            BOOST_REQUIRE_SMALL(delta_phi.getPosition()[2], 1e-6);

            BOOST_REQUIRE_SMALL(delta_theta.getPosition()[0], 1e-6);
            BOOST_REQUIRE_SMALL(delta_theta.getPosition()[1], 1e-6);
            BOOST_REQUIRE_SMALL(delta_theta.getPosition()[2], 1e-6);
        }

        std::size_t NUM_PARTICLES = 200000;

        UniformityCheck check_phi;
        UniformityCheck check_vx;
        UniformityCheck check_vy;
        UniformityCheck check_vz;
    };

    struct DeterministicRadial3D : public Radial3DTest
    {
        DeterministicRadial3D() : wave(3)
        {
            InitialConditionConfiguration config;
            config.setParticleCount(NUM_PARTICLES).setEnergyNormalization(false)
                    .setSupport(std::vector<double>{1.0, 1.0, 1.0})
                    .setOffset(zero_vec(3));

            wave.init(config);
        }

        RadialWave3D wave;
    };

    struct RandomRadial3D : public Radial3DTest
    {
        RandomRadial3D() : wave(3)
        {
            InitialConditionConfiguration config;
            config.setParticleCount(NUM_PARTICLES).setEnergyNormalization(false)
                    .setSupport(std::vector<double>{1.0, 1.0, 1.0})
                    .setOffset(zero_vec(3));

            wave.init(config);
        }

        ~RandomRadial3D()
        {
            check_phi.save("phi_test_det.txt");
        }

        RandomRadial wave;
    };

    BOOST_FIXTURE_TEST_CASE(deterministic_3d_velocity_distribution, DeterministicRadial3D)
    {
        for(InitialCondition cond = wave.next(); cond; ++cond)
        {
            checkCondition(cond);

            auto& delta = cond.getDelta(0);
        }
    }

    BOOST_FIXTURE_TEST_CASE(random_3d_velocity_distribution, RandomRadial3D)
    {
        UniformityCheck check_z(true, "z");
        UniformityCheck check_y(true, "y");
        UniformityCheck check_x(true, "x");
        for(InitialCondition cond = wave.next(); cond; ++cond)
        {
            checkCondition(cond);
            check_x.put_in_bin(cond.getState().getPosition()[0]);
            check_y.put_in_bin(cond.getState().getPosition()[1]);
            check_z.put_in_bin(cond.getState().getPosition()[2]);
        }
    }

BOOST_AUTO_TEST_SUITE_END()


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110113    E. Iorfida        First creation of the code.
 *      110126    E. Iorfida        Added test for velocities.
 *      110130    J. Melman         Simplified variable names, e.g., 'normOfVelocityVector' became
 *                                  'speed'. Also corrected 'tangential' to Suggested less trivial
 *                                  test case.
 *      110201    E. Iorfida        Added pointerToCelestialBody and modified variable names (from
 *                                  heliocentric, to inertial). Added elliptical case and check
 *                                  for the anti-clockwise direction of motion.
 *      110207    E. Iorfida        Added a more stric value for tolerance of semi-major axis for
 *                                  hyperbolic case.
 *      110208    E. Iorfida        Added CartesianPositionElements objects as input and
 *                                  CartesianVelocityElements objects as output.
 *      110512    K. Kumar          Updated code not to use dynamic memory allocation.
 *      110627    K. Kumar          Updated to use new predefined planets code.
 *      120416    T. Secretin       Boostified unit test.
 *      120418    T. Secretin       Adapted to new class implementation.
 *      120704    P. Musegaas       Changed tolerance.
 *      120705    T. Secretin       Updated Eigen::Vector3d constructors to const-correctness.
 *
 *    References
 *      Noomen, R., Lambert targeter Excel file.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/MissionSegments/lambertTargeter.h"

namespace tudat
{
namespace mission_segments
{

// A dummy derived class to test the functionality of the Lambert targeter base class.
class LambertTargeterDummy : public LambertTargeter
{
public:

    // Default constructor.
    LambertTargeterDummy( const Eigen::Vector3d& cartesianPositionAtDeparture,
                          const Eigen::Vector3d& cartesianPositionAtArrival,
                          const double& timeOfFlight,
                          const double& gravitationalParameter )
        : LambertTargeter( cartesianPositionAtDeparture, cartesianPositionAtArrival, timeOfFlight,
                           gravitationalParameter )
    {
        execute( );
    }

protected:

    void execute( )
    {
        // Set inertial velocities.
        cartesianVelocityAtDeparture << 2735.8, 6594.3, 0.0;
        cartesianVelocityAtArrival << -1367.9, 4225.03, 0.0;
    }

};

} // namespace mission_segments

namespace unit_tests
{

//! Test the Izzo Lambert targeting algorithm code.
BOOST_AUTO_TEST_SUITE( test_lambert_targeter )

//! Test the return of the inertial velocities.
BOOST_AUTO_TEST_CASE( testGetInertialVelocity )
{
    // Set tolerance.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    // Set canonical units for Earth (see page 29 [1]).
    const double distanceUnit = 6.378136e6;
    const double timeUnit = 806.78;

    // Set expected inertial vectors.
    const Eigen::Vector3d expectedInertialVelocityAtDeparture( 2735.8, 6594.3, 0.0 ),
            expectedInertialVelocityAtArrival( -1367.9, 4225.03, 0.0 );

    // Time conversions.
    const double testTimeOfFlight = 5.0 * timeUnit;

    // Set central body graviational parameter.
    const double testGravitationalParameter = 398600.4418e9;

    // Set position at departure and arrival.
    const Eigen::Vector3d testCartesianPositionAtDeparture( 2.0 * distanceUnit, 0.0, 0.0 ),
            testCartesianPositionAtArrival( 2.0 * distanceUnit, 2.0 * sqrt( 3.0 ) * distanceUnit,
                                            0.0 );
    // Declare Lambert targeter object.
    mission_segments::LambertTargeterDummy
            testLambertTargeter( testCartesianPositionAtDeparture, testCartesianPositionAtArrival,
                                 testTimeOfFlight, testGravitationalParameter );

    // Check that returned vectors are equal to expected vectors.
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.x( ),
                                testLambertTargeter.getInertialVelocityAtDeparture( ).x( ),
                                tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.y( ),
                                testLambertTargeter.getInertialVelocityAtDeparture( ).y( ),
                                tolerance );
    BOOST_CHECK_SMALL( testLambertTargeter.getInertialVelocityAtDeparture( ).z( ), tolerance );

    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.x( ),
                                testLambertTargeter.getInertialVelocityAtArrival( ).x( ),
                                tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.y( ),
                                testLambertTargeter.getInertialVelocityAtArrival( ).y( ),
                                tolerance );
    BOOST_CHECK_SMALL( testLambertTargeter.getInertialVelocityAtArrival( ).z( ), tolerance );

    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.x( ),
                                testLambertTargeter.getInertialVelocityVectors( ).first.x( ),
                                tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.y( ),
                                testLambertTargeter.getInertialVelocityVectors( ).first.y( ),
                                tolerance );
    BOOST_CHECK_SMALL( testLambertTargeter.getInertialVelocityVectors( ).first.z( ), tolerance );

    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.x( ),
                                testLambertTargeter.getInertialVelocityVectors( ).second.x( ),
                                tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.y( ),
                                testLambertTargeter.getInertialVelocityVectors( ).second.y( ),
                                tolerance );
    BOOST_CHECK_SMALL( testLambertTargeter.getInertialVelocityVectors( ).second.z( ), tolerance );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <Eigen/Geometry>
#include <unsupported/Eigen/AutoDiff>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <sdf_tools/SDF.h>

#ifndef SDF_HPP
#define SDF_HPP

namespace sdf_tools
{
using VoxelGrid::GRID_INDEX;

class SignedDistanceField : public VoxelGrid::VoxelGrid<float>
{
protected:

  std::string frame_;
  bool locked_;

  /*
   * You *MUST* provide valid indices to this function, hence why it is
   * protected (there are safe wrappers available - use them!)
   */
  void FollowGradientsToLocalExtremaUnsafe(
      VoxelGrid<Eigen::Vector3d>& watershed_map,
      const int64_t x_index,
      const int64_t y_index,
      const int64_t z_index) const;

  bool GradientIsEffectiveFlat(const Eigen::Vector3d& gradient) const;

  GRID_INDEX GetNextFromGradient(const GRID_INDEX& index,
                                 const Eigen::Vector3d& gradient) const;

public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  inline SignedDistanceField(const std::string& frame,
                             double resolution,
                             double x_size,
                             double y_size,
                             double z_size,
                             float OOB_value)
    : VoxelGrid::VoxelGrid<float>(resolution,
                                  x_size, y_size, z_size,
                                  OOB_value),
      frame_(frame), locked_(false) {}

  inline SignedDistanceField(const Eigen::Isometry3d& origin_transform,
                             const std::string& frame,
                             double resolution,
                             double x_size,
                             double y_size,
                             double z_size,
                             float OOB_value)
    : VoxelGrid::VoxelGrid<float>(origin_transform, resolution,
                                  x_size, y_size, z_size,
                                  OOB_value),
      frame_(frame), locked_(false) {}

  inline SignedDistanceField(const std::string& frame,
                             double resolution,
                             int64_t x_cells,
                             int64_t y_cells,
                             int64_t z_cells,
                             float OOB_value)
    : VoxelGrid::VoxelGrid<float>(resolution,
                                  x_cells, y_cells, z_cells,
                                  OOB_value),
      frame_(frame), locked_(false) {}

  inline SignedDistanceField(const Eigen::Isometry3d& origin_transform,
                             const std::string& frame,
                             double resolution,
                             int64_t x_cells,
                             int64_t y_cells,
                             int64_t z_cells,
                             float OOB_value)
    : VoxelGrid::VoxelGrid<float>(origin_transform, resolution,
                                  x_cells, y_cells, z_cells,
                                  OOB_value),
      frame_(frame), locked_(false) {}

  inline SignedDistanceField()
    : VoxelGrid::VoxelGrid<float>(), frame_(""), locked_(false) {}

  virtual VoxelGrid<float>* Clone() const
  {
    return new SignedDistanceField(
          static_cast<const SignedDistanceField&>(*this));
  }

  inline bool IsLocked() const
  {
      return locked_;
  }

  inline void Lock()
  {
      locked_ = true;
  }

  inline void Unlock()
  {
      locked_ = false;
  }

  virtual uint64_t SerializeSelf(
      std::vector<uint8_t>& buffer,
      const std::function<uint64_t(
        const float&, std::vector<uint8_t>&)>& value_serializer
      =arc_helpers::SerializeFixedSizePOD<float>) const;

  virtual uint64_t DeserializeSelf(
      const std::vector<uint8_t>& buffer, const uint64_t current,
      const std::function<std::pair<float, uint64_t>(
        const std::vector<uint8_t>&, const uint64_t)>& value_deserializer
      =arc_helpers::DeserializeFixedSizePOD<float>);

  /*
   * Mutable access and setter functions MUST be used carefully!
   * If you arbitrarily change SDF values, it is not a proper SDF any more!
   *
   * Use of these functions can be prevented by calling
   * SignedDistanceField::Lock() on the SDF
   */

  virtual std::pair<float&, bool> GetMutable3d(
      const Eigen::Vector3d& location)
  {
    const GRID_INDEX index = LocationToGridIndex3d(location);
    if (IndexInBounds(index))
    {
      return GetMutable(index);
    }
    else
    {
      return std::pair<float&, bool>(oob_value_, false);
    }
  }

  virtual std::pair<float&, bool> GetMutable4d(
      const Eigen::Vector4d& location)
  {
    const GRID_INDEX index = LocationToGridIndex4d(location);
    if (IndexInBounds(index))
    {
      return GetMutable(index);
    }
    else
    {
      return std::pair<float&, bool>(oob_value_, false);
    }
  }

  virtual std::pair<float&, bool> GetMutable(const double x,
                                             const double y,
                                             const double z)
  {
    const Eigen::Vector4d location(x, y, z, 1.0);
    return GetMutable4d(location);
  }

  virtual std::pair<float&, bool> GetMutable(const GRID_INDEX& index)
  {
    if (IndexInBounds(index) && !locked_)
    {
      return std::pair<float&, bool>(
            AccessIndex(GetDataIndex(index)), true);
    }
    else
    {
      return std::pair<float&, bool>(oob_value_, false);
    }
  }

  virtual std::pair<float&, bool> GetMutable(const int64_t x_index,
                                             const int64_t y_index,
                                             const int64_t z_index)
  {
    if (IndexInBounds(x_index, y_index, z_index) && !locked_)
    {
      return std::pair<float&, bool>(
            AccessIndex(GetDataIndex(x_index, y_index, z_index)), true);
    }
    else
    {
      return std::pair<float&, bool>(oob_value_, false);
    }
  }

  virtual bool SetValue3d(const Eigen::Vector3d& location,
                          const float& value)
  {
    const GRID_INDEX index = LocationToGridIndex3d(location);
    if (IndexInBounds(index))
    {
      return SetValue(index, value);
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue4d(const Eigen::Vector4d& location,
                          const float& value)
  {
    const GRID_INDEX index = LocationToGridIndex4d(location);
    if (IndexInBounds(index))
    {
      return SetValue(index, value);
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue(const double x,
                        const double y,
                        const double z,
                        const float& value)
  {
    const Eigen::Vector4d location(x, y, z, 1.0);
    return SetValue4d(location, value);
  }

  virtual bool SetValue(const GRID_INDEX& index,
                        const float& value)
  {
    if (IndexInBounds(index) && !locked_)
    {
      AccessIndex(GetDataIndex(index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue(const int64_t x_index,
                        const int64_t y_index,
                        const int64_t z_index,
                        const float& value)
  {
    if (IndexInBounds(x_index, y_index, z_index) && !locked_)
    {
      AccessIndex(GetDataIndex(x_index, y_index, z_index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue3d(const Eigen::Vector3d& location,
                          float&& value)
  {
    const GRID_INDEX index = LocationToGridIndex3d(location);
    if (IndexInBounds(index))
    {
      return SetValue(index, value);
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue4d(const Eigen::Vector4d& location,
                          float&& value)
  {
    const GRID_INDEX index = LocationToGridIndex4d(location);
    if (IndexInBounds(index))
    {
      return SetValue(index, value);
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue(const double x,
                        const double y,
                        const double z,
                        float&& value)
  {
    const Eigen::Vector4d location(x, y, z, 1.0);
    return SetValue4d(location, value);
  }

  virtual bool SetValue(const GRID_INDEX& index,
                        float&& value)
  {
    if (IndexInBounds(index) && !locked_)
    {
      AccessIndex(GetDataIndex(index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue(const int64_t x_index,
                        const int64_t y_index,
                        const int64_t z_index,
                        float&& value)
  {
    if (IndexInBounds(x_index, y_index, z_index) && !locked_)
    {
      AccessIndex(GetDataIndex(x_index, y_index, z_index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  inline double GetResolution() const { return GetCellSizes().x(); }

  inline std::string GetFrame() const
  {
    return frame_;
  }

  inline std::vector<double> GetGradient(
      const double x, const double y, const double z,
      const bool enable_edge_gradients=false) const
  {
    return GetGradient4d(Eigen::Vector4d(x, y, z, 1.0), enable_edge_gradients);
  }

  inline std::vector<double> GetGradient3d(
      const Eigen::Vector3d& location,
      const bool enable_edge_gradients=false) const
  {
    const GRID_INDEX index = LocationToGridIndex3d(location);
    if (IndexInBounds(index))
    {
      return GetGradient(index, enable_edge_gradients);
    }
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetGradient4d(
      const Eigen::Vector4d& location,
      const bool enable_edge_gradients=false) const
  {
    const GRID_INDEX index = LocationToGridIndex4d(location);
    if (IndexInBounds(index))
    {
      return GetGradient(index, enable_edge_gradients);
    }
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetGradient(
      const GRID_INDEX& index,
      const bool enable_edge_gradients=false) const
  {
    return GetGradient(index.x, index.y, index.z, enable_edge_gradients);
  }

  inline std::vector<double> GetGradient(
      const int64_t x_index, const int64_t y_index, const int64_t z_index,
      const bool enable_edge_gradients=false) const
  {
    const std::vector<double> grid_aligned_gradient
        = GetGridAlignedGradient(x_index, y_index, z_index,
                                 enable_edge_gradients);
    if (grid_aligned_gradient.size() == 3)
    {
      const Eigen::Quaterniond grid_rotation(origin_transform_.rotation());
      // Derived from EigenHelpers::RotateVector, but without extra copies
      const Eigen::Quaterniond temp(0.0,
                                    grid_aligned_gradient[0],
                                    grid_aligned_gradient[1],
                                    grid_aligned_gradient[2]);
      const Eigen::Quaterniond result
          = grid_rotation * (temp * grid_rotation.inverse());
      return std::vector<double>{result.x(),
                                 result.y(),
                                 result.z()};
    }
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetGridAlignedGradient(
      const int64_t x_index, const int64_t y_index, const int64_t z_index,
      const bool enable_edge_gradients=false) const
  {
    // Make sure the index is inside bounds
    if (IndexInBounds(x_index, y_index, z_index))
    {
      // See if the index we're trying to query is one cell in from the edge
      if ((x_index > 0) && (y_index > 0) && (z_index > 0)
          && (x_index < (GetNumXCells() - 1))
          && (y_index < (GetNumYCells() - 1))
          && (z_index < (GetNumZCells() - 1)))
      {
        const double inv_twice_resolution = 1.0 / (2.0 * GetResolution());
        const double gx = (GetImmutable(x_index + 1, y_index, z_index).first
                           - GetImmutable(x_index - 1, y_index, z_index).first)
                          * inv_twice_resolution;
        const double gy = (GetImmutable(x_index, y_index + 1, z_index).first
                           - GetImmutable(x_index, y_index - 1, z_index).first)
                          * inv_twice_resolution;
        const double gz = (GetImmutable(x_index, y_index, z_index + 1).first
                           - GetImmutable(x_index, y_index, z_index - 1).first)
                          * inv_twice_resolution;
        return std::vector<double>{gx, gy, gz};
      }
      // If we're on the edge, handle it specially
      // TODO: we actually need to handle corners even more carefully,
      // since if the SDF is build with a virtual border, these cells will
      // get zero gradient from this approach!
      else if (enable_edge_gradients)
      {
        // Get the "best" indices we can use
        const int64_t low_x_index = std::max((int64_t)0, x_index - 1);
        const int64_t high_x_index = std::min(GetNumXCells() - 1, x_index + 1);
        const int64_t low_y_index = std::max((int64_t)0, y_index - 1);
        const int64_t high_y_index = std::min(GetNumYCells() - 1, y_index + 1);
        const int64_t low_z_index = std::max((int64_t)0, z_index - 1);
        const int64_t high_z_index = std::min(GetNumZCells() - 1, z_index + 1);
        // Compute the axis increments
        const double x_increment
            = (high_x_index - low_x_index) * GetResolution();
        const double y_increment
            = (high_y_index - low_y_index) * GetResolution();
        const double z_increment
            = (high_z_index - low_z_index) * GetResolution();
        // Compute the gradients for each axis - by default these are zero
        double gx = 0.0;
        double gy = 0.0;
        double gz = 0.0;
        // Only if the increments are non-zero do we compute the axis gradient
        if (x_increment > 0.0)
        {
          const double inv_x_increment = 1.0 / x_increment;
          const double high_x_value
              = GetImmutable(high_x_index, y_index, z_index).first;
          const double low_x_value
              = GetImmutable(low_x_index, y_index, z_index).first;
          // Compute the gradient
          gx = (high_x_value - low_x_value) * inv_x_increment;
        }
        if (y_increment > 0.0)
        {
          const double inv_y_increment = 1.0 / y_increment;
          const double high_y_value
              = GetImmutable(x_index, high_y_index, z_index).first;
          const double low_y_value
              = GetImmutable(x_index, low_y_index, z_index).first;
          // Compute the gradient
          gy = (high_y_value - low_y_value) * inv_y_increment;
        }
        if (z_increment > 0.0)
        {
          const double inv_z_increment = 1.0 / z_increment;
          const double high_z_value
              = GetImmutable(x_index, y_index, high_z_index).first;
          const double low_z_value
              = GetImmutable(x_index, y_index, low_z_index).first;
          // Compute the gradient
          gz = (high_z_value - low_z_value) * inv_z_increment;
        }
        // Assemble and return the computed gradient
        return std::vector<double>{gx, gy, gz};
      }
      // Edge gradients disabled, return no gradient
      else
      {
        return std::vector<double>();
      }
    }
    // If we're out of bounds, return no gradient
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetSmoothGradient3d(
      const Eigen::Vector3d& location,
      const double nominal_window_size) const
  {
    return GetSmoothGradient(location.x(), location.y(), location.z(),
                             nominal_window_size);
  }

  inline std::vector<double> GetSmoothGradient4d(
      const Eigen::Vector4d& location,
      const double nominal_window_size) const
  {
    return GetSmoothGradient(location(0), location(1), location(2),
                             nominal_window_size);
  }

  inline std::vector<double> GetSmoothGradient(
      const double x, const double y, const double z,
      const double nominal_window_size) const
  {
    const double ideal_window_size = std::abs(nominal_window_size);
    if (LocationInBounds(x, y, z))
    {
      const double min_x = x - ideal_window_size;
      const double max_x = x + ideal_window_size;
      const double min_y = y - ideal_window_size;
      const double max_y = y + ideal_window_size;
      const double min_z = z - ideal_window_size;
      const double max_z = z + ideal_window_size;
      // Retrieve distance estimates
      const std::pair<double, bool> point_distance = EstimateDistance(x, y, z);
      const std::pair<double, bool> mx_distance = EstimateDistance(min_x, y, z);
      const std::pair<double, bool> px_distance = EstimateDistance(max_x, y, z);
      const std::pair<double, bool> my_distance = EstimateDistance(x, min_y, z);
      const std::pair<double, bool> py_distance = EstimateDistance(x, max_y, z);
      const std::pair<double, bool> mz_distance = EstimateDistance(x, y, min_z);
      const std::pair<double, bool> pz_distance = EstimateDistance(x, y, max_z);
      // Compute gradient for each axis
      const double gx = ComputeAxisSmoothGradient(point_distance,
                                                  mx_distance,
                                                  px_distance,
                                                  x, min_x, max_x);
      const double gy = ComputeAxisSmoothGradient(point_distance,
                                                  my_distance,
                                                  py_distance,
                                                  y, min_y, max_y);
      const double gz = ComputeAxisSmoothGradient(point_distance,
                                                  mz_distance,
                                                  pz_distance,
                                                  z, min_z, max_z);
      return std::vector<double>{gx, gy, gz};
    }
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetSmoothGradient(
      const GRID_INDEX& index, const double nominal_window_size) const
  {
    return GetSmoothGradient4d(GridIndexToLocation(index), nominal_window_size);
  }

  inline std::vector<double> GetSmoothGradient(
      const int64_t x_index, const int64_t y_index, const int64_t z_index,
      const double nominal_window_size) const
  {
    return GetSmoothGradient4d(
          GridIndexToLocation(x_index, y_index, z_index), nominal_window_size);
  }

  inline std::vector<double> GetAutoDiffGradient3d(
      const Eigen::Vector3d& location) const
  {
    return GetAutoDiffGradient(location.x(), location.y(), location.z());
  }

  inline std::vector<double> GetAutoDiffGradient4d(
      const Eigen::Vector4d& location) const
  {
    return GetAutoDiffGradient(location(0), location(1), location(2));
  }

  // TODO: this does not work if you query at cell centers!
  inline std::vector<double> GetAutoDiffGradient(
      const double x, const double y, const double z) const
  {
    const GRID_INDEX index = LocationToGridIndex(x, y, z);
    if (IndexInBounds(index))
    {
      // Use with AutoDiffScalar
      typedef Eigen::AutoDiffScalar<Eigen::Vector4d> AScalar;
      typedef Eigen::Matrix<AScalar, 4, 1> APosition;
      APosition Alocation;
      Alocation(0) = x;
      Alocation(1) = y;
      Alocation(2) = z;
      Alocation(3) = 1.0;
      Alocation(0).derivatives() = Eigen::Vector4d::Unit(0);
      Alocation(1).derivatives() = Eigen::Vector4d::Unit(1);
      Alocation(2).derivatives() = Eigen::Vector4d::Unit(2);
      Alocation(3).derivatives() = Eigen::Vector4d::Unit(3);
      AScalar Adist = EstimateDistanceInterpolateFromNeighbors<AScalar>(
                        Alocation, index.x, index.y, index.z);
      return std::vector<double>{
        Adist.derivatives()(0), Adist.derivatives()(1), Adist.derivatives()(2)};
    }
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetAutoDiffGradient(const GRID_INDEX& index) const
  {
    return GetAutoDiffGradient4d(GridIndexToLocation(index));
  }

  inline std::vector<double> GetAutoDiffGradient(
      const int64_t x_index, const int64_t y_index, const int64_t z_index) const
  {
    return GetAutoDiffGradient4d(
          GridIndexToLocation(x_index, y_index, z_index));
  }

protected:

  inline double ComputeAxisSmoothGradient(
      const std::pair<double, bool>& query_point_distance_estimate,
      const std::pair<double, bool>& minus_axis_distance_estimate,
      const std::pair<double, bool>& plus_axis_distance_estimate,
      const double query_point_axis_value,
      const double minus_point_axis_value,
      const double plus_point_axis_value) const
  {
    if (query_point_distance_estimate.second
        && minus_axis_distance_estimate.second
        && plus_axis_distance_estimate.second)
    {
      const double window_size = plus_point_axis_value
                                 - minus_point_axis_value;
      const double distance_delta = plus_axis_distance_estimate.first
                                    - minus_axis_distance_estimate.first;
      return distance_delta / window_size;
    }
    else if (query_point_distance_estimate.second
             && minus_axis_distance_estimate.second)
    {
      const double window_size = query_point_axis_value
                                 - minus_point_axis_value;
      const double distance_delta = query_point_distance_estimate.first
                                    - minus_axis_distance_estimate.first;
      return distance_delta / window_size;
    }
    else if (query_point_distance_estimate.second
             && plus_axis_distance_estimate.second)
    {
      const double window_size = plus_point_axis_value
                                 - query_point_axis_value;
      const double distance_delta = plus_axis_distance_estimate.first
                                    - query_point_distance_estimate.first;
      return distance_delta / window_size;
    }
    else
    {
      throw std::runtime_error(
            "Window size for GetSmoothGradient is too large for SDF");
    }
  }

  template<typename T>
  inline T BilinearInterpolate(const double low_d1,
                               const double high_d1,
                               const double low_d2,
                               const double high_d2,
                               const T query_d1,
                               const T query_d2,
                               const double l1l2_val,
                               const double l1h2_val,
                               const double h1l2_val,
                               const double h1h2_val) const
  {
    Eigen::Matrix<T, 1, 2> d1_offsets;
    d1_offsets(0, 0) = high_d1 - query_d1;
    d1_offsets(0, 1) = query_d1 - low_d1;
    Eigen::Matrix<T, 2, 2> values;
    values(0, 0) = l1l2_val;
    values(0, 1) = l1h2_val;
    values(1, 0) = h1l2_val;
    values(1, 1) = h1h2_val;
    Eigen::Matrix<T, 2, 1> d2_offsets;
    d2_offsets(0, 0) = high_d2 - query_d2;
    d2_offsets(1, 0) = query_d2 - low_d2;
    const T multiplier = 1.0 / ((high_d1 - low_d1) * (high_d2 - low_d2));
    const T bilinear_interpolated
        = multiplier * d1_offsets * values * d2_offsets;
    return bilinear_interpolated;
  }

  template<typename T>
  inline T BilinearInterpolateDistanceXY(
      const Eigen::Vector4d& corner_location,
      const Eigen::Matrix<T, 4, 1>& query_location,
      const double mxmy_dist, const double mxpy_dist,
      const double pxmy_dist, const double pxpy_dist) const
  {
    return BilinearInterpolate(corner_location(0),
                               corner_location(0) + GetResolution(),
                               corner_location(1),
                               corner_location(1) + GetResolution(),
                               query_location(0),
                               query_location(1),
                               mxmy_dist, mxpy_dist,
                               pxmy_dist, pxpy_dist);
  }

  template<typename T>
  inline T TrilinearInterpolateDistance(
      const Eigen::Vector4d& corner_location,
      const Eigen::Matrix<T, 4, 1>& query_location,
      const double mxmymz_dist, const double mxmypz_dist,
      const double mxpymz_dist, const double mxpypz_dist,
      const double pxmymz_dist, const double pxmypz_dist,
      const double pxpymz_dist, const double pxpypz_dist) const
  {
    // Do bilinear interpolation in the lower XY plane
    const T mz_bilinear_interpolated
        = BilinearInterpolateDistanceXY(corner_location, query_location,
                                        mxmymz_dist, mxpymz_dist,
                                        pxmymz_dist, pxpymz_dist);
    // Do bilinear interpolation in the upper XY plane
    const T pz_bilinear_interpolated
        = BilinearInterpolateDistanceXY(corner_location, query_location,
                                        mxmypz_dist, mxpypz_dist,
                                        pxmypz_dist, pxpypz_dist);
    // Perform linear interpolation/extrapolation between lower and upper planes
    const double inv_resolution = 1.0 / GetResolution();
    const T distance_delta
        = pz_bilinear_interpolated - mz_bilinear_interpolated;
    const T distance_slope = distance_delta * inv_resolution;
    const T query_z_delta = query_location(2) - T(corner_location(2));
    return mz_bilinear_interpolated + (query_z_delta * distance_slope);
  }

  inline double GetCorrectedCenterDistance(const int64_t x_idx,
                                           const int64_t y_idx,
                                           const int64_t z_idx) const
  {
    const std::pair<const float&, bool> query
        = GetImmutable(x_idx, y_idx, z_idx);
    if (query.second)
    {
      const double nominal_sdf_distance = (double)query.first;
      const double cell_center_distance_offset = GetResolution() * 0.5;
      if (nominal_sdf_distance >= 0.0)
      {
        return nominal_sdf_distance - cell_center_distance_offset;
      }
      else
      {
        return nominal_sdf_distance + cell_center_distance_offset;
      }
    }
    else
    {
        throw std::invalid_argument("Index out of bounds");
    }
  }

  template<typename T>
  std::pair<int64_t, int64_t> GetAxisInterpolationIndices(
      const int64_t initial_index,
      const int64_t axis_size,
      const T axis_offset) const
  {
    int64_t lower = initial_index;
    int64_t upper = initial_index;
    if (axis_offset >= 0.0)
    {
      upper = initial_index + 1;
      if (upper >= axis_size)
      {
        upper = initial_index;
        lower = initial_index -1;
        if (lower < 0)
        {
          lower = initial_index;
        }
      }
    }
    else
    {
      lower = initial_index - 1;
      if (lower < 0)
      {
        upper = initial_index + 1;
        lower = initial_index;
        if (upper >= axis_size)
        {
          upper = initial_index;
        }
      }
    }
    return std::make_pair(lower, upper);
  }

  template<typename T>
  inline T EstimateDistanceInterpolateFromNeighbors(
      const Eigen::Matrix<T, 4, 1>& query_location,
      const int64_t x_idx, const int64_t y_idx, const int64_t z_idx) const
  {
    // Get the query location in grid frame
    const Eigen::Matrix<T, 4, 1> grid_frame_query_location
        = GetInverseOriginTransform() * query_location;
    // Switch between all the possible options of where we are
    const Eigen::Vector4d cell_center_location
        = GridIndexToLocationGridFrame(x_idx, y_idx, z_idx);
    const Eigen::Matrix<T, 4, 1> query_offset
        = grid_frame_query_location - cell_center_location.cast<T>();
    // Catch the easiest case
//    if ((query_offset(0) == 0.0)
//        && (query_offset(1) == 0.0)
//        && (query_offset(2) == 0.0))
//    {
//      return GetCorrectedCenterDistance(x_idx, y_idx, z_idx);
//    }
    // Find the best-matching 8 surrounding cell centers
    const std::pair<int64_t, int64_t> x_axis_indices
        = GetAxisInterpolationIndices(x_idx, GetNumXCells(), query_offset(0));
    const std::pair<int64_t, int64_t> y_axis_indices
        = GetAxisInterpolationIndices(y_idx, GetNumYCells(), query_offset(1));
    const std::pair<int64_t, int64_t> z_axis_indices
        = GetAxisInterpolationIndices(z_idx, GetNumZCells(), query_offset(2));
    const Eigen::Vector4d lower_corner_location
        = GridIndexToLocationGridFrame(x_axis_indices.first,
                                       y_axis_indices.first,
                                       z_axis_indices.first);
    const double mxmymz_distance
        = GetCorrectedCenterDistance(x_axis_indices.first,
                                     y_axis_indices.first,
                                     z_axis_indices.first);
    const double mxmypz_distance
        = GetCorrectedCenterDistance(x_axis_indices.first,
                                     y_axis_indices.first,
                                     z_axis_indices.second);
    const double mxpymz_distance
        = GetCorrectedCenterDistance(x_axis_indices.first,
                                     y_axis_indices.second,
                                     z_axis_indices.first);
    const double mxpypz_distance
        = GetCorrectedCenterDistance(x_axis_indices.first,
                                     y_axis_indices.second,
                                     z_axis_indices.second);
    const double pxmymz_distance
        = GetCorrectedCenterDistance(x_axis_indices.second,
                                     y_axis_indices.first,
                                     z_axis_indices.first);
    const double pxmypz_distance
        = GetCorrectedCenterDistance(x_axis_indices.second,
                                     y_axis_indices.first,
                                     z_axis_indices.second);
    const double pxpymz_distance
        = GetCorrectedCenterDistance(x_axis_indices.second,
                                     y_axis_indices.second,
                                     z_axis_indices.first);
    const double pxpypz_distance
        = GetCorrectedCenterDistance(x_axis_indices.second,
                                     y_axis_indices.second,
                                     z_axis_indices.second);
    return TrilinearInterpolateDistance(lower_corner_location,
                                        grid_frame_query_location,
                                        mxmymz_distance, mxmypz_distance,
                                        mxpymz_distance, mxpypz_distance,
                                        pxmymz_distance, pxmypz_distance,
                                        pxpymz_distance, pxpypz_distance);
  }

public:

  inline std::pair<double, bool> EstimateDistance(const double x,
                                                  const double y,
                                                  const double z) const
  {
      return EstimateDistance4d(Eigen::Vector4d(x, y, z, 1.0));
  }

  inline std::pair<double, bool> EstimateDistance3d(
      const Eigen::Vector3d& location) const
  {
    const GRID_INDEX index = LocationToGridIndex3d(location);
    if (IndexInBounds(index))
    {
      return std::make_pair(
            EstimateDistanceInterpolateFromNeighbors<double>(
              Eigen::Vector4d(location.x(), location.y(), location.z(), 1.0),
              index.x, index.y, index.z),
            true);
    }
    else
    {
      return std::make_pair((double)GetOOBValue(), false);
    }
  }

  inline std::pair<double, bool> EstimateDistance4d(
      const Eigen::Vector4d& location) const
  {
    const GRID_INDEX index = LocationToGridIndex4d(location);
    if (IndexInBounds(index))
    {
      return std::make_pair(EstimateDistanceInterpolateFromNeighbors<double>(
                              location, index.x, index.y, index.z),
                            true);
    }
    else
    {
      return std::make_pair((double)GetOOBValue(), false);
    }
  }

  inline Eigen::Vector3d ProjectOutOfCollision(
      const double x, const double y, const double z,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    const Eigen::Vector4d result
        = ProjectOutOfCollision4d(Eigen::Vector4d(x, y, z, 1.0),
                                  stepsize_multiplier);
    return result.head<3>();
  }

  inline Eigen::Vector3d ProjectOutOfCollision3d(
      const Eigen::Vector3d& location,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    return ProjectOutOfCollision(location.x(), location.y(), location.z(),
                                 stepsize_multiplier);
  }

  inline Eigen::Vector4d ProjectOutOfCollision4d(
      const Eigen::Vector4d& location,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    return ProjectOutOfCollisionToMinimumDistance4d(location, 0.0,
                                                    stepsize_multiplier);
  }

  inline Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance(
      const double x, const double y, const double z,
      const double minimum_distance,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    return ProjectOutOfCollisionToMinimumDistance4d(
          Eigen::Vector4d(x, y, z, 1.0),
          minimum_distance, stepsize_multiplier).head<3>();
  }

  inline Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance3d(
      const Eigen::Vector3d& location, const double minimum_distance,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    return ProjectOutOfCollisionToMinimumDistance(
          location.x(), location.y(), location.z(),
          minimum_distance, stepsize_multiplier);
  }

  inline Eigen::Vector4d ProjectOutOfCollisionToMinimumDistance4d(
      const Eigen::Vector4d& location,
      const double minimum_distance,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    // To avoid potential problems with alignment, we need to pass location
    // by reference, so we make a local copy here that we can change.
    // See https://eigen.tuxfamily.org/dox/group__TopicPassingByValue.html
    Eigen::Vector4d mutable_location = location;
    // If we are in bounds, start the projection process,
    // otherwise return the location unchanged
    if (LocationInBounds4d(mutable_location))
    {
      // Add a small collision margin to account for rounding and similar
      const double minimum_distance_with_margin
          = minimum_distance + GetResolution() * stepsize_multiplier * 1e-3;
      const double max_stepsize = GetResolution() * stepsize_multiplier;
      const bool enable_edge_gradients = true;
      double sdf_dist = EstimateDistance4d(mutable_location).first;
      while (sdf_dist <= minimum_distance)
      {
        const std::vector<double> gradient
            = GetGradient4d(mutable_location, enable_edge_gradients);
        if (gradient.size() == 3)
        {
          const Eigen::Vector4d grad_vector(
                gradient[0], gradient[1], gradient[2], 0.0);
          if (grad_vector.norm() > GetResolution() * 0.25) // Sanity check
          {
            // Don't step any farther than is needed
            const double step_distance
                = std::min(max_stepsize,
                           minimum_distance_with_margin - sdf_dist);
            mutable_location += grad_vector.normalized() * step_distance;
            sdf_dist = EstimateDistance4d(mutable_location).first;
          }
          else
          {
            throw std::runtime_error("Encountered flat gradient - stuck");
          }
        }
        else
        {
          throw std::runtime_error("Failed to compute gradient - out of SDF?");
        }
      }
    }
    return mutable_location;
  }

  static void SaveToFile(const SignedDistanceField& sdf,
                         const std::string& filepath,
                         const bool compress);

  static SignedDistanceField LoadFromFile(const std::string& filepath);

  static sdf_tools::SDF GetMessageRepresentation(
      const SignedDistanceField& sdf);

  static SignedDistanceField LoadFromMessageRepresentation(
      const sdf_tools::SDF& message);

  visualization_msgs::Marker ExportForDisplay(const float alpha = 0.01f) const;

  visualization_msgs::Marker ExportForDisplayCollisionOnly(
      const float alpha = 0.01f) const;

  /*
   * The following function can be *VERY EXPENSIVE* to compute, since it
   * performs gradient ascent/descent across the SDF
   */
  VoxelGrid<Eigen::Vector3d> ComputeLocalExtremaMap() const;
};
}

#endif // SDF_HPP


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

using namespace std;

#include "common/types.h"
#include "common/distance.h"
#include "features/galif.h"
#include "quantize/quantizer.h"
#include "index/invertedindex.h"
#include "index/tfidf.h"
#include "io/reader_writer.h"

#include <boost/shared_ptr.hpp>
#include <boost/lexical_cast.hpp>
using namespace sse;

void usage() {
    cout << "Usage: test_similarity image1 image2 k n peak fu wv feature_size vocabulary"<<endl;
}

int main(int argc, char **argv)
{
    if(argc < 10) {
        usage();
        exit(-1);
    }

    cv::Mat image1 = cv::imread(argv[1]);
    cv::Mat image2 = cv::imread(argv[2]);

    uint k;//numofOrient
    uint n;//titles
    try {
        k = boost::lexical_cast<uint>(argv[3]);
        n = boost::lexical_cast<uint>(argv[4]);
    } catch (boost::bad_lexical_cast&) {
        std::cerr << "bad parameter value: "<< argv[3] <<endl;
        std::cerr << "bad parameter value: "<< argv[4] <<endl;
    }

    double peak;
    double fu;
    double wv;
    double size;

    try {
        peak = boost::lexical_cast<double>(argv[5]);
        fu = boost::lexical_cast<double>(argv[6]);
        wv = boost::lexical_cast<double>(argv[7]);
        size = boost::lexical_cast<double>(argv[8]);
    } catch (boost::bad_lexical_cast&) {
        std::cerr << "bad parameter value: "<< argv[5] <<endl;
        std::cerr << "bad parameter value: "<< argv[6] <<endl;
        std::cerr << "bad parameter value: "<< argv[7] <<endl;
        std::cerr << "bad parameter value: "<< argv[8] <<endl;
    }

    double line_width = fu/256.0;
    double lambda = fu/wv;

    Galif *galif = new Galif(
            256,
            k,
            n,
            peak, line_width, lambda,
            size, true,
            "l2", "grid",
            625);

    KeyPoints_t keypoints1;
    Features_t features1;
    galif->compute(image1, keypoints1, features1);

    KeyPoints_t keypoints2;
    Features_t features2;
    galif->compute(image2, keypoints2, features2);

    Vocabularys_t vocabulary;
    read(argv[9], vocabulary);

    //QuantizerHard
    Quantizer_fn quantizer = sse::QuantizerHard<Vec_f32_t, L2norm_squared<Vec_f32_t> >();

    Vec_f32_t hist1, hist2;
    quantize(features1, vocabulary, hist1, quantizer);
    quantize(features2, vocabulary, hist2, quantizer);

    InvertedIndex index(vocabulary.size());
    index.addSample(hist1);
    TF_simple tf;
    IDF_simple idf;
    index.createIndex(tf, idf);

    std::vector<ResultItem_t> results;
    index.query(hist2, tf, idf, 1, results);

    float similarity = results[0].first;
    cout << similarity<<endl;

    delete galif;
    return 0;
}


// Author(s): Gijs Kant
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file quotienting_test.cpp
/// \brief Add your file description here.

#include <iostream>
#include <iterator>
#include <boost/test/included/unit_test_framework.hpp>
#include "mcrl2/utilities/test_utilities.h"
#include "mcrl2/lps/parse.h"
#include "mcrl2/modal_formula/parelm.h"
#include "mcrl2/modal_formula/parse.h"

using namespace mcrl2;

BOOST_AUTO_TEST_CASE(test_parelm)
{
  const std::string LPS =
    "act a;                                  \n"
    "    b : Nat;                            \n"
    "                                        \n"
    "proc P = P;                             \n"
    "                                        \n"
    "init P;                                 \n"
    ;

  const std::string FORMULA_TEXT =
    "nu X(c: Pos = 2, d: Nat = 1) . X(c, d) && val(d > 0) && mu Y(e: Nat = d+1) . [b(e)](X(c, e) || Y(e))";

  const std::string EXPECTED_RESULT_TEXT =
    "nu X(d: Nat = 1) . X(d) && val(d > 0) && mu Y(e: Nat = d+1) . [b(e)](X(e) || Y(e))";

  lps::specification spec = lps::parse_linear_process_specification(LPS);

  state_formulas::state_formula formula = state_formulas::parse_state_formula(FORMULA_TEXT, spec);
  state_formulas::state_formula expected_result = state_formulas::parse_state_formula(EXPECTED_RESULT_TEXT, spec);

  std::clog << "formula: " << pp(formula) << std::endl;

  std::clog << "eliminating unused parameters..." << std::endl;
  state_formulas::state_formula_parelm_algorithm algorithm;
  algorithm.run(formula);

  std::clog << "result: " << pp(formula) << std::endl;

  std::string result_text = pp(formula);
  BOOST_CHECK(formula == expected_result);
}

boost::unit_test::test_suite* init_unit_test_suite(int argc, char* argv[])
{
  return 0;
}


#ifndef ALEPH_PERSISTENCE_DIAGRAMS_DISTANCES_BOTTLENECK_HH__
#define ALEPH_PERSISTENCE_DIAGRAMS_DISTANCES_BOTTLENECK_HH__

#include <aleph/geometry/distances/Infinity.hh>

#include <aleph/persistenceDiagrams/PersistenceDiagram.hh>
#include <aleph/persistenceDiagrams/distances/detail/Orthogonal.hh>

#include <boost/iterator/counting_iterator.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/max_cardinality_matching.hpp>

namespace aleph
{

namespace distances
{

namespace detail
{

template <class T> struct Edge
{
  std::size_t source;
  std::size_t target;

  T weight;

  Edge( std::size_t s, std::size_t t, T w )
    : source( s )
    , target( t )
    , weight( w )
  {
  }

  bool operator<( const Edge& other ) const
  {
    return weight < other.weight;
  }
};

template <class T> struct CheckMatchingCardinality
{
  using GraphType          = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
  using MatchingVectorType = std::vector<boost::graph_traits<GraphType>::vertex_descriptor>;

  CheckMatchingCardinality( std::size_t size, typename std::vector<Edge<T> >::const_iterator begin )
    : _maximumSize( size )
    , _last( begin )
    , _graph( 2 * size )
    , _mates( 2 * size )
  {
    boost::add_edge( begin->source, begin->target, _graph );
  }

  bool operator()( typename std::vector<Edge<T> >::const_iterator /* it1 */, typename std::vector<Edge<T> >::const_iterator it2 )
  {
    // The new edge lies beyond the edges that are already known, so the
    // edges between the last edge and the new iterator position need to
    // be added.
    if( it2 > _last )
    {
      do
      {
        ++_last;
        boost::add_edge( _last->source, _last->target, _graph );
      }
      while( _last != it2 );
    }

    // The new edge lies behind the edges that are already known, so the
    // surplus edges need to be removed.
    else
    {
      do
      {
        boost::remove_edge( _last->source, _last->target, _graph );
        --_last;
      }
      while( _last != it2 );
    }

    boost::edmonds_maximum_cardinality_matching( _graph,
                                                 &_mates[0] );

    // Look out for _perfect matchings_ in the bipartite graph. Any other
    // maximum cardinality matching does not qualify for the Bottleneck
    // distance.
    return boost::matching_size( _graph, &_mates[0] ) == _maximumSize;
  }

  std::size_t _maximumSize;

  typename std::vector<Edge<T> >::const_iterator _last;

  GraphType          _graph; // Input data
  MatchingVectorType _mates; // Edges of the matching
};

} // namespace detail

/**
  Calculates the Bottleneck distance between two persistence diagrams.
  The algorithm used for this involves checking a (complete) bipartite
  graph for perfect matchings.

  A brief description of the algoritmh is given in

    Computational Topology
    Herbert Edelsbrunner and John Harer

  on page 191.

  The implementation has been inspired by Dmitriy Morozov's "Dionysus"
  framework.

  @param D1 First persistence diagram
  @param D2 Second persistence diagram

  @returns Bottleneck distance between the two persistence diagrams
*/

template <
  class DataType,
  class Distance = aleph::geometry::distances::InfinityDistance<DataType>
> DataType bottleneckDistance( const PersistenceDiagram<DataType>& D1,
                               const PersistenceDiagram<DataType>& D2 )
{
  auto n           = D1.size();
  auto m           = D2.size();
  auto maximumSize = n + m;

  using SizeType = decltype(n);
  using Edge     = detail::Edge<DataType>;

  std::vector<Edge> edges;

  // Diagonal edges ----------------------------------------------------

  for( SizeType i = n; i < maximumSize; i++ )
    for( SizeType j = maximumSize + i; j < 2 * maximumSize; j++ )
      edges.push_back( Edge( static_cast<std::size_t>(i), static_cast<std::size_t>(j), DataType() ) );

  SizeType i = 0;

  // Edges between regular points --------------------------------------

  Distance distance;

  for( auto it1 = D1.begin(); it1 != D1.end(); ++it1 )
  {
    auto j = maximumSize;

    for( auto it2 = D2.begin(); it2 != D2.end(); ++it2 )
    {
      auto weight = distance( *it1, *it2 );

      edges.push_back( Edge( static_cast<std::size_t>(i), static_cast<std::size_t>(j),
                             weight ) );

      ++j;
    }

    ++i;
  }

  // Edges between points and their projections ------------------------

  i = 0;

  for( auto it1 = D1.begin(); it1 != D1.end(); ++it1 )
  {
    edges.push_back( Edge( static_cast<std::size_t>(i), static_cast<std::size_t>(maximumSize + m + i),
                           aleph::distances::detail::orthogonalDistance<Distance>( *it1 ) ) );

    ++i;
  }

  i = maximumSize;

  for( auto it2 = D2.begin(); it2 != D2.end(); ++it2 )
  {
    edges.push_back( Edge( static_cast<std::size_t>(n + i - maximumSize), static_cast<std::size_t>(i),
                           aleph::distances::detail::orthogonalDistance<Distance>( *it2 ) ) );
  }

  // Identify matchings ------------------------------------------------

  std::sort( edges.begin(), edges.end() );

  // Perform binary search over edge sets. Starting from the empty graph, use
  // more and more edges to find out the first graph that permits a maximum
  // cardinality matching.

  using EdgeIteratorType     = typename std::vector<Edge>::const_iterator;
  using CountingIteratorType = boost::counting_iterator<EdgeIteratorType>;

  auto itEdge = std::upper_bound( CountingIteratorType( edges.begin() ),
                                  CountingIteratorType( edges.end() ),
                                  edges.begin(),
                                  detail::CheckMatchingCardinality<DataType>( maximumSize, edges.begin() ) );

  return (*itEdge)->weight;
}

} // namespace distances

} // namespace aleph

#endif


//
// Copyright 2013 Christian Henning
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#include <boost/gil.hpp>
#include <boost/gil/extension/toolbox/metafunctions.hpp>
#include <boost/gil/extension/toolbox/metafunctions/pixel_bit_size.hpp>

namespace gil = boost::gil;

void test_pixel_bit_size() {
  using image_t = gil::bit_aligned_image5_type<16, 16, 16, 8, 8,
                                               gil::devicen_layout_t<5>>::type;
  static_assert(gil::pixel_bit_size<image_t::view_t::reference>::value == 64,
                "");
}

int main() { test_pixel_bit_size(); }


// segment.cpp
// initial program to apply c++ tools from various libraries, including vlfeat and opencv
// to recreate the pipeline of cell segmentation described in the paper
// by Zhi Lu et al. :
// https://cs.adelaide.edu.au/~zhi/publications/paper_TIP_Jan04_2015_Finalised_two_columns.pdf

/*
The following is about linking libraries - still applicable, but hopefully
most of it is taken care of via `make`

Linking the vlfeat library requires some doing:

g++ segment.cpp -I$VLROOT -L$VLROOT/bin/glnxa64/ -lvl
where $VLROOT is the path to your vlroot source files, so ex: /usr/local/lib/vlfeat-0.9.21

then setup and env variable in the terminal,
export LD_LIBRARY_PATH=/usr/local/lib/vlfeat-0.9.21/bin/glnxa64
(or append this to the existing LD_LIBRARY_PATH if needed)

then run the executable and cross your fingers

For command line arguments:
how to use reference: https://theboostcpplibraries.com/boost.program_options
get boost on linux: https://stackoverflow.com/questions/12578499/how-to-install-boost-on-ubuntu
		/use the apt-get version

compliation example: g++ -std=gnu++17 segment.cpp -l boost_program_options
	Make sure to include the "-l boost_program_options" otherwise you will get errors that do not make sense and the program will not work
*/

#include <iostream>
#include "opencv2/opencv.hpp"
#include "Segmenter.cpp"
#include <boost/program_options.hpp>

using namespace std;
using namespace boost::program_options;

int main (int argc, const char * argv[])
{
    // Default parameters
    // Quickshift params
    int kernelsize = 2;
    int maxdist = 4;
    // Canny params
    int threshold1 = 20;
    int threshold2 = 40;
    // GMM params
    int maxGmmIterations = 10;
    // GMM post processing params
    double minAreaThreshold = 200.0;
    // MSER params
    int delta = 8, minArea = 15, maxArea = 100;
    double maxVariation = 0.5, minDiversity = 0.25;

    try
    {
        options_description desc{"Options"};
        desc.add_options()
          ("help,h", "Help screen")
          ("maxVariation", value<float>()->default_value(maxVariation), "MaxVariation")
          ("minDiversity", value<float>()->default_value(minDiversity), "MinDiversity")
          ("minAreaThreshold", value<float>()->default_value(minAreaThreshold), "minAreaThreshold")
          ("kernelsize", value<int>()->default_value(kernelsize), "Kernelsize")
          ("maxdist", value<int>()->default_value(maxdist), "Maxdist")
          ("threshold1", value<int>()->default_value(threshold1), "threshold1")
          ("threshold2", value<int>()->default_value(threshold2), "threshold2")
          ("maxGmmIterations", value<int>()->default_value(maxGmmIterations), "maxGmmIterations")
          ("delta", value<int>()->default_value(delta), "delta")
          ("minArea", value<int>()->default_value(minArea), "minArea")
          ("maxArea", value<int>()->default_value(maxArea), "maxArea")
          ("photo,p", value<std::string>()->default_value("cyto.tif"), "photo");

        variables_map vm;
        store(parse_command_line(argc, argv, desc), vm);
        notify(vm);

        segment::Segmenter seg = segment::Segmenter(
            vm["kernelsize"].as<int>(),
            vm["maxdist"].as<int>(),
            vm["threshold1"].as<int>(),
        	vm["threshold2"].as<int>(),
            vm["maxGmmIterations"].as<int>(),
            vm["minAreaThreshold"].as<float>(),
            vm["delta"].as<int>(),
        	vm["minArea"].as<int>(),
            vm["maxArea"].as<int>(),
            vm["maxVariation"].as<float>(),
            vm["minDiversity"].as<float>()
        );

        seg.runSegmentation(vm["photo"].as<std::string>());
    }
    catch (const error &ex)
    {
      std::cerr << ex.what() << '\n';
    }
}


#include "geom/Vector2D.hpp"
#include "geom/Transform.hpp"
#include "geom/BoundingBox.hpp"
#include "geom/Math.hpp"
#include "geom/CollisionCheck.hpp"

#include <boost/python.hpp>

void export_geom()
{
    namespace bp = boost::python;
    namespace ag = avs::geom;

    // Make from avsimpy.geometrics import ... work
    bp::object utilModule(bp::handle<>(bp::borrowed(PyImport_AddModule("avsimpy.geometrics"))));
    // Make from avsimpy import geometrics work
    bp::scope().attr("geometrics") = utilModule;
    bp::scope util_scope = utilModule;

    bp::class_<ag::Vector2D>("Vector2D")
        .def(bp::init<const float, const float>((bp::arg("x")=0.0f, bp::arg("y")=0.0f)))
        .def(bp::init<const ag::Vector2D>((bp::arg("vector"))))
        .def_readwrite("x", &ag::Vector2D::x)
        .def_readwrite("y", &ag::Vector2D::y)
        .def("get_length", &ag::Vector2D::get_length)
        .def("get_unit_vector", &ag::Vector2D::get_unit_vector)
        .def("__eq__", &ag::Vector2D::operator==)
        .def("__ne__", &ag::Vector2D::operator!=)
        .def(bp::self += bp::self)
        .def(bp::self + bp::self)
        .def(bp::self -= bp::self)
        .def(bp::self - bp::self)
        .def(bp::self *= double())
        .def(bp::self * double())
        .def(double() * bp::self)
        .def(bp::self /= double())
        .def(bp::self / double())
        .def(double() / bp::self)
        .def(bp::self_ns::str(bp::self_ns::self))
    ;

    bp::class_<ag::Transform>("Transform")
        .def(bp::init<const ag::Vector2D, const float>(
            (bp::arg("location")=ag::Vector2D(), bp::arg("rotation")=0.0f)))
        .def(bp::init<const ag::Transform&>((bp::arg("transform"))))
        .def_readwrite("location", &ag::Transform::location)
        .def_readwrite("rotation", &ag::Transform::rotation)
        .def("get_forward_vector", &ag::Transform::get_forward_vector)
        .def("calculate_distance", &ag::Transform::calculate_distance)
        .def("__eq__", &ag::Transform::operator==)
        .def("__ne__", &ag::Transform::operator!=)
        .def(bp::self_ns::str(bp::self_ns::self))
    ;

    bp::class_<ag::BoundingBox>("BoundingBox")
        .def(bp::init<const ag::Vector2D, const ag::Vector2D>(
            (bp::arg("location")=ag::Vector2D(), bp::arg("extent")=ag::Vector2D())))
        .def(bp::init<const float, const float, const float, const float>(
            (bp::arg("x")=0.0f, bp::arg("y")=0.0f, bp::arg("extent_x")=1.0f, bp::arg("extent_y")=1.0f)))
        .def(bp::init<const ag::BoundingBox&>((bp::arg("bounding_box"))))
        .def_readwrite("location", &ag::BoundingBox::center)
        .def_readwrite("extent", &ag::BoundingBox::extent)
        .def("contains", &ag::BoundingBox::contains, bp::arg("point"))
        .def("intersects", &ag::BoundingBox::intersects, bp::arg("bounding_box"))
        .def("__eq__", &ag::BoundingBox::operator==)
        .def("__ne__", &ag::BoundingBox::operator!=)
        .def(bp::self_ns::str(bp::self_ns::self))
    ;

    bp::class_<ag::Math>("math")
        .def("to_radian", &ag::Math::to_radian, bp::arg("deg"))
        .def("to_degree", &ag::Math::to_degree, bp::arg("rad"))
        .def("dot_product", &ag::Math::dot_product, (bp::arg("vector_A"), bp::arg("vector_B")))
        .def("calculate_distance", &ag::Math::calculate_distance, (bp::arg("vector_A"), bp::arg("vector_B")))
        .def("calculate_angle", &ag::Math::calculate_angle, (bp::arg("vector_A"), bp::arg("vector_B")))
        .def("get_forward_vector", &ag::Math::get_forward_vector, bp::arg("deg"))
        .def("rotate_vector_on_origin", &ag::Math::rotate_vector_on_origin, (bp::arg("vector"), bp::arg("deg")))
        .def("rotate_vector_on_point", &ag::Math::rotate_vector_on_point, (bp::arg("vector"), bp::arg("deg"), bp::arg("point")))
    ;

    bp::class_<ag::CollisionChecker>("CollisionChecker")
        .def("check_collision", &ag::CollisionChecker::check_collision, (bp::arg("entity_A"), bp::arg("entity_B")))
        .def(bp::self_ns::str(bp::self_ns::self))
    ;

}


/*
 * TypeDefs.hpp
 *
 *  Created on: March 18, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

// Eigen

#ifndef EIGEN_FUNCTORS_PLUGIN
#define EIGEN_FUNCTORS_PLUGIN "grid_map_core/eigen_plugins/FunctorsPlugin.hpp"
#endif

#ifndef EIGEN_DENSEBASE_PLUGIN
#define EIGEN_DENSEBASE_PLUGIN "grid_map_core/eigen_plugins/DenseBasePlugin.hpp"
#endif

#include <Eigen/Core>

#pragma once

namespace grid_map {

  typedef Eigen::MatrixXf Matrix;
  typedef Eigen::Vector2d Position;
  typedef Eigen::Vector2d Vector;
  typedef Eigen::Vector3d Position3;
  typedef Eigen::Vector3d Vector3;
  typedef Eigen::Array2i Index;
  typedef Eigen::Array2i Size;
  typedef Eigen::Array2d Length;
  typedef uint64_t Time;

} /* namespace */


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"MinimumJerkInterpolation.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include "right_hand.h"
#include "left_hand.h"
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseArray.h>
#include <cstdlib>
#include<termios.h>
#include <iostream>
#include <QTime>
#include"pidcontroller.h"

using namespace  std;
using namespace  Eigen;

double X_face=320;
double Y_face=200;
PIDController head_yaw_pid;
PIDController head_pitch_pid;

VectorXd r_bottle_cam(3);
VectorXd r_bottle_shoulder(3);
double head_yaw=0;
double head_pitch=0;
double head_roll=0;
int n;
double saturate(double a, double min, double max){
    if(a<min){//ROS_INFO("subceeding!");
        return min;}
    else if(a>max){//ROS_INFO("exceeding!");
        return max;}
    else{return a;}
}

void numplot(double num,double min,double max){
    //⬛

    QString str;
    int l=100;
    int n=int((num-min)/(max-min)*l);
    if (num<min){n=0;}
    if (num>max){n=100;}
    str+=QString::number(min);
    str+="|";
    if (n<=l/2){
        for (int i = 0; i < n; ++i) {
            str+=" ";
        }
        for (int i = 0; i < l/2-n; ++i) {
            str+="|";

        }
        str+="|";
        for (int i = 0; i < l/2; ++i) {
            str+=" ";
        }
    }
    else {
        for (int i = 0; i < l/2; ++i) {
            str+=" ";
        }
        for (int i = 0; i < n-l/2; ++i) {
            str+="|";

        }
        str+="|";
        for (int i = 0; i < l-n; ++i) {
            str+=" ";
        }

    }

    str+="|";
    str+=QString::number(max);
    str+="=>";str+=QString::number(num);
    qDebug()<<str;
    qDebug()<<"";

}

void matrix_view(MatrixXd M){

    for (int i = 0; i <M.rows() ; ++i) {
        QString str;
        for (int j = 0; j <M.cols() ; ++j) {
            str+=QString::number(M(i,j));
            str+="   ";
        }
        qDebug()<<str;
    }
    qDebug()<<"";
}

void matrix_view(VectorXd M){
    QString str;
    for (int i = 0; i <M.rows() ; ++i) {str+=QString::number(M(i));str+="   ";}
    qDebug()<<str;
    qDebug()<<"";
}

double move2pose(double max,double t_local,double T_start ,double T_end){
    double T_move=T_end-T_start;
    double c3=(10*max)/pow(T_move,3);
    double c4=-(15*max)/pow(T_move,4);
    double c5=(6*max)/pow(T_move,5);
    double theta=0;
    if(t_local<T_start){theta=0;}
    else if (t_local<T_end){theta=c3*pow(t_local-T_start,3)+c4*pow(t_local-T_start,4)+c5*pow(t_local-T_start,5);}
    else{theta=max;}
    return theta;
}

int getch()
{
    static struct termios oldt, newt;
    tcgetattr( STDIN_FILENO, &oldt);           // save old settings
    newt = oldt;
    newt.c_lflag &= ~(ICANON);                 // disable buffering
    tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

    int c = getchar();  // read character (non-blocking)

    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
    return c;
}

int qc_offset[12];
int qra_offset[4];
int qla_offset[4];
bool qc_initial_bool;
void qc_initial(const sensor_msgs::JointState & msg){
    if (qc_initial_bool){

        for (int i = 0; i < 12; ++i) {
            qc_offset[i]=int(msg.position[i+1]);

        }

        for (int i = 12; i < 16; ++i){
            qra_offset[i-12]=int(msg.position[i+1]);
        }

        for (int i = 20; i < 24; ++i){
            qla_offset[i-20]=int(msg.position[i+1]);
        }

        qc_initial_bool=false;

        ROS_INFO("Offset_feet=%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\n",
                 qc_offset[0],qc_offset[1],qc_offset[2],qc_offset[3],qc_offset[4],
                qc_offset[5],qc_offset[6],qc_offset[7],qc_offset[8],qc_offset[9],
                qc_offset[10],qc_offset[11]);
        ROS_INFO("Offset_righthand=%d\t%d\t%d\t%d",qra_offset[0],qra_offset[1], qra_offset[2],qra_offset[3]);
        ROS_INFO("Offset_lefthand=%d\t%d\t%d\t%d",qla_offset[0],qla_offset[1], qla_offset[2],qla_offset[3]);
        ROS_INFO("Initialized!");
    }
}

VectorXd r_camera2shoulder(VectorXd r_camera)
{
    double roll2pitch=.05;
    double yaw2roll=.1;
    VectorXd r_neck(3);
    r_neck<<.03,0.3,0;

    MatrixXd T0(4,4);
    T0<<1,0,0,0,  0,cos(M_PI/12),-sin(M_PI/12),0,  0,sin(M_PI/12),cos(M_PI/12),0,  0,0,0,1;
    MatrixXd T1(4,4);
    T1<<1,0,0,r_camera(0),  0,1,0,r_camera(1),  0,0,1,r_camera(2),  0,0,0,1;
  MatrixXd T2(4,4);
  T2<<1,0,0,0,  0,cos(head_pitch),-sin(head_pitch),0,  0,sin(head_pitch),cos(head_pitch),0,  0,0,0,1;
  MatrixXd T3(4,4);
  T3<<cos(head_roll),0,sin(head_roll),0,  0,1,0,0,    -sin(head_roll),0,cos(head_roll),roll2pitch,   0,0,0,1;
  MatrixXd T4(4,4);
  T4<<cos(head_yaw),-sin(head_yaw),0,0,  sin(head_yaw),cos(head_yaw),0,0,  0,0,1,yaw2roll,  0,0,0,1;
  MatrixXd T5(4,4);
  T5<<1,0,0,r_neck(0),  0,1,0,r_neck(1),  0,0,1,r_neck(2),  0,0,0,1;
  MatrixXd T(4,4);
  T=T5*T4*T3*T2*T1*T0;
  VectorXd temp(4); temp<<r_camera,1;
  temp=T*temp;

  return temp.block(0,0,3,1);

}
void object_detect(const geometry_msgs::PoseArray & msg){
    double l=640;
    double w=480;
    double a=.51;
    double b=.385;
    double X0=.493;
    double Y0,Z0,L0,X,Y,Z;
    double x=l/2;
    double y=w/2;
    double z=0;
  int n=msg.poses.size();
  for (int i = 0; i < n; ++i) {
      if (msg.poses[i].orientation.x==39) {
         x=msg.poses[i].position.x;
         y=msg.poses[i].position.y;
         z=msg.poses[i].position.z;
        break;
      }
 }
Y0=-(x-l/2)/l*a;
Z0=-(y-w/2)/w*b;
L0=sqrt(X0*X0+Y0*Y0+Z0*Z0);
X=z/L0*X0;
Y=X*Y0/X0;
Z=X*Z0/X0;
if(X!=0){r_bottle_cam<<X,Y,Z;}
r_bottle_shoulder=r_camera2shoulder(r_bottle_cam);
matrix_view(r_bottle_shoulder);

}

void face_detect(const geometry_msgs::PoseArray & msg){

  n=msg.poses.size();

if(n!=0) {
      X_face=msg.poses[0].position.x;
      Y_face=msg.poses[0].position.y;
  }

}

// waist
double Waist2ArmZ=0.2694;
double Waist2RArmY=-0.239;
double Waist2LArmY=0.239;
//double WaistYaw=0; double WaistPitch=0;
//MatrixXd R_shoulder(3,3);
//VectorXd r_left_shoulder(3);
//VectorXd r_right_shoulder(3);
//    R_shoulder<<cos(WaistYaw)*cos(WaistPitch), -sin(WaistYaw), cos(WaistYaw)*sin(WaistPitch),
//            cos(WaistPitch)*sin(WaistYaw),  cos(WaistYaw), sin(WaistYaw)*sin(WaistPitch),
//            -sin(WaistPitch),              0,               cos(WaistPitch);
//    r_left_shoulder<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2LArmY*sin(WaistYaw),
//                      Waist2LArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2LArmY,
//                                                               Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;
//    r_right_shoulder<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2RArmY*sin(WaistYaw),
//                       Waist2RArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2RArmY,
//                                                                Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;
MatrixXd rightshoulder2waist(double WaistYaw, double WaistPitch){
    MatrixXd T(4,4);
    MatrixXd R(3,3);
    VectorXd P(3);
    R<<cos(WaistYaw)*cos(WaistPitch), -sin(WaistYaw), cos(WaistYaw)*sin(WaistPitch),
            cos(WaistPitch)*sin(WaistYaw),  cos(WaistYaw), sin(WaistYaw)*sin(WaistPitch),
            -sin(WaistPitch),              0,               cos(WaistPitch);

    P<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2RArmY*sin(WaistYaw),
            Waist2RArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2RArmY,
            Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;

    T<<R.transpose(),-R.transpose()*P,0,0,0,1;

    return T;
}
MatrixXd leftshoulder2waist(double WaistYaw, double WaistPitch){
    MatrixXd T(4,4);
    MatrixXd R(3,3);
    VectorXd P(3);
    R<<cos(WaistYaw)*cos(WaistPitch), -sin(WaistYaw), cos(WaistYaw)*sin(WaistPitch),
            cos(WaistPitch)*sin(WaistYaw),  cos(WaistYaw), sin(WaistYaw)*sin(WaistPitch),
            -sin(WaistPitch),              0,               cos(WaistPitch);

    P<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2LArmY*sin(WaistYaw),
            Waist2LArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2LArmY,
            Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;                                                                1;
    T<<R.transpose(),-R.transpose()*P,0,0,0,1;
    return T;
}

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

    qc_initial_bool=true;

    ros::init(argc, argv, "head");
    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",100);
    ros::Rate loop_rate(200);
    ros::Subscriber qcinit = nh.subscribe("/surena/inc_joint_state", 1000, qc_initial);
    ros::Subscriber object_sub=nh.subscribe("/ai/object", 1000, object_detect);
    ros::Subscriber face_sub=nh.subscribe("/ai/face", 1000, face_detect);
    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);

    head_yaw_pid.Init(.005,-45,45,.01,0,0);
    head_pitch_pid.Init(.005,-15,15,.01,0,0);
double time=0;

//QTime chronometer;
//chronometer.start();
    while (ros::ok())
    {

   // chronometer.restart();

//        if (qc_initial_bool) {
//            ROS_INFO_ONCE("qc is initializing!");
//            ros::spinOnce();
//            continue;
//        }

//head_yaw=head_yaw_pid.Calculate(320,X_face);
//head_pitch=head_pitch_pid.Calculate(200,Y_face);
        if(n!=0){
            head_yaw+=.0004*(320-X_face);
            head_pitch-=.0004*(200-Y_face);
        }
        else{
           // head_yaw-=.005*head_yaw;
            //head_pitch-=.005*head_pitch;
        }

        head_yaw=saturate(head_yaw,-45,45);
        head_pitch=saturate(head_pitch,-15,15);

//qDebug()<<"X_face:  "<<X_face<<"\tY_face:  "<<Y_face;
//numplot(head_yaw,-45,45);
//numplot(head_pitch,-15,15);

int head_yaw_motor,head_pitch_motor;
head_pitch_motor=int(head_pitch/30*342);
head_yaw_motor=int(head_yaw/30*342);

//head_pitch_motor=int(20*sin(time/10)/30*342);
//head_yaw_motor=int(20*sin(time/10)/30*342);

//int hand_yaw_motor=int((20*sin(time))*(4000-2050)/(23));

//qDebug()<<head_pitch_motor<<"\t"<<head_yaw_motor;
    msg.data.clear();
    for(int  i = 0;i < 12;i++)
    {
        msg.data.push_back(qc_offset[i]);
    }
    //right hand epose
    msg.data.push_back(qra_offset[0]);//12 -y  a,z
    msg.data.push_back(qra_offset[1]);//13 +x
    msg.data.push_back(qra_offset[2]);//14 -z
    msg.data.push_back(qra_offset[3]);//15 +y
    //right hand dynamixel + fingers
    msg.data.push_back(0);//16
    msg.data.push_back(0);//17
    msg.data.push_back(0);//18
    msg.data.push_back(0);//19
    //left hand epose
    msg.data.push_back(qla_offset[0]);//20 +y
    msg.data.push_back(qla_offset[1]);//21 +x
    msg.data.push_back(qla_offset[2]);//22 -z
    msg.data.push_back(qla_offset[3]);//23 -y
    //left hand dynamixel + fingers
    msg.data.push_back(0);//24
    msg.data.push_back(0);//25
    msg.data.push_back(0);//26
    msg.data.push_back(0);//27
    msg.data.push_back(0);//waist pitch 28
    msg.data.push_back(0);//waist yaw 29
    msg.data.push_back(head_pitch_motor);//head pitch 30
    msg.data.push_back(0);//head roll 31
    msg.data.push_back(head_yaw_motor);//head yaw 32

    chatter_pub.publish(msg);

    ros::spinOnce();
    loop_rate.sleep();
    time+=.005;
}

return 0;
}


#ifndef CLIPPER_LIB_CONTAINERS_HPP
#define CLIPPER_LIB_CONTAINERS_HPP

constexpr int DEFAULT_BATCH_SIZE = -1;

#include <cmath>
#include <memory>
#include <random>
#include <unordered_map>

#include <boost/circular_buffer.hpp>

#include <clipper/datatypes.hpp>
#include <clipper/dlib_dependencies.hpp>
#include <clipper/metrics.hpp>
#include <clipper/threadpool.hpp>
#include <clipper/util.hpp>

namespace clipper {

// We use the system clock for the deadline time point
// due to its cross-platform consistency (consistent epoch, resolution)
using Deadline = std::chrono::time_point<std::chrono::system_clock>;

enum class BatchSizeDeterminationMethod {
  Default = 0,
  Exploration = 1,
  Estimation = 2
};

// pair of batch size, method by which the batch size was determined
using BatchSizeInfo = std::pair<size_t, BatchSizeDeterminationMethod>;

class ModelContainer {
 public:
  ~ModelContainer();
  ModelContainer(VersionedModelId model, int container_id, int replica_id,
                 InputType input_type, int batch_size);
  // disallow copy
  ModelContainer(const ModelContainer &) = delete;
  ModelContainer &operator=(const ModelContainer &) = delete;

  ModelContainer(ModelContainer &&) = default;
  ModelContainer &operator=(ModelContainer &&) = default;

  BatchSizeInfo get_batch_size(Deadline deadline);
  void add_processing_datapoint(size_t batch_size,
                                long long processing_latency_micros);
  void send_feedback(PredictTask task);
  void set_batch_size(int batch_size);
  void set_inactive();
  bool is_active();

  VersionedModelId model_;
  int container_id_;
  int replica_id_;
  InputType input_type_;
  int batch_size_;
  clipper::metrics::Histogram latency_hist_;

 private:
  using EstimatorLatency = dlib::matrix<double, 1, 1>;
  using EstimatorBatchSize = double;
  using EstimatorKernel = dlib::linear_kernel<EstimatorLatency>;
  using Estimator = dlib::decision_function<EstimatorKernel>;
  // Tuple of num latencies, mean latency, latency std
  using LatencyInfo = std::tuple<double, double, double>;

  // Updates the specified LatencyInfo tuple to account
  // for a new latency entry. The info tuple's mean and
  // standard deviation are updated recursively; the recursive
  // standard deviation relation is defined here:
  // https://link.springer.com/article/10.1007%2FBF02262936
  LatencyInfo update_mean_std(LatencyInfo &info, double new_latency);

  std::shared_ptr<bool> active_;
  std::shared_ptr<std::mutex> activity_mtx_;

  std::atomic_bool connected_{true};

  Queue<FeedbackTask> feedback_queue_;
  boost::shared_mutex datapoints_mtx_;
  std::unordered_map<EstimatorBatchSize, LatencyInfo> processing_datapoints_;

  size_t max_batch_size_;
  long long max_latency_;
  Estimator estimator_;
  dlib::rr_trainer<EstimatorKernel> estimator_trainer_;
  std::mutex estimator_mtx_;

  // Exploration and estimation parameters
  // for adaptive batching
  double explore_dist_mu_;
  double explore_dist_std_;
  double budget_decay_;
  std::normal_distribution<double> exploration_distribution_;
  std::default_random_engine exploration_engine_;

  static constexpr size_t HISTOGRAM_SAMPLE_SIZE = 256;
  // The minimum number of latency entries associated with a batch
  // size that must exist in order to continue exploration
  // or incorporate variance data
  static constexpr uint32_t MINIMUM_BATCH_SAMPLE_SIZE = 5;
  static constexpr uint32_t LATENCY_Z_SCORE = 3;
  static constexpr double REGRESSION_DATA_SCALE_FACTOR = .001;
  static constexpr size_t ADDITIVE_EXPANSION_THRESHOLD = 10;

  void fit_estimator();
  size_t explore();
  size_t estimate(double budget);
};

namespace IterativeUpdater {

/**
 * Given information about a data set's previous mean, std, and cardinality,
 * as well as a new value, calculates the mean and std of the data set augmented
 * by the new value
 *
 * @return A pair consisting of (augmented_mean, augmented_std)
 */
inline std::pair<double, double> calculate_new_mean_std(double prev_num_samples,
                                                        double prev_mean,
                                                        double prev_std,
                                                        double new_value) {
  double new_num_samples = prev_num_samples + 1;
  double new_mean =
      ((prev_num_samples * prev_mean) + new_value) / new_num_samples;

  double old_s = std::pow(prev_std, 2) * prev_num_samples;
  double new_s = old_s + ((new_num_samples / std::max(1.0, prev_num_samples)) *
                          std::pow((new_mean - new_value), 2));
  double new_std = std::sqrt(new_s / std::max(1.0, new_num_samples));

  return std::make_pair(new_mean, new_std);
}
}  // namespace IterativeUpdater

/// This is a lightweight wrapper around the map of active containers
/// to make it threadsafe so it can be safely shared between threads between
/// with a shared_ptr.
class ActiveContainers {
 public:
  explicit ActiveContainers();

  // Disallow copy
  ActiveContainers(const ActiveContainers &) = delete;
  ActiveContainers &operator=(const ActiveContainers &) = delete;

  ActiveContainers(ActiveContainers &&) = default;
  ActiveContainers &operator=(ActiveContainers &&) = default;

  void add_container(VersionedModelId model, int connection_id, int replica_id,
                     InputType input_type);

  void remove_container(VersionedModelId model, int replica_id);

  void register_batch_size(VersionedModelId model, int batch_size);

  void unregister_batch_size(VersionedModelId model);

  /// This method returns the active container specified by the
  /// provided model id and replica id. This is threadsafe because each
  /// individual ModelContainer object is threadsafe, and this method returns
  /// a shared_ptr to a ModelContainer object.
  std::shared_ptr<ModelContainer> get_model_replica(
      const VersionedModelId &model, const int replica_id);

  std::map<int, std::shared_ptr<ModelContainer>> get_replicas_for_model(
      const VersionedModelId &model);

  /// Get list of all models that have at least one active replica.
  std::vector<VersionedModelId> get_known_models();
  std::unordered_map<VersionedModelId, int> batch_sizes_;

 private:
  void log_active_containers();

  // Protects the models-replicas map. Must acquire an exclusive
  // lock to modify the map and a shared_lock when accessing
  // replicas. The replica ModelContainer entries are independently threadsafe.
  boost::shared_mutex m_;

  // A mapping of models to their replicas. The replicas
  // for each model are represented as a map keyed on replica id.
  std::unordered_map<VersionedModelId,
                     std::map<int, std::shared_ptr<ModelContainer>>>
      containers_;
};
}  // namespace clipper

#endif