//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSbd:E_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_TRIG_REDUCTION_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_TRIG_REDUCTION_HPP_INCLUDED

#include <boost/simd/arch/common/detail/generic/trig_reduction.hpp>
#include <boost/simd/function/rem_pio2_medium.hpp>
#include <boost/simd/function/rem_pio2_cephes.hpp>
#include <boost/simd/function/rem_pio2.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/split.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/simd/detail/dispatch/meta/upgrade.hpp>

#include <boost/simd/function/all.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/if_else_nan.hpp>
#include <boost/simd/function/is_not_greater.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/all.hpp>
#include <boost/simd/function/inrad.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/detail/constant/medium_pi.hpp>
#include <boost/simd/constant/false.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/real.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <utility>

namespace boost { namespace simd
{
  namespace detail
  {
    template<class A0, class mode>
    struct trig_reduction < A0, tag::radian_tag, tag::simd_type, mode>
    {
      using i_t = bd::as_integer_t<A0, signed>;
      using l_t = bs::as_logical_t<A0>;
      using conversion_allowed_t = bd::is_upgradable<A0>;

      static BOOST_FORCEINLINE auto is_0_pio4_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_not_greater(a0, Pio_4<A0>()))
      {
        return is_not_greater(a0, Pio_4<A0>());
      }
      static BOOST_FORCEINLINE auto is_0_pio2_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_not_greater(a0, Pio_2<A0>()))
      {
        return is_not_greater(a0, Pio_2<A0>());
      }
      static BOOST_FORCEINLINE auto is_0_20pi_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_not_greater(a0, Real<A0, 0X404F6A7A2955385EULL, 0X427B53D1UL>()))
      {
        return is_not_greater(a0, Real<A0, 0X404F6A7A2955385EULL, 0X427B53D1UL>()); //20 pi;
      }
      static BOOST_FORCEINLINE auto is_0_mpi_reduced (const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_not_greater(a0, Medium_pi<A0>()))
      {
        return is_not_greater(a0, Medium_pi<A0>()); //2^6 pi
      }
      static BOOST_FORCEINLINE auto is_0_dmpi_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_not_greater(a0,Constant<A0,262144>()))
      {
        return is_not_greater(a0, Ratio<A0,262144>()); //2^18 pi
      }

      static BOOST_FORCEINLINE l_t cot_invalid(const A0& )  BOOST_NOEXCEPT
      {
        return False<l_t>();
      }
      static BOOST_FORCEINLINE l_t tan_invalid(const A0& )  BOOST_NOEXCEPT
      {
        return False<l_t>();
      }

      static BOOST_FORCEINLINE i_t reduce(const A0& x, A0& xr)  BOOST_NOEXCEPT
      {
        return inner_reduce(x, xr);
      }

      static BOOST_FORCEINLINE i_t inner_reduce(const A0& x, A0& xr) BOOST_NOEXCEPT
      {
        A0 xx =  preliminary<mode>::clip(x);
        return select_mode(xx, xr, boost::mpl::int_<mode::start>());
      }

      template < class Mode, bool clipped = Mode::clipped>
      struct preliminary
      {
        static BOOST_FORCEINLINE A0 const& clip(const A0& x) BOOST_NOEXCEPT  { return x; }
      };

      template < class Mode>
      struct preliminary<Mode, true>
      {
        static BOOST_FORCEINLINE A0 clip(const A0& x) BOOST_NOEXCEPT
        {
          return clipto(x, boost::mpl::int_<Mode::range>());
        }
      private :
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_pio4> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_pio4_reduced(x), x);
        }
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_20pi> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_20pi_reduced(x), x);
        }
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_mpi> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_mpi_reduced(x), x);
        }
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_dmpi> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_dmpi_reduced(x), x);
        }
      };

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::true_ const&
                  , boost::mpl::int_<tag::r_0_pio4> const&
                  ) BOOST_NOEXCEPT
      {
        xr = xx;
        return Zero<i_t>();
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::false_ const&
                  , boost::mpl::int_<tag::r_0_pio4> const& r
                  ) BOOST_NOEXCEPT
      {
        if(all(is_0_pio4_reduced(xx)))
        {
          return select_range(xx,xr,boost::mpl::true_(), r);
        }

        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_pio2>());
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_<tag::r_0_pio4> const& r) BOOST_NOEXCEPT
      {
        return select_range(xx,xr,boost::mpl::bool_<mode::range == tag::r_0_pio4>(),r);
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_<tag::r_0_pio2> const&) BOOST_NOEXCEPT
      {
        if(all(is_0_pio2_reduced(xx)))
        {
          auto test = is_greater(xx, Pio_4<A0>());
          xr = xx-Pio2_1<A0>();
          xr -= Pio2_2<A0>();
          xr -= Pio2_3<A0>();
          xr = if_else(test, xr, xx);
          return -bitwise_cast<i_t>(genmask(test));
        }

        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_20pi>());
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::true_ const&
                  , boost::mpl::int_<tag::r_0_20pi> const&
                  ) BOOST_NOEXCEPT
      {
        i_t n;
        std::tie(n, xr) = rem_pio2_cephes(xx);
        return n;
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::false_ const&
                  , boost::mpl::int_<tag::r_0_20pi> const& r
                  ) BOOST_NOEXCEPT
      {
        if(all(is_0_20pi_reduced(xx)))
          return select_range(xx,xr,boost::mpl::true_(), r);

        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_mpi>());
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_< tag::r_0_20pi> const& r) BOOST_NOEXCEPT
      {
        return select_range(xx,xr,boost::mpl::bool_<mode::range == tag::r_0_20pi>(),r);
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::true_ const&
                  , boost::mpl::int_<tag::r_0_mpi> const&
                  ) BOOST_NOEXCEPT
      {
        i_t n;
        std::tie(n, xr) = rem_pio2_medium(xx);
        return n;
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::false_ const&
                  , boost::mpl::int_<tag::r_0_mpi> const& r
                  ) BOOST_NOEXCEPT
      {
        if(all(is_0_mpi_reduced(xx)))
          return select_range(xx,xr,boost::mpl::true_(), r);

        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_dmpi>());
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_< tag::r_0_mpi> const& r) BOOST_NOEXCEPT
      {
        return select_range(xx,xr,boost::mpl::bool_<mode::range == tag::r_0_mpi>(),r);
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_< tag::r_0_dmpi> const&) BOOST_NOEXCEPT
      {
        if(all(is_0_dmpi_reduced(xx)))
           return use_conversion(xx, xr, conversion_allowed_t());
        i_t n;
        std::tie(n, xr) = rem_pio2(xx);
        return n;
      }

      static BOOST_FORCEINLINE i_t
      use_conversion(const A0 & xx,  A0& xr
                    , std::false_type) BOOST_NOEXCEPT
      {
        i_t n;
        std::tie(n, xr) = rem_pio2(xx);
        return n;
      }

      static BOOST_FORCEINLINE i_t
      use_conversion(const A0 & x,  A0& xr
                    ,std::true_type) BOOST_NOEXCEPT
      {
        // all of x are in [0, 2^18*pi],  conversion to double is used to reduce
        using uA0 = bd::upgrade_t<A0>;
        using aux_reduc_t = trig_reduction< uA0, tag::radian_tag,  tag::simd_type, mode, double>;

        auto uxs = split(x);
        uA0  uxr1, uxr2;

        auto n1 = aux_reduc_t::reduce(uxs[0], uxr1);
        auto n2 = aux_reduc_t::reduce(uxs[1], uxr2);

          xr = group(uxr1, uxr2);
        return group(n1, n2);
      }
    };

  }
} }

#endif


//----------------------------------------------------------------------------//

/*
 * Copyright (c) 2009 Sony Pictures Imageworks Inc
 *
 * 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 Sony Pictures Imageworks 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.
 */

//----------------------------------------------------------------------------//

/*! \file MIPUtil.cpp
  Contains implementations of resampling-related functions.
*/

//----------------------------------------------------------------------------//

// Header include
#include "MIPUtil.h"

// System includes
#include <cmath>

// Library includes
#include <boost/foreach.hpp>

// Project includes
#include "CoordSys.h"

//----------------------------------------------------------------------------//

FIELD3D_NAMESPACE_OPEN

//----------------------------------------------------------------------------//

namespace detail {

  //--------------------------------------------------------------------------//

  const std::string k_mipOffsetStr = "mipoffset";

  //--------------------------------------------------------------------------//

  V3i mipResolution(const V3i &baseRes, const size_t level, const V3i &add)
  {
    const float factor = 1.0 / (1 << level);
    const V3f   floatRes(baseRes);
    return V3i(static_cast<int>(std::ceil(floatRes.x * factor)) + add.x,
               static_cast<int>(std::ceil(floatRes.y * factor)) + add.y,
               static_cast<int>(std::ceil(floatRes.z * factor)) + add.z);
  }

