/*

 * Software License Agreement (BSD License)

 *

 *  Copyright (c) 2021, INRIA

 *  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 Open Source Robotics Foundation nor the names of its

 *     contributors may be used to endorse or promote products derived

 *     from this software without specific prior written permission.

 *

 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 *  POSSIBILITY OF SUCH DAMAGE.

 */

/** \author Justin Carpentier */

#ifndef HPP_FCL_HEIGHT_FIELD_H

#define HPP_FCL_HEIGHT_FIELD_H

#include <hpp/fcl/fwd.hh>

#include <hpp/fcl/collision_object.h>

#include <hpp/fcl/BV/BV_node.h>

#include <hpp/fcl/BVH/BVH_internal.h>

#include <vector>

namespace hpp {

namespace fcl {

/// @addtogroup Construction_Of_HeightField

/// @{

struct HPP_FCL_DLLAPI HFNodeBase {

  /// @brief An index for first child node or primitive

  /// If the value is positive, it is the index of the first child bv node

  /// If the value is negative, it is -(primitive index + 1)

  /// Zero is not used.

  size_t first_child;

  Eigen::DenseIndex x_id, x_size;

  Eigen::DenseIndex y_id, y_size;

  FCL_REAL max_height;

  /// @brief Default constructor

        x_id(-1),

        x_size(0),

        y_id(-1),

        y_size(0),

  /// @brief Comparison operator

  }

  /// @brief Difference operator

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

  /// @brief Whether current node is a leaf node (i.e. contains a primitive

  /// index)

  /// @brief Return the index of the first child. The index is referred to the

  /// bounding volume array (i.e. bvs) in BVHModel

  /// @brief Return the index of the second child. The index is referred to the

  /// bounding volume array (i.e. bvs) in BVHModel

  inline Eigen::Vector2i leftChildIndexes() const {

    return Eigen::Vector2i(x_id, y_id);

  }

  inline Eigen::Vector2i rightChildIndexes() const {

    return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2);

  }

};

template <typename BV>

struct HPP_FCL_DLLAPI HFNode : public HFNodeBase {

  typedef HFNodeBase Base;

  /// @brief bounding volume storing the geometry

  BV bv;

  /// @brief Equality operator

  }

  /// @brief Difference operator

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

  /// @brief Check whether two BVNode collide

  bool overlap(const HFNode& other) const { return bv.overlap(other.bv); }

  /// @brief Check whether two BVNode collide

  bool overlap(const HFNode& other, const CollisionRequest& request,

               FCL_REAL& sqrDistLowerBound) const {

    return bv.overlap(other.bv, request, sqrDistLowerBound);

  }

  /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and

  /// the underlying BV supports distance, return the nearest points.

  FCL_REAL distance(const HFNode& other, Vec3f* P1 = NULL,

                    Vec3f* P2 = NULL) const {

    return bv.distance(other.bv, P1, P2);

  }

  /// @brief Access to the center of the BV

  Vec3f getCenter() const { return bv.center(); }

  /// @brief Access to the orientation of the BV

  const Matrix3f& getOrientation() const {

    static const Matrix3f id3 = Matrix3f::Identity();

    return id3;

  }

  /// \cond

  /// \endcond

};

namespace details {

template <typename BV>

struct UpdateBoundingVolume {

    //      AABB bv_aabb;

    //      bv_aabb.update(pointA,pointB);

};

template <>

struct UpdateBoundingVolume<AABB> {

    //      bv.update(pointA,pointB);

};

}  // namespace details

/// @brief Data structure depicting a height field given by the base grid

/// dimensions and the elevation along the grid. \tparam BV one of the bounding

/// volume class in \ref Bounding_Volume.

///

/// An height field is defined by its base dimensions along the X and Y axes and

/// a set ofpoints defined by their altitude, regularly dispatched on the grid.

/// The height field is centered at the origin and the corners of the geometry

/// correspond to the following coordinates [± x_dim/2; ± y_dim/2].

template <typename BV>

class HPP_FCL_DLLAPI HeightField : public CollisionGeometry {

 public:

  typedef CollisionGeometry Base;

  typedef HFNode<BV> Node;

  typedef std::vector<Node> BVS;

  /// @brief Constructing an empty HeightField

      : CollisionGeometry(),

