//
// Copyright (c) 2021 INRIA
//

#ifndef HPP_FCL_SERIALIZATION_HFIELD_H
#define HPP_FCL_SERIALIZATION_HFIELD_H

#include "hpp/fcl/hfield.h"

#include "hpp/fcl/serialization/fwd.h"
#include "hpp/fcl/serialization/OBBRSS.h"

namespace boost
{
  namespace serialization
  {
  
    template <class Archive>
    void serialize(Archive & ar,
                   hpp::fcl::HFNodeBase & node,
                   const unsigned int /*version*/)
    {
      ar & make_nvp("first_child",node.first_child);
      ar & make_nvp("x_id",node.x_id);
      ar & make_nvp("x_size",node.x_size);
      ar & make_nvp("y_id",node.y_id);
      ar & make_nvp("y_size",node.y_size);
    }
  
    template <class Archive, typename BV>
    void serialize(Archive & ar,
                   hpp::fcl::HFNode<BV> & node,
                   const unsigned int /*version*/)
    {
      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::HFNodeBase>(node));
      ar & make_nvp("bv",node.bv);
    }
  
    namespace internal
    {
      template<typename BV>
      struct HeightFieldAccessor : hpp::fcl::HeightField<BV>
      {
        typedef hpp::fcl::HeightField<BV> Base;
        using Base::bvs;
        using Base::num_bvs;
        using Base::x_dim;
        using Base::y_dim;
        using Base::x_grid;
        using Base::y_grid;
        using Base::heights;
        using Base::min_height;
        using Base::max_height;
      };
    }
  
    template <class Archive, typename BV>
    void serialize(Archive & ar,
                   hpp::fcl::HeightField<BV> & hf_model,
                   const unsigned int /*version*/)
    {
      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::CollisionGeometry>(hf_model));
      
      
      typedef internal::HeightFieldAccessor<BV> Accessor;
      Accessor & access = reinterpret_cast<Accessor &>(hf_model);
      
      ar & make_nvp("x_dim",access.x_dim);
      ar & make_nvp("y_dim",access.y_dim);
      ar & make_nvp("heights",access.heights);
      ar & make_nvp("min_height",access.min_height);
      ar & make_nvp("max_height",access.max_height);
      ar & make_nvp("x_grid",access.x_grid);
      ar & make_nvp("y_grid",access.y_grid);

      ar & make_nvp("bvs",access.bvs);
      ar & make_nvp("num_bvs",access.num_bvs);
    }
  }
}

#endif // ifndef HPP_FCL_SERIALIZATION_HFIELD_H