  //--------------------------------------------------------------------------//

  //! \todo Update to use MIPField's mipOffset() instead of metadata
  FieldMapping::Ptr adjustedMIPFieldMapping(const FieldRes *base,
                                            const V3i &/*baseRes*/,
                                            const Box3i &extents,
                                            const size_t level)
  {
    typedef MatrixFieldMapping::MatrixCurve MatrixCurve;

    FieldMapping::Ptr mapping = base->mapping();

    const V3i   zero   = V3i(0);
    const V3i   mipOff = base->metadata().vecIntMetadata(k_mipOffsetStr, zero);
    const float mult   = 1 << level;
    const V3i   res    = extents.size() + V3i(1);

    // Compute offset of current level
    const V3i offset((mipOff.x >> level) << level,
                     (mipOff.y >> level) << level,
                     (mipOff.z >> level) << level);

    // Difference between current offset and base offset is num voxels
    // to offset current level by
    const V3d diff = offset - mipOff;

    if (MatrixFieldMapping::Ptr mfm =
        field_dynamic_cast<MatrixFieldMapping>(mapping)) {
      // Local space positions
      const V3d lsOrigin(0.0), lsX(1.0, 0.0, 0.0), lsY(0.0, 1.0, 0.0),
        lsZ(0.0, 0.0, 0.1);
      // Find base voxel size
      const V3f wsBaseVoxelSize = mfm->wsVoxelSize(0, 0, 0);
      // Compute current levels' voxel size
      const V3f wsVoxelSize = wsBaseVoxelSize * mult;
      // Grab the matrices
      const MatrixCurve::SampleVec lsToWsSamples = mfm->localToWorldSamples();
      // New mapping to construct
      MatrixFieldMapping::Ptr newMapping(new MatrixFieldMapping);
      // For each time sample
      BOOST_FOREACH (const MatrixCurve::Sample &sample, lsToWsSamples){
        // Find origin and orientation vectors
        V3d wsOrigin, wsX, wsY, wsZ;
        mfm->localToWorld(lsOrigin, wsOrigin, sample.first);
        mfm->localToWorld(lsX, wsX, sample.first);
        mfm->localToWorld(lsY, wsY, sample.first);
        mfm->localToWorld(lsZ, wsZ, sample.first);
        // Normalize orientation vectors
        wsX = (wsX - wsOrigin).normalized();
        wsY = (wsY - wsOrigin).normalized();
        wsZ = (wsZ - wsOrigin).normalized();
        // Origin shift due to mip offset
        wsOrigin += wsX * wsBaseVoxelSize.x * diff.x;
        wsOrigin += wsY * wsBaseVoxelSize.y * diff.y;
        wsOrigin += wsZ * wsBaseVoxelSize.z * diff.z;
        // Mult by voxel size
        wsX *= wsVoxelSize.x * res.x;
        wsY *= wsVoxelSize.y * res.y;
        wsZ *= wsVoxelSize.z * res.z;
        // Construct new mapping
        M44d mtx = coordinateSystem(wsX, wsY, wsZ, wsOrigin);
        // Update mapping
        newMapping->setLocalToWorld(sample.first, mtx);
      }
      // Done
      return newMapping;
    } else {
      // For non-uniform grids, there is nothing we can do.
      return mapping;
    }
  }

  //--------------------------------------------------------------------------//

} // namespace detail

//----------------------------------------------------------------------------//

V3i computeOffset(const FieldRes &f)
{
  V3d wsOrigin(0.0), vsOrigin;

  f.mapping()->worldToVoxel(wsOrigin, vsOrigin);

  V3i offset(-static_cast<int>(std::floor(vsOrigin.x + 0.5)),
             -static_cast<int>(std::floor(vsOrigin.y + 0.5)),
             -static_cast<int>(std::floor(vsOrigin.z + 0.5)));

  return offset;
}

//----------------------------------------------------------------------------//

FIELD3D_NAMESPACE_SOURCE_CLOSE

//----------------------------------------------------------------------------//


//
// Copyright (c) 2015-2021 CNRS INRIA
//

#include <iostream>

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"

#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"

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

using namespace pinocchio;

typedef std::map <std::string, pinocchio::SE3> PositionsMap_t;
typedef std::map <std::string, pinocchio::SE3> JointPositionsMap_t;
typedef std::map <std::string, pinocchio::SE3> GeometryPositionsMap_t;
typedef std::map <std::pair < std::string , std::string >, fcl::DistanceResult > PairDistanceMap_t;
JointPositionsMap_t fillPinocchioJointPositions(const pinocchio::Model& model, const pinocchio::Data & data);
GeometryPositionsMap_t fillPinocchioGeometryPositions(const pinocchio::GeometryModel & geomModel,
                                                      const pinocchio::GeometryData & geomData);

std::vector<std::string> getBodiesList();

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( simple_boxes )
{
  using namespace pinocchio;
  Model model;
  GeometryModel geomModel;

  Model::JointIndex idx;
  idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar1_joint");
  model.addJointFrame(idx);
  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
  model.addBodyFrame("planar1_body", idx, SE3::Identity());

  idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar2_joint");
  model.addJointFrame(idx);
  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
  model.addBodyFrame("planar2_body", idx, SE3::Identity());

  boost::shared_ptr<fcl::Box> sample(new fcl::Box(1, 1, 1));
  Model::FrameIndex body_id_1 = model.getBodyId("planar1_body");
  Model::JointIndex joint_parent_1 = model.frames[body_id_1].parent;
  Model::JointIndex idx_geom1 = geomModel.addGeometryObject(GeometryObject("ff1_collision_object",
                                                                           model.getBodyId("planar1_body"),joint_parent_1,
                                                                           sample,SE3::Identity(), "", Eigen::Vector3d::Ones())
                                                            );
  geomModel.geometryObjects[idx_geom1].parentJoint = model.frames[body_id_1].parent;

  boost::shared_ptr<fcl::Box> sample2(new fcl::Box(1, 1, 1));
  Model::FrameIndex body_id_2 = model.getBodyId("planar2_body");
  Model::JointIndex joint_parent_2 = model.frames[body_id_2].parent;
  Model::JointIndex idx_geom2 = geomModel.addGeometryObject(GeometryObject("ff2_collision_object",
                                                                           model.getBodyId("planar2_body"),joint_parent_2,
                                                                           sample2,SE3::Identity(), "", Eigen::Vector3d::Ones()),
                                                            model);
  BOOST_CHECK(geomModel.geometryObjects[idx_geom2].parentJoint == model.frames[body_id_2].parent);

  boost::shared_ptr<fcl::Box> universe_body_geometry(new fcl::Box(1, 1, 1));
  model.addBodyFrame("universe_body", 0, SE3::Identity());
  Model::FrameIndex body_id_3 = model.getBodyId("universe_body");
  Model::JointIndex joint_parent_3 = model.frames[body_id_3].parent;
  SE3 universe_body_placement = SE3::Random();
  Model::JointIndex idx_geom3 = geomModel.addGeometryObject(GeometryObject("universe_collision_object",
                                                                           model.getBodyId("universe_body"),joint_parent_3,
                                                                           universe_body_geometry,universe_body_placement, "", Eigen::Vector3d::Ones()),
                                                            model);

  BOOST_CHECK(geomModel.geometryObjects[idx_geom3].parentJoint == model.frames[body_id_3].parent);

  geomModel.addAllCollisionPairs();
  pinocchio::Data data(model);
  pinocchio::GeometryData geomData(geomModel);

  BOOST_CHECK(CollisionPair(0,1) == geomModel.collisionPairs[0]);

  std::cout << "------ Model ------ " << std::endl;
  std::cout << model;
  std::cout << "------ Geom ------ " << std::endl;
  std::cout << geomModel;
  std::cout << "------ DataGeom ------ " << std::endl;
  std::cout << geomData;

  Eigen::VectorXd q(model.nq);
  q <<  0, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  BOOST_CHECK(geomData.oMg[idx_geom3].isApprox(universe_body_placement));
  BOOST_CHECK(computeCollision(geomModel,geomData,0) == true);

  q <<  2, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);

  q <<  0.99, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  BOOST_CHECK(computeCollision(geomModel,geomData,0) == true);

  q <<  1.01, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);
}