  /// @brief Constructing an HeightField from its base dimensions and the set of

  /// heights points.

  ///        The granularity of the height field along X and Y direction is

  ///        extraded from the Z grid.

  ///

  /// \param[in] x_dim Dimension along the X axis

  /// \param[in] y_dim Dimension along the Y axis

  /// \param[in] heights Matrix containing the altitude of each point compositng

  /// the height field

  /// \param[in] min_height Minimal height of the height field

  ///

              const MatrixXf& heights, const FCL_REAL min_height = (FCL_REAL)0)

  /// @brief Copy contructor from another HeightField

  ///

  /// \param[in] other to copy.

  ///

      : CollisionGeometry(other),

  /// @brief Returns a const reference of the grid along the X direction.

  /// @brief Returns a const reference of the grid along the Y direction.

  /// @brief Returns a const reference of the heights

  /// @brief Returns the dimension of the Height Field along the X direction.

  /// @brief Returns the dimension of the Height Field along the Y direction.

  /// @brief Returns the minimal height value of the Height Field.

  /// @brief Returns the maximal height value of the Height Field.

  /// @brief deconstruction, delete mesh data related.

  /// @brief Compute the AABB for the HeightField, used for broad-phase

  /// collision

  /// @brief Update Height Field height

          "The matrix containing the new heights values does not have the same "

          "matrix size as the original one.\n"

          "\tinput values - rows: "

              << new_heights.rows() << " - cols: " << new_heights.cols() << "\n"

              << "\texpected values - rows: " << heights.rows()

              << " - cols: " << heights.cols() << "\n",

          std::invalid_argument);

 protected:

            const FCL_REAL min_height) {

    // Allocate BVS

    // Build tree

  /// @brief Get the object type: it is a HFIELD

 protected:

  /// @brief Dimensions in meters along X and Y directions

  FCL_REAL x_dim, y_dim;

  /// @brief Elevation values in meters of the Height Field

  MatrixXf heights;

  /// @brief Minimal height of the Height Field: all values bellow min_height

  /// will be discarded.

  FCL_REAL min_height, max_height;

  /// @brief Grids along the X and Y directions. Useful for plotting or other

  /// related things.

  VecXf x_grid, y_grid;

  /// @brief Bounding volume hierarchy

  BVS bvs;

  unsigned int num_bvs;

  /// @brief Build the bounding volume hierarchy

           "the maximal height is not correct");

    HPP_FCL_UNUSED_VARIABLE(max_recursive_height);

  }

    FCL_REAL max_height;

    } else {

      FCL_REAL

    }

  }

                              const Eigen::DenseIndex x_size,

                              const Eigen::DenseIndex y_id,

                              const Eigen::DenseIndex y_size) {

           "x_size or y_size are not of correct value");

    FCL_REAL max_height;

        y_size == 1)  // don't build any BV for the current child node

    {

    } else {

      {

                                             x_size_half, y_id, y_size);

            recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half,

                               x_size - x_size_half, y_id, y_size);

      } else  // splitting along the Y axis

      {

                                             y_id, y_size_half);

            recursiveBuildTree(bv_node.rightChild(), x_id, x_size,

                               y_id + y_size_half, y_size - y_size_half);

      }

    }

    //    max_height = std::max(max_height,min_height);

                       max_height);

  }

 public:

  /// @brief Access the bv giving the its index

  }

  /// @brief Access the bv giving the its index

  }

  /// @brief Get the BV type: default is unknown

  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }

 private:

  }

 public:

};

/// @brief Specialization of getNodeType() for HeightField with different BV

/// types

template <>

NODE_TYPE HeightField<AABB>::getNodeType() const;

template <>

NODE_TYPE HeightField<OBB>::getNodeType() const;

template <>

NODE_TYPE HeightField<RSS>::getNodeType() const;

template <>

NODE_TYPE HeightField<kIOS>::getNodeType() const;

template <>

NODE_TYPE HeightField<OBBRSS>::getNodeType() const;

template <>

NODE_TYPE HeightField<KDOP<16> >::getNodeType() const;

template <>

NODE_TYPE HeightField<KDOP<18> >::getNodeType() const;

template <>

NODE_TYPE HeightField<KDOP<24> >::getNodeType() const;

/// @}

}  // namespace fcl

}  // namespace hpp

#endif