BOOST_AUTO_TEST_CASE ( loading_model )
{
  typedef pinocchio::Model Model;
  typedef pinocchio::GeometryModel GeometryModel;
  typedef pinocchio::Data Data;
  typedef pinocchio::GeometryData GeometryData;

  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
  std::vector < std::string > packageDirs;
  std::string meshDir  = PINOCCHIO_MODEL_DIR;
  packageDirs.push_back(meshDir);

  Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
  GeometryModel geomModel;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );
  geomModel.addAllCollisionPairs();

  GeometryModel geomModelOther = pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );
  BOOST_CHECK(geomModelOther == geomModel);

  Data data(model);
  GeometryData geomData(geomModel);
  fcl::CollisionResult result;

  Eigen::VectorXd q(model.nq);
  q << 0, 0, 0.840252, 0, 0, 0, 1, 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, 0, -0.3490658,
       0.6981317, -0.3490658, 0, 0, 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, 0, 0, 0, 0,
       1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  pinocchio::Index idx = geomModel.findCollisionPair(CollisionPair(1,10));
  BOOST_CHECK(computeCollision(geomModel,geomData,idx) == false);

  fcl::DistanceResult distance_res = computeDistance(geomModel,geomData,idx);
  BOOST_CHECK(distance_res.min_distance > 0.);
}

BOOST_AUTO_TEST_CASE(manage_collision_pairs)
{
  typedef pinocchio::Model Model;
  typedef pinocchio::GeometryModel GeometryModel;
  typedef pinocchio::GeometryData GeometryData;

  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
  std::vector < std::string > package_dirs;
  std::string mesh_dir  = PINOCCHIO_MODEL_DIR;
  package_dirs.push_back(mesh_dir);

  Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
  GeometryModel geom_model;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, package_dirs);
  geom_model.addAllCollisionPairs();

  GeometryModel::MatrixXb collision_map(GeometryModel::MatrixXb::Zero((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));

  for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
  {
    const CollisionPair & cp = geom_model.collisionPairs[k];
    collision_map((Eigen::DenseIndex)cp.first,(Eigen::DenseIndex)cp.second) = true;
  }
  GeometryModel::MatrixXb collision_map_lower = collision_map.transpose();

  GeometryModel geom_model_copy, geom_model_copy_lower;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy, package_dirs);
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy_lower, package_dirs);
  geom_model_copy.setCollisionPairs(collision_map);
  geom_model_copy_lower.setCollisionPairs(collision_map_lower,false);

  BOOST_CHECK(geom_model_copy.collisionPairs.size() == geom_model.collisionPairs.size());
  BOOST_CHECK(geom_model_copy_lower.collisionPairs.size() == geom_model.collisionPairs.size());
  for(size_t k = 0; k < geom_model_copy.collisionPairs.size(); ++k)
  {
    BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy.collisionPairs[k]));
    BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy_lower.collisionPairs[k]));
  }
  for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
  {
    BOOST_CHECK(geom_model_copy.existCollisionPair(geom_model.collisionPairs[k]));
    BOOST_CHECK(geom_model_copy_lower.existCollisionPair(geom_model.collisionPairs[k]));
  }

  {
    GeometryData geom_data(geom_model);
    geom_data.activateAllCollisionPairs();

    for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
      BOOST_CHECK(geom_data.activeCollisionPairs[k]);
  }

  {
    GeometryData geom_data(geom_model);
    geom_data.deactivateAllCollisionPairs();

    for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
      BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
  }

  {
    GeometryData geom_data(geom_model), geom_data_copy(geom_model), geom_data_copy_lower(geom_model);
    geom_data_copy.deactivateAllCollisionPairs();
    geom_data_copy_lower.deactivateAllCollisionPairs();

    GeometryData::MatrixXb collision_map(GeometryModel::MatrixXb::Zero((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
    for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
    {
      const CollisionPair & cp = geom_model.collisionPairs[k];
      collision_map((Eigen::DenseIndex)cp.first,(Eigen::DenseIndex)cp.second) = geom_data.activeCollisionPairs[k];
    }
    GeometryData::MatrixXb collision_map_lower = collision_map.transpose();

    geom_data_copy.setActiveCollisionPairs(geom_model, collision_map);
    BOOST_CHECK(geom_data_copy.activeCollisionPairs == geom_data.activeCollisionPairs);

    geom_data_copy_lower.setActiveCollisionPairs(geom_model, collision_map_lower, false);
    BOOST_CHECK(geom_data_copy_lower.activeCollisionPairs == geom_data.activeCollisionPairs);
  }

  // Test security margins
  {
    GeometryData geom_data_upper(geom_model), geom_data_lower(geom_model);

    const GeometryData::MatrixXs security_margin_map(GeometryData::MatrixXs::Ones((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
    GeometryData::MatrixXs security_margin_map_upper(security_margin_map);
    security_margin_map_upper.triangularView<Eigen::Lower>().fill(0.);

    geom_data_upper.setSecurityMargins(geom_model, security_margin_map);
    for(size_t k = 0; k < geom_data_upper.collisionRequests.size(); ++k)
    {
      BOOST_CHECK(geom_data_upper.collisionRequests[k].security_margin == 1.);
    }

    geom_data_lower.setSecurityMargins(geom_model, security_margin_map, false);
    for(size_t k = 0; k < geom_data_lower.collisionRequests.size(); ++k)
    {
      BOOST_CHECK(geom_data_lower.collisionRequests[k].security_margin == 1.);
    }
  }

  // Test enableGeometryCollision
  {
    GeometryData geom_data(geom_model);
    geom_data.deactivateAllCollisionPairs();
    geom_data.setGeometryCollisionStatus(geom_model,0,true);

    for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
    {
      const CollisionPair & cp = geom_model.collisionPairs[k];
      if(cp.first == 0 || cp.second == 0)
      {
        BOOST_CHECK(geom_data.activeCollisionPairs[k]);
      }
      else
      {
        BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
      }
    }

  }

  // Test disableGeometryCollision
  {
    GeometryData geom_data(geom_model);
    geom_data.activateAllCollisionPairs();
    geom_data.setGeometryCollisionStatus(geom_model,0,false);

    for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
    {
      const CollisionPair & cp = geom_model.collisionPairs[k];
      if(cp.first == 0 || cp.second == 0)
      {
        BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
      }
      else
      {
        BOOST_CHECK(geom_data.activeCollisionPairs[k]);
      }
    }

  }
}

BOOST_AUTO_TEST_CASE ( test_collisions )
{
  typedef pinocchio::Model Model;
  typedef pinocchio::GeometryModel GeometryModel;
  typedef pinocchio::Data Data;
  typedef pinocchio::GeometryData GeometryData;

  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
  std::vector < std::string > packageDirs;
  const std::string meshDir  = PINOCCHIO_MODEL_DIR;
  packageDirs.push_back(meshDir);
  const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");

  Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
  GeometryModel geom_model;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs);
  geom_model.addAllCollisionPairs();
  pinocchio::srdf::removeCollisionPairs(model,geom_model,srdf_filename,false);

  Data data(model);
  GeometryData geom_data(geom_model);

  pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename,false);
  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];

  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);

  BOOST_CHECK(computeCollisions(geom_model,geom_data) == false);
  BOOST_CHECK(computeCollisions(geom_model,geom_data,false) == false);

  for(size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
  {
    const CollisionPair & cp = geom_model.collisionPairs[cp_index];
    const GeometryObject & obj1 = geom_model.geometryObjects[cp.first];
    const GeometryObject & obj2 = geom_model.geometryObjects[cp.second];

    hpp::fcl::CollisionResult other_res;
    computeCollision(geom_model,geom_data,cp_index);

    fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[cp.first ])),
                     oM2 (toFclTransform3f(geom_data.oMg[cp.second]));

    fcl::collide(obj1.geometry.get(), oM1,
                 obj2.geometry.get(), oM2,
                 geom_data.collisionRequests[cp_index],
                 other_res);

    const hpp::fcl::CollisionResult & res = geom_data.collisionResults[cp_index];

    BOOST_CHECK(res.isCollision() == other_res.isCollision());
    BOOST_CHECK(!res.isCollision());
  }

  // test other signatures
  {
    Data data(model);
    GeometryData geom_data(geom_model);
    BOOST_CHECK(computeCollisions(model,data,geom_model,geom_data,q) == false);
  }
}

BOOST_AUTO_TEST_CASE ( test_distances )
{
  typedef pinocchio::Model Model;
  typedef pinocchio::GeometryModel GeometryModel;
  typedef pinocchio::Data Data;
  typedef pinocchio::GeometryData GeometryData;

  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
  std::vector < std::string > packageDirs;
  const std::string meshDir  = PINOCCHIO_MODEL_DIR;
  packageDirs.push_back(meshDir);
  const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");

  Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
  GeometryModel geom_model;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs);
  geom_model.addAllCollisionPairs();
  pinocchio::srdf::removeCollisionPairs(model,geom_model,srdf_filename,false);

  Data data(model);
  GeometryData geom_data(geom_model);

  pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename,false);
  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];

  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);

  BOOST_CHECK(computeDistances(geom_model,geom_data) < geom_model.collisionPairs.size());

  {
    Data data(model);
    GeometryData geom_data(geom_model);
    BOOST_CHECK(computeDistances(model,data,geom_model,geom_data,q) < geom_model.collisionPairs.size());
  }
}

BOOST_AUTO_TEST_CASE ( test_append_geom_models )
{
  typedef pinocchio::Model Model;
  typedef pinocchio::GeometryModel GeometryModel;

  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
  std::vector < std::string > packageDirs;
  const std::string meshDir  = PINOCCHIO_MODEL_DIR;
  packageDirs.push_back(meshDir);

  Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
  GeometryModel geom_model1;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model1, packageDirs);

  GeometryModel geom_model2;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model2, packageDirs);

  appendGeometryModel(geom_model2,geom_model1);
  BOOST_CHECK(geom_model2.ngeoms == 2*geom_model1.ngeoms);

  {
    GeometryModel geom_model_empty;
    GeometryModel geom_model;
    BOOST_CHECK(geom_model_empty.ngeoms == 0);
    appendGeometryModel(geom_model,geom_model_empty);
    BOOST_CHECK(geom_model.ngeoms== 0);
  }
}

#if defined(PINOCCHIO_WITH_URDFDOM) && defined(PINOCCHIO_WITH_HPP_FCL)
BOOST_AUTO_TEST_CASE (radius)
{
  std::vector < std::string > packageDirs;

  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
  std::string meshDir  = PINOCCHIO_MODEL_DIR;
  packageDirs.push_back(meshDir);

  pinocchio::Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
  pinocchio::GeometryModel geom;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom, packageDirs);
  Data data(model);
  GeometryData geomData(geom);

  // Test that the algorithm does not crash
  pinocchio::computeBodyRadius(model, geom, geomData);
  BOOST_FOREACH( double radius, geomData.radius) BOOST_CHECK(radius>=0.);
}
#endif // if defined(PINOCCHIO_WITH_URDFDOM) && defined(PINOCCHIO_WITH_HPP_FCL)

BOOST_AUTO_TEST_SUITE_END ()


#pragma once

#include <polyfem/Common.hpp>

#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/ElementBases.hpp>

#include <polyfem/AutodiffTypes.hpp>

#include <Eigen/Dense>
#include <functional>

//Navier-Stokes local assembler
namespace polyfem
{
	template <bool full_gradient>
	//full graidnet used for Picard iteration
	class NavierStokesVelocity
	{
	public:
		// res is R^{dim²}
		//pde
		Eigen::VectorXd
		assemble_grad(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const;

		Eigen::MatrixXd
		//gradient of pde, this returns full gradient or partil depending on the template
		assemble_hessian(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const;

		//navier stokes is not energy based
		double compute_energy(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const
		{
			//not used, this formulation is gradient based!
			assert(false);
			return 0;
		}

		//rhs for fabbricated solution
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		void set_size(const int size);
		inline int size() const { return size_; }

		//set viscosity
		void set_parameters(const json &params);

		//return velociry and norm, for compliancy with elasticity
		void compute_norm_velocity(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &velocity, Eigen::MatrixXd &norms) const;
		void compute_stress_tensor(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &velocity, Eigen::MatrixXd &tensor) const;

	private:
		int size_ = 2;
		double viscosity_ = 1;

		Eigen::MatrixXd compute_N(const ElementAssemblyValues &vals, const Eigen::MatrixXd &velocity, const QuadratureVector &da) const;
		Eigen::MatrixXd compute_W(const ElementAssemblyValues &vals, const Eigen::MatrixXd &velocity, const QuadratureVector &da) const;
	};
} // namespace polyfem


////////////////////////////////////////////////////////////////
// Orkid Media Engine
// Copyright 1996-2020, Michael T. Mayers.
// Distributed under the Boost Software License - Version 1.0 - August 17, 2003
// see http://www.boost.org/LICENSE_1_0.txt
////////////////////////////////////////////////////////////////

#include <ork/lev2/config.h>
#if defined(ENABLE_IGL)

#include <ork/kernel/orklut.hpp>
#include <ork/math/plane.h>
#include <ork/lev2/gfx/meshutil/submesh.h>
#include <ork/lev2/gfx/meshutil/igl.h>
#include <iostream>

#include <Eigen/Core>

// #define USE_CGAL

#if defined(USE_CGAL)
#include <igl/copyleft/cgal/remesh_self_intersections.h> // GNU GPL (todo move to external executable?)
#include <igl/copyleft/cgal/extract_cells.h>             // GNU GPL (todo move to external executable?)
#endif

#include <igl/copyleft/tetgen/tetrahedralize.h>
#include <igl/copyleft/tetgen/cdt.h>
#include <igl/remove_unreferenced.h>
#include <igl/unique_simplices.h>
#include <igl/barycenter.h>
#include <igl/winding_number.h>
#include <igl/extract_manifold_patches.h>
#include <igl/embree/reorient_facets_raycast.h>

namespace ork::meshutil {
//////////////////////////////////////////////////////////////////////////////
iglmesh_ptr_t IglMesh::cleaned() const {
#if defined(USE_CGAL)
  auto rval = std::make_shared<IglMesh>(_verts, _faces);
  using namespace Eigen;
  using namespace igl;
  using namespace igl::copyleft::tetgen;
  using namespace igl::copyleft::cgal;

  ////////////////////////////////////////////////////////////////////////
  auto V = _verts;
  auto F = _faces;
  MatrixXd CV; // clean output verts
  MatrixXi CF; // clean output faces
  VectorXi IM;
  MatrixXi _1;
  VectorXi _2;
  ////////////////////////////////////////////////////////////////////////
  // check manifold status
  ////////////////////////////////////////////////////////////////////////
  // OrkAssert(isVertexManifold() and isEdgeManifold());
  ////////////////////////////////////////////////////////////////////////
  // remesh_self_intersections
  ////////////////////////////////////////////////////////////////////////
  remesh_self_intersections(V, F, {false, false, false}, CV, CF, _1, _2, IM);
  std::for_each(CF.data(), CF.data() + CF.size(), [&IM](int& a) { a = IM(a); });
  // validate_IM(V,CV,IM);
  std::cout << "clean: remove_unreferenced" << std::endl;
  {
    MatrixXi oldCF = CF;
    unique_simplices(oldCF, CF);
  }
  MatrixXd oldCV = CV;
  MatrixXi oldCF = CF;
  VectorXi nIM;
  remove_unreferenced(oldCV, oldCF, CV, CF, nIM);
  std::for_each(IM.data(), IM.data() + IM.size(), [&nIM](int& a) { a = a >= 0 ? nIM(a) : a; });
  ////////////////////////////////////////////////////////////////////////
  // tetrahedralize
  ////////////////////////////////////////////////////////////////////////
  MatrixXd TV;
  MatrixXi TT;
  {
    MatrixXi _1;
    // c  convex hull
    // Y  no boundary steiners
    // p  polygon input
    // T1e-16  sometimes helps tetgen
    // cout << "clean: tetrahedralize" << endl;
    // writeOBJ("CVCF.obj", CV, CF);
    CDTParam params;
    params.flags            = "CYT1e-16";
    params.use_bounding_box = true;
    if (cdt(CV, CF, params, TV, TT, _1) != 0) {
      OrkAssert(false);
    }
    // writeMESH("TVTT.mesh",TV,TT,MatrixXi());
  }
  ////////////////////////////////////////////////////////////////////////
  {
    MatrixXd BC;
    barycenter(TV, TT, BC);
    VectorXd W;
    // cout << "clean: winding_number" << endl;
    winding_number(V, F, BC, W);
    W                   = W.array().abs();
    const double thresh = 0.5;
    const int count     = (W.array() > thresh).cast<int>().sum();
    MatrixXi CT(count, TT.cols());
    int c = 0;
    for (int t = 0; t < TT.rows(); t++) {
      if (W(t) > thresh) {
        CT.row(c++) = TT.row(t);
      }
    }
    //////
    assert(c == count);
    boundary_facets(CT, CF);
    // writeMESH("CVCTCF.mesh",TV,CT,CF);
    // cout << "clean: remove_unreferenced" << endl;
    // Force all original vertices to be referenced
    MatrixXi FF = F;
    std::for_each(FF.data(), FF.data() + FF.size(), [&IM](int& a) { a = IM(a); });
    int ncf = CF.rows();
    MatrixXi ref(ncf + FF.rows(), 3);
    ref << CF, FF;
    VectorXi nIM;
    remove_unreferenced(TV, ref, CV, CF, nIM);
    // Only keep boundary faces
    CF.conservativeResize(ncf, 3);
    // cout << "clean: IM.minCoeff(): " << IM.minCoeff() << endl;
    // reindex nIM through IM
    std::for_each(IM.data(), IM.data() + IM.size(), [&nIM](int& a) { a = a >= 0 ? nIM(a) : a; });
  }
  ////////////////////////////////////////////////////////////////////////
  // output
  ////////////////////////////////////////////////////////////////////////
  rval->_verts = CV;
  rval->_faces = CF;
  return rval;
#else
  return nullptr;
#endif
}
//////////////////////////////////////////////////////////////////////////////
iglmesh_ptr_t IglMesh::reOriented() const {
  auto rval = std::make_shared<IglMesh>(_verts, _faces);
  std::vector<Eigen::VectorXi> C(2);
  std::vector<Eigen::MatrixXi> FF(2);
  // Compute patches
  for (int pass = 0; pass < 2; pass++) {
    Eigen::VectorXi I;
    igl::embree::reorient_facets_raycast(
        rval->_verts,
        rval->_faces, //
        rval->_faces.rows() * 100,
        10,
        pass == 1,
        false,
        false,
        I,
        C[pass]);
    // apply reorientation
    FF[pass].conservativeResize(
        rval->_faces.rows(), //
        rval->_faces.cols());
    for (int i = 0; i < I.rows(); i++) {
      if (I(i)) {
        FF[pass].row(i) = (rval->_faces.row(i).reverse()).eval();
      } else {
        FF[pass].row(i) = rval->_faces.row(i);
      }
    }
  }
  constexpr int selector = 0; // 0==patchwise, 1==facetwise
  rval->_faces           = FF[selector];
  return rval;
}
//////////////////////////////////////////////////////////////////////////////
manifold_extraction_ptr_t IglMesh::extractManifolds() const {
#if defined(USE_CGAL)
  auto ue   = uniqueEdges();
  auto rval = std::make_shared<ManifoldExtraction>();
  // Compute patches (F,EMAP,uE2E) --> (P)
  rval->_numpatches = igl::extract_manifold_patches(
      _faces, //
      ue->EMAP,
      ue->_ue2e,
      rval->P);
  // Compute cells (V,F,P,E,uE,EMAP) -> (per_patch_cells)
  rval->_numcells = igl::copyleft::cgal::extract_cells(
      _verts, //
      _faces,
      rval->P,
      ue->E,
      ue->uE,
      ue->_ue2e,
      ue->EMAP,
      rval->per_patch_cells);
  return rval;
#else
  return nullptr;
#endif
}
//////////////////////////////////////////////////////////////////////////////
} // namespace ork::meshutil
#endif


#pragma once

#include <cmath>
#include <fstream>

#include <boost/asio/serial_port.hpp>
#include <boost/foreach.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/none.hpp>

#include <mil_tools/msg_helpers.hpp>

#include <mil_msgs/RangeStamped.h>
#include <mil_msgs/VelocityMeasurements.h>

namespace rdi_explorer_dvl
{
static uint16_t getu16le(uint8_t *i)
{
  return *i | (*(i + 1) << 8);
}

static int32_t gets32le(uint8_t *i)
{
  return *i | (*(i + 1) << 8) | (*(i + 2) << 16) | (*(i + 3) << 24);
}

class Device
{
private:
  typedef std::vector<boost::uint8_t> ByteVec;

  const std::string port;
  const int baudrate;
  boost::asio::io_service io;
  boost::asio::serial_port p;

  /*
    Reads byte into a uint8_t
    res - uint8_t reference to write byte into
  */
  bool read_byte(uint8_t &res)
  {
    while (true)
    {
      try
      {
        p.read_some(boost::asio::buffer(&res, sizeof(res)));
        return true;
      }
      catch (const std::exception &exc)
      {
        ROS_ERROR_THROTTLE(0.5, "DVL: error on read: %s; reopening serial port", exc.what());
        open();
        return false;
      }
    }
  }

  /*
    Reads 2 bytes into a uint16_t
    res - uint16_t reference to write byte into
  */
  bool read_short(uint16_t &x)
  {
    uint8_t low;
    if (!read_byte(low))
      return false;
    uint8_t high;
    if (!read_byte(high))
      return false;
    x = 256 * high + low;
    return true;
  }

  /*
    Attempts to close and reopen the serial port
    Sleeps for one second before returning if an exception is caught
  */
  void open()
  {
    try
    {
      p.close();
      p.open(port);
      p.set_option(boost::asio::serial_port::baud_rate(baudrate));
      return;
    }
    catch (const std::exception &exc)
    {
      ROS_ERROR("DVL: error on open(port=%s): %s; reopening after delay", port.c_str(), exc.what());
      boost::this_thread::sleep(boost::posix_time::seconds(1));
    }
  }

public:
  Device(const std::string port, int baudrate) : port(port), baudrate(baudrate), p(io)
  {
    // open is called on first read() in the _polling_ thread
  }

  void read(boost::optional<mil_msgs::VelocityMeasurements> &res, boost::optional<mil_msgs::RangeStamped> &height_res)
  {
    res = boost::none;
    height_res = boost::none;

    if (!p.is_open())  // Open serial port if closed
    {
      open();
      return;
    }

    ByteVec ensemble;
    ensemble.resize(4);

    // Header ID
    if (!read_byte(ensemble[0]))
      return;
    if (ensemble[0] != 0x7F)
      return;

    ros::Time stamp = ros::Time::now();

    // Data Source ID
    if (!read_byte(ensemble[1]))
      return;
    if (ensemble[1] != 0x7F)
      return;

    // Size low
    if (!read_byte(ensemble[2]))
      return;
    // Size high
    if (!read_byte(ensemble[3]))
      return;

    uint16_t ensemble_size = getu16le(ensemble.data() + 2);
    ensemble.resize(ensemble_size);
    for (int i = 4; i < ensemble_size; i++)
    {
      if (!read_byte(ensemble[i]))
        return;
    }

    uint16_t checksum = 0;
    BOOST_FOREACH (uint16_t b, ensemble)
      checksum += b;
    uint16_t received_checksum;
    if (!read_short(received_checksum))
      return;
    if (received_checksum != checksum)
    {
      ROS_ERROR_THROTTLE(0.5, "DVL: invalid ensemble checksum. received: %i calculated: %i size: %i", received_checksum,
                         checksum, ensemble_size);
      return;
    }

    if (ensemble.size() < 6)
      return;

    std::vector<double> correlations(4, nan(""));
    for (int dt = 0; dt < ensemble[5]; dt++)
    {
      int offset = getu16le(ensemble.data() + 6 + 2 * dt);
      if (ensemble.size() - offset < 2)
        continue;
      // Three modes, encoded by the section_id: Bottom Track High Resolution Velocity
      // Bottom Track, Bottom Track Range
      uint16_t section_id = getu16le(ensemble.data() + offset);

      if (section_id == 0x5803)  // Bottom Track High Resolution Velocity
      {
        if (ensemble.size() - offset < 2 + 4 * 4)
          continue;
        res = boost::make_optional(mil_msgs::VelocityMeasurements());
        res->header.stamp = stamp;

        std::vector<geometry_msgs::Vector3> dirs;
        {
          double tilt = 30 * boost::math::constants::pi<double>() / 180;
          double x = sin(tilt);
          double z = cos(tilt);
          dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(-x, 0, -z));
          dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(+x, 0, -z));
          dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, +x, -z));
          dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, -x, -z));
        }

        // Keep track of which beams didn't return for logging
        std::vector<size_t> invalid_beams;
        invalid_beams.reserve(4);
        for (int i = 0; i < 4; i++)
        {
          mil_msgs::VelocityMeasurement m;
          m.direction = dirs[i];
          int32_t vel = gets32le(ensemble.data() + offset + 2 + 4 * i);
          m.velocity = -vel * .01e-3;
          if (vel == -3276801)  // -3276801 indicates no data
          {
            invalid_beams.push_back(i + 1);
            m.velocity = nan("");
          }
          res->velocity_measurements.push_back(m);
        }

        // Report  a list of invalid beams
        if (invalid_beams.size() > 0)
        {
          std::string to_log{ "DVL: didn't return bottom velocity for beam(s): " };
          for (auto beam : invalid_beams)
            to_log += std::to_string(beam) + " ";
          ROS_ERROR_THROTTLE(0.5, "%s", to_log.c_str());
        }
      }
      else if (section_id == 0x0600)  // Bottom Track
      {
        for (int i = 0; i < 4; i++)
          correlations[i] = *(ensemble.data() + offset + 32 + i);
      }
      else if (section_id == 0x5804)  // Bottom Track Range
      {
        if (ensemble.size() - offset < 2 + 4 * 3)
          continue;
        if (gets32le(ensemble.data() + offset + 10) <= 0)
        {
          ROS_ERROR_THROTTLE(0.5, "%s", "DVL: didn't return height over bottom");
          continue;
        }
        height_res = boost::make_optional(mil_msgs::RangeStamped());
        height_res->header.stamp = stamp;
        height_res->range = gets32le(ensemble.data() + offset + 10) * 0.1e-3;
      }
      if (res)
      {
        for (int i = 0; i < 4; i++)
        {
          res->velocity_measurements[i].correlation = correlations[i];
        }
      }
    }
  }

  void send_heartbeat()
  {
    double maxdepth = 15;

    std::stringstream buf;
    buf << "CR0\r";              // load factory settings (won't change baud rate)
    buf << "#BJ 100 110 000\r";  // enable only bottom track high res velocity and bottom track
                                 // range
    // buf << "#BK2\r"; // send water mass pings when bottom track pings fail
    // buf << "#BL7,36,46\r"; // configure near layer and far layer to 12 and 15 feet
    buf << "ES0\r";         // 0 salinity
    buf << "EX00000\r";     // no transformation
    buf << "EZ10000010\r";  // configure sensor sources. Provide manual data for everything except
                            // speed of sound and temperature
    buf << "BX" << std::setw(5) << std::setfill('0') << (int)(maxdepth * 10 + 0.5) << '\r';  // configure max depth
    buf << "TT2012/03/04, 05:06:07\r";                                                       // set RTC
    buf << "CS\r";                                                                           // start pinging

    std::string str = buf.str();

    try  // Write heartbeat to serial port
    {
      size_t written = 0;
      while (written < str.size())
      {
        written += p.write_some(boost::asio::buffer(str.data() + written, str.size() - written));
      }
    }
    catch (const std::exception &exc)
    {
      ROS_ERROR_THROTTLE(0.5, "DVL: error on write: %s; dropping heartbeat", exc.what());
    }
  }

  void abort()
  {
    p.close();
  }
};

}  // namespace rdi_explorer_dvl


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_DETAIL_EXPLODER_HPP
#define CRYPTO3_DETAIL_EXPLODER_HPP

#include <nil/crypto3/detail/stream_endian.hpp>
#include <nil/crypto3/detail/unbounded_shift.hpp>

#include <boost/integer.hpp>
#include <boost/static_assert.hpp>

#include <iterator>

#include <climits>
#include <cstring>

namespace nil {
    namespace crypto3 {
        namespace detail {

            // By definition, for all exploders, InputBits > OutputBits,
            // so we're taking one value and splitting it into many smaller values

            template<typename OutIter, int OutBits, typename T = typename std::iterator_traits<OutIter>::value_type>
            struct outvalue_helper {
                typedef T type;
            };
            template<typename OutIter, int OutBits>
            struct outvalue_helper<OutIter, OutBits, void> {
                typedef typename boost::uint_t<OutBits>::least type;
            };

            template<typename Endianness, int InputBits, int OutputBits, int k>
            struct exploder_step;

            template<int UnitBits, int InputBits, int OutputBits, int k>
            struct exploder_step<stream_endian::big_unit_big_bit<UnitBits>, InputBits, OutputBits, k> {
                template<typename InputValue, typename OutIter>
                static void step(InputValue const &x, OutIter &out) {
                    int const shift = InputBits - (OutputBits + k);
                    typedef typename outvalue_helper<OutIter, OutputBits>::type OutValue;
                    InputValue y = unbounded_shr<shift>(x);
                    *out++ = OutValue(low_bits<OutputBits>(y));
                }
            };

            template<int UnitBits, int InputBits, int OutputBits, int k>
            struct exploder_step<stream_endian::little_unit_big_bit<UnitBits>, InputBits, OutputBits, k> {
                template<typename InputValue, typename OutIter>
                static void step(InputValue const &x, OutIter &out) {
                    int const kb = (k % UnitBits);
                    int const ku = k - kb;
                    int const shift = OutputBits >= UnitBits ? k :
                                      InputBits >= UnitBits  ? ku + (UnitBits - (OutputBits + kb)) :
                                                               InputBits - (OutputBits + kb);
                    typedef typename outvalue_helper<OutIter, OutputBits>::type OutValue;
                    InputValue y = unbounded_shr<shift>(x);
                    *out++ = OutValue(low_bits<OutputBits>(y));
                }
            };

            template<int UnitBits, int InputBits, int OutputBits, int k>
            struct exploder_step<stream_endian::big_unit_little_bit<UnitBits>, InputBits, OutputBits, k> {
                template<typename InputValue, typename OutIter>
                static void step(InputValue const &x, OutIter &out) {
                    int const kb = (k % UnitBits);
                    int const ku = k - kb;
                    int const shift = OutputBits >= UnitBits ? InputBits - (OutputBits + k) :
                                      InputBits >= UnitBits  ? InputBits - (UnitBits + ku) + kb :
                                                               kb;
                    typedef typename outvalue_helper<OutIter, OutputBits>::type OutValue;
                    InputValue y = unbounded_shr<shift>(x);
                    *out++ = OutValue(low_bits<OutputBits>(y));
                }
            };

            template<int UnitBits, int InputBits, int OutputBits, int k>
            struct exploder_step<stream_endian::little_unit_little_bit<UnitBits>, InputBits, OutputBits, k> {
                template<typename InputValue, typename OutIter>
                static void step(InputValue const &x, OutIter &out) {
                    int const shift = k;
                    typedef typename outvalue_helper<OutIter, OutputBits>::type OutValue;
                    InputValue y = unbounded_shr<shift>(x);
                    *out++ = OutValue(low_bits<OutputBits>(y));
                }
            };

            template<int UnitBits, int InputBits, int OutputBits, int k>
            struct exploder_step<stream_endian::host_unit<UnitBits>, InputBits, OutputBits, k> {
                template<typename InputValue, typename OutIter>
                static void step(InputValue const &x, OutIter &out) {
                    typedef typename outvalue_helper<OutIter, OutputBits>::type OutValue;
                    BOOST_STATIC_ASSERT(sizeof(InputValue) * CHAR_BIT == InputBits);
                    BOOST_STATIC_ASSERT(sizeof(OutValue) * CHAR_BIT == OutputBits);
                    OutValue value;
                    std::memcpy(&value, (char *)&x + k / CHAR_BIT, OutputBits / CHAR_BIT);
                    *out++ = value;
                }
            };

            template<typename Endianness, int InputBits, int OutputBits, int k = 0>
            struct exploder;

            template<template<int> class Endian, int UnitBits, int InputBits, int OutputBits, int k>
            struct exploder<Endian<UnitBits>, InputBits, OutputBits, k> {

                // To keep the implementation managable, input and output sizes must
                // be multiples or factors of the unit size.
                // If one of these is firing, you may want a bit-only stream_endian
                // rather than one that mentions bytes or octets.
                BOOST_STATIC_ASSERT(!(InputBits % UnitBits && UnitBits % InputBits));
                BOOST_STATIC_ASSERT(!(OutputBits % UnitBits && UnitBits % OutputBits));

                typedef Endian<UnitBits> Endianness;
                typedef exploder_step<Endianness, InputBits, OutputBits, k> step_type;
                typedef exploder<Endianness, InputBits, OutputBits, k + OutputBits> next_type;

                template<typename InputValue, typename OutIter>
                static void explode(InputValue const &x, OutIter &out) {
                    step_type::step(x, out);
                    next_type::explode(x, out);
                }
            };

            template<template<int> class Endian, int UnitBits, int InputBits, int OutputBits>
            struct exploder<Endian<UnitBits>, InputBits, OutputBits, InputBits> {
                template<typename InputValue, typename OutIter>
                static void explode(InputValue const &, OutIter &) {
                }
            };

        }    // namespace detail
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_BLOCK_DETAIL_EXPLODER_HPP


#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <algorithm>
#include <boost/timer.hpp>

#include "myslam/config.h"
#include "myslam/visual_odometry.h"

namespace myslam
{

VisualOdometry::VisualOdometry() : state_(INITIALIZING), ref_(nullptr), curr_(nullptr), map_(new Map), num_lost_(0), num_inliers_(0)
{
    num_of_features_ = Config::get<int>("number_of_features");
    scale_factor_ = Config::get<double>("scale_factor");
    level_pyramid_ = Config::get<int>("level_pyramid");
    match_ratio_ = Config::get<float>("match_ratio");
    max_num_lost_ = Config::get<float>("max_num_lost");
    min_inliers_ = Config::get<int>("min_inliers");
    key_frame_min_rot = Config::get<double>("keyframe_rotation");
    key_frame_min_trans = Config::get<double>("keyframe_translation");
    orb_ = cv::ORB::create(num_of_features_, scale_factor_, level_pyramid_);
}

VisualOdometry::~VisualOdometry()
{
}

bool VisualOdometry::addFrame(Frame::Ptr frame)
{
    switch (state_)
    {
    case INITIALIZING:
    {
        state_ = OK;
        ref_ = curr_ = frame;
        map_->insertKeyFrame(frame);
        extractKeyPoints();
        computeDescriptors();
        setRef3DPoints();
        break;
    }
    case OK:
    {
        curr_ = frame;
        extractKeyPoints();
        computeDescriptors();
        featureMatching();
        poseEstimationPnP();
        if (checkEstimatedPose())
        {
            curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_; //T_cw=T_cr*T_rw
            ref_ = curr_;
            setRef3DPoints();
            num_lost_ = 0;
            if (checkKeyFrame())
                addKeyFrame();
        }
        else
        {
            num_lost_++;
            if (num_lost_ > max_num_lost_)
                state_ = LOST;
            return false;
        }
        break;
    }
    case LOST:
    {
        cout << "vo has lost" << endl;
        break;
    }
    }
    return true;
}

void VisualOdometry::extractKeyPoints()
{
    orb_->detect(curr_->color_, keypoints_curr_);
}

void VisualOdometry::computeDescriptors()
{
    orb_->compute(curr_->color_, keypoints_curr_, descriptors_curr_);
}

void VisualOdometry::featureMatching()
{
    vector<cv::DMatch> matches;
    cv::BFMatcher matcher(cv::NORM_HAMMING);
    matcher.match(descriptors_ref_, descriptors_curr_, matches);

    //定义lambda函数，从matches里，按照其中元素的cv::DMatch.distance找到最小距离的元素
    //得到它的最小距离
    float min_dis = std::min_element(
                        matches.begin(), matches.end(),
                        [](const cv::DMatch &m1, const cv::DMatch &m2) {
                            return m1.distance < m2.distance;
                        })
                        ->distance;

    feature_matches_.clear();

    // 找到距离合适的匹配
    // 距离小于30或min_dis的match_ratio倍，算匹配上
    for (cv::DMatch &m : matches)
    {
        if (m.distance < max<float>(min_dis * match_ratio_, 30.0))
        {
            feature_matches_.push_back(m);
        }
    }
    cout << "good matches:" << feature_matches_.size() << endl;
}

void VisualOdometry::setRef3DPoints()
{
    pts_3d_ref_.clear();
    descriptors_ref_ = Mat();
    //调用它的时候，已经把curr_赋给ref_了，所以此时res_实际上是curr_。
    // 找当前帧所有关键点，看能不能有深度，有的话计算相机坐标系的三维坐标，
    // 并且把由深度的描述子保留给ref_。
    // 但其实，把ref和curr统一是不是更好？
    for (size_t i = 0; i < keypoints_curr_.size(); i++)
    {
        double d = ref_->findDepth(keypoints_curr_[i]);
        if (d > 0)
        {
            //cout<<keypoints_curr_[i].pt.x<<' '<<keypoints_curr_[i].pt.y<<' '<<d<<' ';
            Vector3d p_cam=ref_->camera_->pixel2camera(
                Vector2d(keypoints_curr_[i].pt.x,keypoints_curr_[i].pt.y),d);
            pts_3d_ref_.push_back(cv::Point3f(p_cam(0, 0), p_cam(1, 0), p_cam(2, 0)));
            //cout<<p_cam(0, 0)<<' '<<p_cam(1, 0)<<' '<<p_cam(2, 0)<<endl;
            descriptors_ref_.push_back(descriptors_curr_.row(i));
        }
    }
}

void VisualOdometry::poseEstimationPnP()
{
    vector<cv::Point3f> pts3d;
    vector<cv::Point2f> pts2d;
    // query是要匹配的描述子，train是被匹配的描述子
    for (cv::DMatch m : feature_matches_)
    {
        pts3d.push_back(pts_3d_ref_[m.queryIdx]);
        pts2d.push_back(keypoints_curr_[m.trainIdx].pt);
    }
    // 内参
    Mat K = (cv::Mat_<double>(3, 3) << ref_->camera_->fx_, 0, ref_->camera_->cx_,
             0, ref_->camera_->fy_, ref_->camera_->cy_,
             0, 0, 1);
    //输出
    Mat rvec, tvec, inliers;
    cv::solvePnPRansac(pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers);
    num_inliers_ = inliers.rows;
    cout << "pnp inliers:" << num_inliers_ << endl;
    T_c_r_estimated_ = SE3(
        SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)),
        Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
    );
}

bool VisualOdometry::checkEstimatedPose()
{
    if (num_inliers_ < min_inliers_)
    {
        cout << "reject because inlier is too small:" << num_inliers_ << endl;
        return false;
    }
    Sophus::Vector6d d = T_c_r_estimated_.log();
    if (d.norm() > 5.0)
    {
        cout << "reject because motion is too large:" << d.norm() << endl;
        return false;
    }
    return true;
}

bool VisualOdometry::checkKeyFrame()
{
    Sophus::Vector6d d = T_c_r_estimated_.log();
    Vector3d trans = d.head<3>();
    Vector3d rot = d.tail<3>();
    if (rot.norm() > key_frame_min_rot || trans.norm() > key_frame_min_trans)
        return true;
    return false;
}

void VisualOdometry::addKeyFrame()
{
    cout << "adding a key frame" << endl;
    map_->insertKeyFrame(curr_);
}

} // namespace myslam


#include "tromble_loss.h"
#include "fast_lexical_cast.hpp"

#include <boost/algorithm/string/predicate.hpp>
#include <boost/circular_buffer.hpp>
#include <boost/functional/hash.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/tokenizer.hpp>
#include <boost/unordered_map.hpp>

#include <cmath>
#include <fstream>
#include <vector>

#include "sentence_metadata.h"
#include "trule.h"
#include "tdict.h"

using namespace std;

namespace {

typedef unsigned char GramCount;

struct RefCounts {
  GramCount max;
  std::vector<GramCount> refs;
  size_t length;
};

typedef boost::unordered_map<std::vector<WordID>, size_t, boost::hash<std::vector<WordID> > > NGramMap;

// Take all the n-grams in the references and stuff them into ngrams.
void MakeNGramMapFromReferences(const vector<vector<WordID> > &references,
                                int n,
                                vector<RefCounts> *counts,
                                NGramMap *ngrams) {
  ngrams->clear();
  std::pair<vector<WordID>, size_t> insert_me;
  vector<WordID> &ngram = insert_me.first;
  ngram.reserve(n);
  size_t &id = insert_me.second;
  id = 0;
  for (int refi = 0; refi < references.size(); ++refi) {
    const vector<WordID>& ref = references[refi];
    const int s = ref.size();
    for (int j=0; j<s; ++j) {
      const int remaining = s-j;
      const int k = (n < remaining ? n : remaining);
      ngram.clear();
      for (unsigned int i = 0; i < k; ++i) {
        ngram.push_back(ref[j + i]);
        std::pair<NGramMap::iterator, bool> ret(ngrams->insert(insert_me));
        if (ret.second) {
          counts->resize(id + 1);
          RefCounts &ref_counts = counts->back();
          ref_counts.max = 1;
          ref_counts.refs.resize(references.size());
          ref_counts.refs[refi] = 1;
          ref_counts.length = ngram.size();
          ++id;
        } else {
          RefCounts &ref_counts = (*counts)[ret.first->second];
          ref_counts.max = std::max(ref_counts.max, ++ref_counts.refs[refi]);
        }
      }
    }
  }
}

struct MutableState {
  MutableState(void *from, size_t n) : length(reinterpret_cast<size_t*>(from)), left(reinterpret_cast<WordID *>(length + 1)), right(left + n - 1), counts(reinterpret_cast<GramCount *>(right + n - 1)) {}
  size_t *length;
  WordID *left, *right;
  GramCount *counts;
  static size_t Size(size_t n, size_t bound_ngram_id) { return sizeof(size_t) + (n - 1) * 2 * sizeof(WordID) + bound_ngram_id * sizeof(GramCount); }
};

struct ConstState {
  ConstState(const void *from, size_t n) : length(reinterpret_cast<const size_t*>(from)), left(reinterpret_cast<const WordID *>(length + 1)), right(left + n - 1), counts(reinterpret_cast<const GramCount *>(right + n - 1)) {}
  const size_t *length;
  const WordID *left, *right;
  const GramCount *counts;
  static size_t Size(size_t n, size_t bound_ngram_id) { return sizeof(size_t) + (n - 1) * 2 * sizeof(WordID) + bound_ngram_id * sizeof(GramCount); }
};

template <class T> struct CompatibleHashRange : public std::unary_function<const boost::iterator_range<T> &, size_t> {
  size_t operator()(const boost::iterator_range<T> &range) const {
    return boost::hash_range(range.begin(), range.end());
  }
};

template <class T> struct CompatibleEqualsRange : public std::binary_function<const boost::iterator_range<T> &, const std::vector<WordID> &, size_t> {
  size_t operator()(const boost::iterator_range<T> &range, const std::vector<WordID> &vec) const {
    return boost::algorithm::equals(range, vec);
  }
  size_t operator()(const std::vector<WordID> &vec, const boost::iterator_range<T> &range) const {
    return boost::algorithm::equals(range, vec);
  }
};

void AddWord(const boost::circular_buffer<WordID> &segment, size_t min_length, const NGramMap &ref_grams, GramCount *counters) {
  typedef boost::circular_buffer<WordID>::const_iterator BufferIt;
  typedef boost::iterator_range<BufferIt> SegmentRange;
  if (segment.size() < min_length) return;
#if 0
  CompatibleHashRange<BufferIt> hasher;
  CompatibleEqualsRange<BufferIt> equals;
  for (BufferIt seg_start(segment.end() - min_length); ; --seg_start) {
    NGramMap::const_iterator found = ref_grams.find(SegmentRange(seg_start, segment.end()));
    if (found == ref_grams.end()) break;
    ++counters[found->second];
    if (seg_start == segment.begin()) break;
  }
#endif
}

} // namespace

class TrombleLossComputerImpl {
 public:
  explicit TrombleLossComputerImpl(const std::string &params) : star_(TD::Convert("<{STAR}>")) {
    typedef boost::tokenizer<boost::char_separator<char> > Tokenizer;
    // Argument parsing
    std::string ref_file_name;
    Tokenizer tok(params, boost::char_separator<char>(" "));
    Tokenizer::iterator i = tok.begin();
    if (i == tok.end()) {
      std::cerr << "TrombleLossComputer needs a reference file name." << std::endl;
      exit(1);
    }
    ref_file_name = *i++;
    if (i == tok.end()) {
      std::cerr << "TrombleLossComputer needs to know how many references." << std::endl;
      exit(1);
    }
    num_refs_ = boost::lexical_cast<unsigned int>(*i++);
    for (; i != tok.end(); ++i) {
     thetas_.push_back(boost::lexical_cast<double>(*i));
    }
    if (thetas_.empty()) {
      std::cerr << "TrombleLossComputer is pointless with no weight on n-grams." << std::endl;
      exit(1);
    }

    // Read references file.
    std::ifstream ref_file(ref_file_name.c_str());
    if (!ref_file) {
      std::cerr << "Could not open TrombleLossComputer file " << ref_file_name << std::endl;
      exit(1);
    }
    std::string ref;
    vector<vector<WordID> > references(num_refs_);
    bound_ngram_id_ = 0;
    for (unsigned int sentence = 0; ref_file; ++sentence) {
      for (unsigned int refidx = 0; refidx < num_refs_; ++refidx) {
        if (!getline(ref_file, ref)) {
          if (refidx == 0) break;
          std::cerr << "Short read of " << refidx << " references for sentence " << sentence << std::endl;
          exit(1);
        }
        TD::ConvertSentence(ref, &references[refidx]);
      }
      ref_ids_.resize(sentence + 1);
      ref_counts_.resize(sentence + 1);
      MakeNGramMapFromReferences(references, thetas_.size(), &ref_counts_.back(), &ref_ids_.back());
      bound_ngram_id_ = std::max(bound_ngram_id_, ref_ids_.back().size());
    }
  }

  size_t StateSize() const {
    // n-1 boundary words plus counts for n-grams currently rendered as bytes even though most would fit in bits.
    // Also, this is cached by higher up classes so no need to cache here.
    return MutableState::Size(thetas_.size(), bound_ngram_id_);
  }

  double Traversal(
      const SentenceMetadata &smeta,
      const TRule &rule,
      const vector<const void*> &ant_contexts,
      void *out_context) const {
    // TODO: get refs from sentence metadata.
    // This will require resizable features.
    if (smeta.GetSentenceID() >= ref_ids_.size()) {
      std::cerr << "Sentence ID " << smeta.GetSentenceID() << " doesn't have references; there are only " << ref_ids_.size() << " references." << std::endl;
      exit(1);
    }
    const NGramMap &ngrams = ref_ids_[smeta.GetSentenceID()];
    MutableState out_state(out_context, thetas_.size());
    memset(out_state.counts, 0, bound_ngram_id_ * sizeof(GramCount));
    boost::circular_buffer<WordID> history(thetas_.size());
    std::vector<const void*>::const_iterator ant_context = ant_contexts.begin();
    *out_state.length = 0;
    size_t pushed = 0;
    const size_t keep = thetas_.size() - 1;
    for (vector<WordID>::const_iterator rhs = rule.e().begin(); rhs != rule.e().end(); ++rhs) {
      if (*rhs < 1) {
        assert(ant_context != ant_contexts.end());
        // Constituent
        ConstState rhs_state(*ant_context, thetas_.size());
        *out_state.length += *rhs_state.length;
        {
          GramCount *accum = out_state.counts;
          for (const GramCount *c = rhs_state.counts; c != rhs_state.counts + ngrams.size(); ++c, ++accum) {
            *accum += *c;
          }
        }
        const WordID *w = rhs_state.left;
        bool long_constit = true;
        for (size_t i = 1; i <= keep; ++i, ++w) {
          if (*w == star_) {
            long_constit = false;
            break;
          }
          history.push_back(*w);
          if (++pushed == keep) {
            std::copy(history.begin(), history.end(), out_state.left);
          }
          // Now i is the length of the history coming from this constituent.  So it needs at least i+1 words to have a cross-child add.
          AddWord(history, i + 1, ngrams, out_state.counts);
        }
        // If the consituent is shorter than thetas_.size(), then the
        // constituent's left is the entire constituent, so history is already
        // correct.  Otherwise, the entire right hand side is the entire
        // history.
        if (long_constit) {
          history.assign(thetas_.size(), rhs_state.right, rhs_state.right + keep);
        }
        ++ant_context;
      } else {
        // Word
        ++*out_state.length;
        history.push_back(*rhs);
        if (++pushed == keep) {
          std::copy(history.begin(), history.end(), out_state.left);
        }
        AddWord(history, 1, ngrams, out_state.counts);
      }
    }
    // Fill in left and right constituents.
    if (pushed < keep) {
      std::copy(history.begin(), history.end(), out_state.left);
      for (WordID *i = out_state.left + pushed; i != out_state.left + keep; ++i) {
        *i = star_;
      }
      std::copy(out_state.left, out_state.left + keep, out_state.right);
    } else if(pushed == keep) {
      std::copy(history.begin(), history.end(), out_state.right);
    } else if ((pushed > keep) && !history.empty()) {
      std::copy(history.begin() + 1, history.end(), out_state.right);
    }
    std::vector<RefCounts>::const_iterator ref_info = ref_counts_[smeta.GetSentenceID()].begin();
    // Clip the counts and count matches.
    // Indexed by reference then by length.
    std::vector<std::vector<unsigned int> > matches(num_refs_, std::vector<unsigned int>(thetas_.size()));
    for (GramCount *c = out_state.counts; c != out_state.counts + ngrams.size(); ++c, ++ref_info) {
      *c = std::min(*c, ref_info->max);
      if (*c) {
        for (unsigned int refidx = 0; refidx < num_refs_; ++refidx) {
          assert(ref_info->length >= 1);
          assert(ref_info->length - 1 < thetas_.size());
          matches[refidx][ref_info->length - 1] += std::min(*c, ref_info->refs[refidx]);
        }
      }
    }
    double best_score = 0.0;
    for (unsigned int refidx = 0; refidx < num_refs_; ++refidx) {
      double score = 0.0;
      for (unsigned int j = 0; j < std::min(*out_state.length, thetas_.size()); ++j) {
        score += thetas_[j] * static_cast<double>(matches[refidx][j]) / static_cast<double>(*out_state.length - j);
      }
      best_score = std::max(best_score, score);
    }
    return best_score;
  }

 private:
  unsigned int num_refs_;
  // Indexed by sentence id.
  std::vector<NGramMap> ref_ids_;
  // Then by id from ref_ids_.
  std::vector<std::vector<RefCounts> > ref_counts_;

  // thetas_[0] is the weight for 1-grams
  std::vector<double> thetas_;

  // All ngram ids in ref_ids_ are < this value.
  size_t bound_ngram_id_;

  const WordID star_;
};

TrombleLossComputer::TrombleLossComputer(const std::string &params) :
    boost::base_from_member<PImpl>(new TrombleLossComputerImpl(params)),
    FeatureFunction(boost::base_from_member<PImpl>::member->StateSize()),
    fid_(FD::Convert("TrombleLossComputer")) {}

TrombleLossComputer::~TrombleLossComputer() {}

void TrombleLossComputer::TraversalFeaturesImpl(const SentenceMetadata& smeta,
                                     const Hypergraph::Edge& edge,
                                     const vector<const void*>& ant_contexts,
                                     SparseVector<double>* features,
                                     SparseVector<double>* estimated_features,
                                     void* out_context) const {
  (void) estimated_features;
  const double loss = boost::base_from_member<PImpl>::member->Traversal(smeta, *edge.rule_, ant_contexts, out_context);
  features->set_value(fid_, loss);
}