#include "include/MMVII_all.h"

// #include <Eigen/Dense>

namespace MMVII
{

/* ========================== */
/*     cDataGenUnTypedIm      */
/* ========================== */

template <const int Dim> cDataGenUnTypedIm<Dim>::cDataGenUnTypedIm
                         (
                             const cPtxd<int,Dim> & aP0,
                             const cPtxd<int,Dim> & aP1
                         )  :
                            cRectObj<Dim>(aP0,aP1)
{
}

/* ========================== */
/*          cPtxd             */
/* ========================== */

/*
template <class Type,const int Dim> cPtxd<Type,Dim>  cPtxd<Type,Dim>::PCste(const Type & aVal)
{
    cPtxd<Type,Dim> aRes;
    for (int aK=0 ; aK<Dim; aK++)
        aRes.mCoords[aK]= aVal;
    return aRes;
}
*/

/* ========================== */
/*          ::                */
/* ========================== */
    //  To test Error_Handler mecanism

static std::string MesNegSz="Negative size in rect object";
static std::string  TestErHandler;
void TestBenchRectObjError(const std::string & aType,const std::string &  aMes,const char * aFile,int aLine)
{
   TestErHandler = aMes;
}

/* ========================== */
/*          cRectObj          */
/* ========================== */

/// Computation to get the point we have at end of iterating a rectangle
template <const int Dim> cPtxd<int,Dim> CalPEnd(const cPtxd<int,Dim> & aP0,const cPtxd<int,Dim> & aP1)
{
    cPtxd<int,Dim> aRes = aP0;
    aRes[Dim-1] = aP1[Dim-1] ;
    return aRes;
}

template <const int Dim>   cRectObj<Dim>::cRectObj(const cPtxd<int,Dim> & aP0,const cPtxd<int,Dim> & aP1,bool AllowEmpty) :
     mP0  (aP0),
     mP1  (aP1),
     mSz  (aP0),
     mBegin  (*this,mP0),
     mEnd    (*this,CalPEnd(aP0,aP1)),
     mNbElem (1)
{
    for (int aK=Dim-1 ; aK>=0 ; aK--)
    {
       mSz[aK] = mP1[aK] - mP0[aK];
       if (AllowEmpty)
       {
          MMVII_INTERNAL_ASSERT_strong(mSz[aK]>=0,MesNegSz);
       }
       else
       {
          MMVII_INTERNAL_ASSERT_strong(mSz[aK]>0,MesNegSz);
       }
       mNbElem *= mSz[aK];
       mSzCum[aK] = mNbElem;
    }
    // std::cout << mNbElem << "\n";
}

template <const int Dim>   cRectObj<Dim>::cRectObj(const cPtxd<int,Dim> & aP0,const cPtxd<int,Dim> & aP1) :
   cRectObj<Dim>(aP0,aP1,false)
{
}

template <const int Dim> void cRectObj<Dim>::AssertSameArea(const cRectObj<Dim> & aR2) const
{
    MMVII_INTERNAL_ASSERT_strong((*this)==aR2,"Rect obj were expected to have identic area");
}
template <const int Dim> void cRectObj<Dim>::AssertSameSz(const cRectObj<Dim> & aR2) const
{
    MMVII_INTERNAL_ASSERT_strong(Sz()==aR2.Sz(),"Rect obj were expected to have identic size");
}

template <const int Dim> bool cRectObj<Dim>::operator == (const cRectObj<Dim> aR2) const
{
    return (mP0==aR2.mP0) && (mP1==aR2.mP1);
}

template <const int Dim> bool cRectObj<Dim>::IncludedIn(const cRectObj<Dim> & aR2) const
{
    return SupEq(mP0,aR2.mP0) && InfEq(mP1,aR2.mP1) ;
}

template <const int Dim> cRectObj<Dim> cRectObj<Dim>::Translate(const cPtxd<int,Dim> & aTr) const
{
   return cRectObj<Dim>(mP0+aTr,mP1+aTr);
}

template <const int Dim> cPtxd<int,Dim>  cRectObj<Dim>::FromNormaliseCoord(const cPtxd<double,Dim> & aPN) const
{
    cPtxd<int,Dim> aRes;
    for (int aK=0 ; aK<Dim ; aK++)
    {
        aRes[aK] = mP0[aK] + round_down(mSz[aK]*aPN[aK]);
    }
    return Proj(aRes);
}

template <const int Dim>  cPtxd<double,Dim>  cRectObj<Dim>::RandomNormalised()
{
   cPtxd<double,Dim>  aRes;
   for (int aK=0 ; aK<Dim ; aK++)
   {
        aRes[aK] = RandUnif_0_1();
   }
   return aRes;
}

template <const int Dim> cPtxd<int,Dim>   cRectObj<Dim>::GeneratePointInside() const
{
   return FromNormaliseCoord(RandomNormalised());
}

template <const int Dim> cRectObj<Dim>  cRectObj<Dim>::GenerateRectInside(double aPowSize) const
{
    cPtxd<int,Dim> aP0;
    cPtxd<int,Dim> aP1;
    for (int aK=0 ; aK<Dim ; aK++)
    {
        double aSzRed = pow(RandUnif_0_1(),aPowSize);
        double aX0 = (1-aSzRed) * RandUnif_0_1();
        double aX1 = aX0 + aSzRed;
        int aI0 = round_down(aX0*mSz[aK]);
        int aI1 = round_down(aX1*mSz[aK]);
        aI1 = std::min(mP1[aK]-1,std::max(aI1,aI0+1));
        aI0  = std::max(mP0[aK],std::min(aI0,aI1-1));
        aP0[aK] = aI0;
        aP1[aK] = aI1;

    }
    return cRectObj<Dim>(aP0,aP1);
}

#define MACRO_INSTATIATE_PRECT_DIM(DIM)\
template class cRectObj<DIM>;\
template class cDataGenUnTypedIm<DIM>;\
template <> const cRectObj<DIM> cRectObj<DIM>::Empty00(cPtxd<int,DIM>::PCste(0),cPtxd<int,DIM>::PCste(0),true);

MACRO_INSTATIATE_PRECT_DIM(1)
MACRO_INSTATIATE_PRECT_DIM(2)
MACRO_INSTATIATE_PRECT_DIM(3)
/*

*/

/* ========================== */
/*          cDataTypedIm      */
/* ========================== */

template <class Type,const int Dim>
    cDataTypedIm<Type,Dim>::cDataTypedIm(const cPtxd<int,Dim> & aP0,const cPtxd<int,Dim> & aP1,Type *aRawDataLin,eModeInitImage aModeInit) :
        cDataGenUnTypedIm<Dim>(aP0,aP1),
        mDoAlloc (aRawDataLin==0),
        mRawDataLin (mDoAlloc ? cMemManager::Alloc<Type>(NbElem())  : aRawDataLin)
{
   Init(aModeInit);
}

template <class Type,const int Dim>
    cDataTypedIm<Type,Dim>::~cDataTypedIm()
{
   if (mDoAlloc)
      cMemManager::Free(mRawDataLin);
}

template <class Type,const int Dim>
        double cDataTypedIm<Type,Dim>::L1Dist(const cDataTypedIm<Type,Dim> & aI2) const
{
    tRO::AssertSameArea(aI2);
    double aRes = 0.0;
    for (int aK=0 ; aK<NbElem() ; aK++)
       aRes += std::abs(mRawDataLin[aK]-aI2.mRawDataLin[aK]);

   return aRes/NbElem();
}
template <class Type,const int Dim>
        double cDataTypedIm<Type,Dim>::L2Dist(const cDataTypedIm<Type,Dim> & aI2) const
{
    tRO::AssertSameArea(aI2);
    double aRes = 0.0;
    for (int aK=0 ; aK<NbElem() ; aK++)
       aRes += R8Square(mRawDataLin[aK]-aI2.mRawDataLin[aK]);

   return sqrt(aRes/NbElem());
}
template <class Type,const int Dim>
        double cDataTypedIm<Type,Dim>::LInfDist(const cDataTypedIm<Type,Dim> & aI2) const
{
    tRO::AssertSameArea(aI2);
    double aRes = 0.0;
    for (int aK=0 ; aK<NbElem() ; aK++)
       aRes = std::max(aRes,(double)std::abs(mRawDataLin[aK]-aI2.mRawDataLin[aK]));

   return aRes;
}

template <class Type,const int Dim>
        double cDataTypedIm<Type,Dim>::L1Norm() const
{
    double aRes = 0.0;
    for (int aK=0 ; aK<NbElem() ; aK++)
       aRes += std::abs(mRawDataLin[aK]);

   return aRes/NbElem();
}
template <class Type,const int Dim>
        double cDataTypedIm<Type,Dim>::L2Norm() const
{
    double aRes = 0.0;
    for (int aK=0 ; aK<NbElem() ; aK++)
       aRes += R8Square(mRawDataLin[aK]);

   return sqrt(aRes/NbElem());
}
template <class Type,const int Dim>
        double cDataTypedIm<Type,Dim>::LInfNorm() const
{
    double aRes = 0.0;
    for (int aK=0 ; aK<NbElem() ; aK++)
       aRes = std::max(aRes,(double) std::abs(mRawDataLin[aK]));

   return aRes;
}

template <class Type,const int Dim> void  cDataTypedIm<Type,Dim>::DupIn(cDataTypedIm<Type,Dim> & aIm) const
{
    tRO::AssertSameSz(aIm);
    MemCopy(aIm.RawDataLin(),RawDataLin(),NbElem());
    // MMVII_INTERNAL_ASSERT_strong(mSz[aK]>=0,"");
}

template <class Type,const int Dim> void  cDataTypedIm<Type,Dim>::InitCste(const Type & aVal)
{
   if (aVal==0)
   {
      InitNull();
   }
   else
   {
      for (tINT8 aK=0 ; aK< NbElem() ; aK++)
           mRawDataLin[aK] = aVal;
   }
}

template <class Type,const int Dim> void  cDataTypedIm<Type,Dim>::InitRandom()
{
   for (tINT8 aK=0 ; aK< NbElem() ; aK++)
       mRawDataLin[aK] = tTraits::RandomValue();
}

template <class Type,const int Dim> void  cDataTypedIm<Type,Dim>::InitNull()
{
    MEM_RAZ(mRawDataLin,NbElem());
}

template <class Type,const int Dim> void  cDataTypedIm<Type,Dim>::InitId()
{
   // Check it is a square matrix
   MMVII_INTERNAL_ASSERT_bench((Dim==2)  ,"Init Id : dim !=2");
   for (int aK=0 ; aK<Dim ; aK++)
   {
       MMVII_INTERNAL_ASSERT_bench((P0()[aK]==0)  ,"Init Id P0!= (0,0)");
   }
   MMVII_INTERNAL_ASSERT_bench((P1()[0]==P1()[1])  ,"Init Id, non square image");

   InitNull();
   for (int aK=0 ;  aK<NbElem()  ; aK += P1()[0]+1)
   {
       mRawDataLin[aK] = 1;
   }
}

template <class Type,const int Dim> void  cDataTypedIm<Type,Dim>::Init(eModeInitImage aMode)
{
    switch(aMode)
    {
       case eModeInitImage::eMIA_Rand     : InitRandom(); return;
       case eModeInitImage::eMIA_Null     : InitNull(); return;
       case eModeInitImage::eMIA_MatrixId : InitId(); return;
       case eModeInitImage::eMIA_NoInit   : ;
    }
}

/*
template class cDataTypedIm<tREAL4,1>;
template class cDataTypedIm<tREAL4,2>;
template class cDataTypedIm<tREAL4,3>;
*/

#define MACRO_INSTANTIATE_cDataTypedIm(aType)\
template class cDataTypedIm<aType,1>;\
template class cDataTypedIm<aType,2>;\
template class cDataTypedIm<aType,3>;

MACRO_INSTANTIATE_cDataTypedIm(tINT1)
MACRO_INSTANTIATE_cDataTypedIm(tINT2)
MACRO_INSTANTIATE_cDataTypedIm(tINT4)

MACRO_INSTANTIATE_cDataTypedIm(tU_INT1)
MACRO_INSTANTIATE_cDataTypedIm(tU_INT2)
MACRO_INSTANTIATE_cDataTypedIm(tU_INT4)

MACRO_INSTANTIATE_cDataTypedIm(tREAL4)
MACRO_INSTANTIATE_cDataTypedIm(tREAL8)
MACRO_INSTANTIATE_cDataTypedIm(tREAL16)
/*
*/

/* ========================== */
/*          cBenchBaseImage   */
/* ========================== */

    ///  cBenchBaseImage
class     cBenchBaseImage
{
    public :
    static void DoBenchBI();
    static void DoBenchRO();
};

void cBenchBaseImage::DoBenchBI()
{
    cMemState  aState = cMemManager::CurState() ;
    {

        // cDataTypedIm<tREAL4,2> aBI(cPt2di(2,3),cPt2di(10,9));
    }
    cMemManager::CheckRestoration(aState);
}

void cBenchBaseImage::DoBenchRO()
{
   {
      cRectObj<1>  aR(cPt1di(2),cPt1di(10));
      MMVII_INTERNAL_ASSERT_bench(cPt1di(aR.Sz()) ==cPt1di(8),"Bench sz RectObj");
      MMVII_INTERNAL_ASSERT_bench(aR.NbElem()==8,"Bench sz RectObj");
   }

   // Test the SetErrorHandler mecanism, abality to recover on error
   {
       MMVII_SetErrorHandler(TestBenchRectObjError);
       cRectObj<1>  aR(cPt1di(10),cPt1di(0));
       MMVII_RestoreDefaultHandle();
       MMVII_INTERNAL_ASSERT_bench(MesNegSz==TestErHandler,"Handler mechanism");
   }
   {
      cRectObj<2>  aR(cPt2di(2,3),cPt2di(10,9));
      MMVII_INTERNAL_ASSERT_bench(cPt2di(aR.Sz()) ==cPt2di(8,6),"Bench sz RectObj");
      MMVII_INTERNAL_ASSERT_bench(aR.NbElem()==48,"Bench sz RectObj");

      MMVII_INTERNAL_ASSERT_bench(aR.Inside(cPt2di(2,8)),"Bench inside rect");
      MMVII_INTERNAL_ASSERT_bench(aR.Inside(cPt2di(9,3)),"Bench inside rect");
      MMVII_INTERNAL_ASSERT_bench(!aR.Inside(cPt2di(2,9)),"Bench inside rect");
      MMVII_INTERNAL_ASSERT_bench(!aR.Inside(cPt2di(1,8)),"Bench inside rect");
      MMVII_INTERNAL_ASSERT_bench(!aR.Inside(cPt2di(10,3)),"Bench inside rect");
      MMVII_INTERNAL_ASSERT_bench(!aR.Inside(cPt2di(9,2)),"Bench inside rect");
      MMVII_INTERNAL_ASSERT_bench(!aR.Inside(cPt2di(10,2)),"Bench inside rect");
   }
}

    //----------  ::  external call ---------

void BenchBaseImage()
{
    cBenchBaseImage::DoBenchBI();
}

void BenchRectObj()
{
    cBenchBaseImage::DoBenchRO();
}

};


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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 SMOOTH__LIE_GROUP_SPARSE_HPP_
#define SMOOTH__LIE_GROUP_SPARSE_HPP_

#include <Eigen/Sparse>

#include "internal/utils.hpp"
#include "lie_group.hpp"

/**
 * @file
 * @brief Sparse versions of certain Lie group methods.
 *
 * Implemented methods contain two parts: a pattern and the function itself. The pattern is in the
 * form of an inline sparse matrix variable that is pre-allocated with the appropriate nonzeros.
 *
 * The intended usage pattern is to copy a pattern and use the resulting variable in calls to these
 * methods. That way no additional  allocation is needed.
 *
 * Example:
 * @code
 * auto my_var = ad_sparse_pattern;  // copy pattern
 * ad_sparse(my_var, a);             // update my_var in-place without allocation
 * @endcode
 */

namespace smooth {

/// @brief Forward-declare
// clang-format off
template<typename D> class C1Base;
template<typename D> class SE2Base;
template<typename D> class SE3Base;
template<typename D> class SO2Base;
template<typename D> class BundleBase;
// clang-format on

namespace traits {

/**
 * @brief Traits that defines sparsity patterns for various groups.
 */
template<typename G>
struct lie_sparse
{};

}  // namespace traits

////////////////////////////////////////////////////////
//// FREE FUNCTIONS FOR SPARSE LIE GROUP PROPERTIES ////
////////////////////////////////////////////////////////

/**
 * @brief Generators of lie algebra (inline variable).
 */
template<LieGroup G>
inline std::array<Eigen::SparseMatrix<Scalar<G>>, Dof<G>> generators_sparse =
  []() -> std::array<Eigen::SparseMatrix<Scalar<G>>, Dof<G>> {
  std::array<Eigen::SparseMatrix<Scalar<G>>, Dof<G>> ret;
  for (auto i = 0u; i < Dof<G>; ++i) {
    ret[i] = ad<G>(Tangent<G>::Unit(i)).sparseView();
    ret[i].makeCompressed();
  }
  return ret;
}();

/**
 * @brief Sparsity pattern of ad_sparse (inline variable).
 */
template<LieGroup G>
inline Eigen::SparseMatrix<Scalar<G>> ad_sparse_pattern = [] {
  Eigen::SparseMatrix<Scalar<G>> ret(Dof<G>, Dof<G>);
  if constexpr (!IsCommutative<G>) {
    for (const auto & gen : generators_sparse<G>) { ret += gen; }
  }
  ret.makeCompressed();
  ret.coeffs().setZero();
  return ret;
}();

/**
 * @brief Sparse ad.
 *
 * @param[in, out] sp allocated and compressed sparse matrix
 * @param[in] a Lie algebra element
 *
 * @warning sp must be pre-allocated with the appropriate nonzeros and compressed
 *
 * @see ad_sparse_pattern() for a pre-allocated pattern.
 */
template<LieGroup G>
inline void ad_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a)
{
  assert(sp.isCompressed());
  sp.coeffs().setZero();
  for (auto k = 0u; k < Dof<G>; ++k) { sp += a(k) * generators_sparse<G>[k]; }
  assert(sp.isCompressed());
}

/**
 * @brief Sparsity pattern of dr_exp_sparse, dr_expinv_sparse (inline variable).
 */
template<LieGroup G>
inline Eigen::SparseMatrix<Scalar<G>> d_exp_sparse_pattern = [] {
  Eigen::SparseMatrix<Scalar<G>> ret(Dof<G>, Dof<G>);
  if constexpr (IsCommutative<G>) {
    // identity matrix
    for (auto i = 0u; i < Dof<G>; ++i) { ret.insert(i, i) = Scalar<G>(1); }
  } else if constexpr (requires { traits::lie_sparse<G>::d_exp_sparse_pattern; }) {
    // use specialized method if one exists
    ret = traits::lie_sparse<G>::d_exp_sparse_pattern;
  } else {
    // fall back on dense pattern
    for (auto i = 0u; i < Dof<G>; ++i) {
      for (auto j = 0u; j < Dof<G>; ++j) {
        ret.insert(i, j) = (i == j) ? Scalar<G>(1) : Scalar<G>(0);
      }
    }
  }
  ret.makeCompressed();
  return ret;
}();

/**
 * @brief Sparse dr_exp.
 *
 * @tparam G Lie group
 * @tparam Inv compute dr_expinv
 *
 * @param[in, out] sp allocated and compressed sparse matrix
 * @param[in] a Lie algebra element
 * @param[in] i0 block index (row and column) in sp where result is inserted
 *
 * @warning sp must be pre-allocated with the appropriate nonzeros and compressed
 *
 * @see d_exp_sparse_pattern() for a pre-allocated pattern.
 */
template<LieGroup G, bool Inv = false>
inline void
dr_exp_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0)
{
  using T = traits::lie_sparse<G>;
  assert(sp.isCompressed());
  assert(sp.rows() >= i0 + Dof<G>);
  assert(sp.cols() >= i0 + Dof<G>);
  if constexpr (IsCommutative<G>) {
    // identity matrix
    for (auto i = 0u; i < a.size(); ++i) { sp.coeffRef(i0 + i, i0 + i) = 1; }
  } else if constexpr (!Inv && requires { T::dr_exp_sparse(sp, a, i0); }) {
    // use specialized method if one exists
    T::dr_exp_sparse(sp, a, i0);
  } else if constexpr (Inv && requires { T::dr_expinv_sparse(sp, a, i0); }) {
    // use specialized method if one exists
    T::dr_expinv_sparse(sp, a, i0);
  } else {
    // fall back on dense method+pattern
    const TangentMap<G> D = [&] {
      if constexpr (Inv)
        return dr_expinv<G>(a);
      else
        return dr_exp<G>(a);
    }();
    for (auto i = 0u; i < Dof<G>; ++i) {
      for (Eigen::InnerIterator it(d_exp_sparse_pattern<G>, i); it; ++it) {
        sp.coeffRef(i0 + it.row(), i0 + it.col()) = D(it.row(), it.col());
      }
    }
  }
  assert(sp.isCompressed());
}

/**
 * @brief Sparse dr_expinv.
 *
 * @tparam G Lie group
 *
 * @param[in, out] sp allocated and compressed sparse matrix
 * @param[in] a Lie algebra element
 * @param[in] i0 block index (row and column) in sp where result is inserted
 *
 * @warning sp must be pre-allocated with the appropriate nonzeros and compressed
 *
 * @see d_exp_sparse_pattern() for a pre-allocated pattern.
 */
template<LieGroup G>
inline void
dr_expinv_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0)
{
  dr_exp_sparse<G, true>(sp, a, i0);
}

/**
 * @brief Sparsity pattern of d2r_exp_sparse(), d2r_expinv_sparse() (inline variable).
 */
template<LieGroup G>
inline Eigen::SparseMatrix<Scalar<G>> d2_exp_sparse_pattern = [] {
  Eigen::SparseMatrix<Scalar<G>> ret(Dof<G>, Dof<G> * Dof<G>);
  if constexpr (IsCommutative<G>) {
    // zero matrix--do nothing
  } else if constexpr (requires { traits::lie_sparse<G>::d2_exp_sparse_pattern; }) {
    // use spacialized method
    ret = traits::lie_sparse<G>::d2_exp_sparse_pattern;
  } else {
    // dense pattern
    ret = Eigen::Matrix<Scalar<G>, Dof<G>, Dof<G> * Dof<G>>::Ones().sparseView();
  }
  ret.makeCompressed();
  return ret;
}();

/**
 * @brief Sparse d2r_exp.
 *
 * @tparam G Lie group
 * @tparam Inv compute d2r_expinv
 *
 * @param[in, out] sp allocated and compressed sparse matrix
 * @param[in] a Lie algebra element
 * @param[in] i0 block index (row and column) in sp where result is inserted
 *
 * @warning sp must be pre-allocated with the appropriate nonzeros and compressed
 *
 * @see d2_exp_sparse_pattern() for a pre-allocated pattern.
 */
template<LieGroup G, bool Inv = false>
inline void
d2r_exp_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0)
{
  using T = traits::lie_sparse<G>;
  assert(sp.isCompressed());
  assert(sp.rows() >= i0 + Dof<G>);
  assert(sp.cols() >= sp.rows() * (i0 + Dof<G>));
  if constexpr (IsCommutative<G>) {
    // zero matrix--do nothing
  } else if constexpr (!Inv && requires { T::d2r_exp_sparse(sp, a, i0); }) {
    // use specialized method if one exists
    T::d2r_exp_sparse(sp, a, i0);
  } else if constexpr (Inv && requires { T::d2r_expinv_sparse(sp, a, i0); }) {
    // use specialized method if one exists
    T::d2r_expinv_sparse(sp, a, i0);
  } else {
    // fall back on dense method + pattern
    const Hessian<G> D = [&] {
      if constexpr (Inv)
        return d2r_expinv<G>(a);
      else
        return d2r_exp<G>(a);
    }();
    for (auto i = 0u; i < Dof<G> * Dof<G>; ++i) {
      for (Eigen::InnerIterator it(d2_exp_sparse_pattern<G>, i); it; ++it) {
        const auto block = i0 + (it.col() / Dof<G>);
        const auto row   = i0 + it.row();
        const auto col   = i0 + (it.col() % Dof<G>);

        sp.coeffRef(row, sp.rows() * block + col) = D(it.row(), it.col());
      }
    }
  }
  assert(sp.isCompressed());
}

/**
 * @brief Sparse d2r_expinv.
 *
 * @tparam G Lie group
 *
 * @param[in, out] sp allocated and compressed sparse matrix
 * @param[in] a Lie algebra element
 * @param[in] i0 block index (row and column) in sp where result is inserted
 *
 * @warning sp must be pre-allocated with the appropriate nonzeros and compressed
 *
 * @see d2_exp_sparse_pattern() for a pre-allocated pattern.
 */
template<LieGroup G>
inline void
d2r_expinv_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0)
{
  return d2r_exp_sparse<G, true>(sp, a, i0);
}

//////////////////////////////////////////////////////
//// SPARSITY INFORMATION FOR SPECIFIC LIE GROUPS ////
//////////////////////////////////////////////////////

namespace traits {

/// @brief Sparsity info for SE2 types.
template<typename G>
  requires(std::is_base_of_v<SE2Base<G>, G>)
struct lie_sparse<G>
{
  static inline Eigen::SparseMatrix<Scalar<G>> d_exp_sparse_pattern = [] {
    Eigen::SparseMatrix<Scalar<G>> ret(3, 3);
    for (auto i = 0u; i < 2; ++i) {
      for (auto j = 0u; j < 3; ++j) { ret.insert(i, j) = Scalar<G>(0); }
    }
    ret.insert(2, 2) = Scalar<G>(0);
    ret.makeCompressed();
    return ret;
  }();

  static inline Eigen::SparseMatrix<Scalar<G>> d2_exp_sparse_pattern = [] {
    Eigen::SparseMatrix<Scalar<G>> ret(3, 9);
    ret.insert(2, 0)     = 0;
    ret.insert(2, 1)     = 0;
    ret.insert(2, 2)     = 0;
    ret.insert(0, 2)     = 0;
    ret.insert(1, 2)     = 0;
    ret.middleCols(3, 3) = ret.leftCols(3);
    ret.makeCompressed();
    return ret;
  }();
};

/// @brief Sparsity info for SE3 types.
template<typename G>
  requires(std::is_base_of_v<SE3Base<G>, G>)
struct lie_sparse<G>
{
  static inline Eigen::SparseMatrix<Scalar<G>> d_exp_sparse_pattern = [] {
    Eigen::SparseMatrix<Scalar<G>> ret(6, 6);
    for (auto i = 0u; i < 6; ++i) {
      for (auto j = 0u; j < 6; ++j) {
        if (i < 3 || j >= 3) { ret.insert(i, j) = (i == j) ? Scalar<G>(1) : Scalar<G>(0); }
      }
    }
    ret.makeCompressed();
    return ret;
  }();

  static inline Eigen::SparseMatrix<Scalar<G>> d2_exp_sparse_pattern = [] {
    Eigen::SparseMatrix<Scalar<G>> ret(6, 36);
    for (auto i = 3u; i < 6u; ++i) {
      for (auto j = 0u; j < 18u; ++j) { ret.insert(i, j) = Scalar<G>(0); }
    }
    for (auto k = 0u; k < 3u; ++k) {
      for (auto i = 0u; i < 3u; ++i) {
        for (auto j = 0u; j < 3u; ++j) {
          ret.insert(i, 3 + 6 * k + j)          = Scalar<G>(0);
          ret.insert(3 + i, 18 + 3 + 6 * k + j) = Scalar<G>(0);
        }
      }
    }
    ret.makeCompressed();
    return ret;
  }();
};

/// @brief Sparsity info for Bundle types.
template<typename G>
  requires(std::is_base_of_v<BundleBase<G>, G>)
struct lie_sparse<G>
{
  /// @brief Sparsity pattern for dr_exp / dr_expinv
  static inline Eigen::SparseMatrix<Scalar<G>> d_exp_sparse_pattern = [] {
    Eigen::SparseMatrix<Scalar<G>> ret(Dof<G>, Dof<G>);
    utils::static_for<G::BundleSize>([&](auto I) {
      static constexpr auto Dof0 = G::template PartStart<I>;
      const auto & tmp           = ::smooth::d_exp_sparse_pattern<typename G::template PartType<I>>;
      for (auto i = 0u; i < tmp.outerSize(); ++i) {
        for (Eigen::InnerIterator it(tmp, i); it; ++it) {
          ret.insert(Dof0 + it.row(), Dof0 + it.col()) = it.value();
        }
      }
    });
    ret.makeCompressed();
    return ret;
  }();

  /// @brief Sparse calculation of dr_exp
  static void
  dr_exp_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0)
  {
    utils::static_for<G::BundleSize>([&](auto I) {
      static constexpr auto Dof0 = G::template PartStart<I>;
      ::smooth::dr_exp_sparse<typename G::template PartType<I>>(
        sp, a.template segment<G::template PartDof<I>>(Dof0), i0 + Dof0);
    });
  }

  /// @brief Sparse calculation of dr_expinv
  static void
  dr_expinv_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0)
  {
    utils::static_for<G::BundleSize>([&](auto I) {
      static constexpr auto Dof0 = G::template PartStart<I>;
      ::smooth::dr_expinv_sparse<typename G::template PartType<I>>(
        sp, a.template segment<G::template PartDof<I>>(Dof0), i0 + Dof0);
    });
  }

  /// @brief Sparsity pattern for d2r_exp / d2r_expinv
  static inline Eigen::SparseMatrix<Scalar<G>> d2_exp_sparse_pattern = [] {
    Eigen::SparseMatrix<Scalar<G>> ret(Dof<G>, Dof<G> * Dof<G>);
    utils::static_for<G::BundleSize>([&ret](auto I) {
      const auto & tmp    = ::smooth::d2_exp_sparse_pattern<typename G::template PartType<I>>;
      constexpr auto Dof0 = G::template PartStart<I>;
      for (auto i = 0u; i < tmp.outerSize(); ++i) {
        for (Eigen::InnerIterator it(tmp, i); it; ++it) {
          const auto block = Dof0 + (it.col() / G::template PartDof<I>);
          const auto row   = it.row();
          const auto col   = Dof0 + (it.col() % G::template PartDof<I>);

          ret.insert(Dof0 + row, Dof<G> * block + col) = it.value();
        }
      }
    });
    ret.makeCompressed();
    return ret;
  }();

  /// @brief Sparse calculation of d2r_exp
  static void
  d2r_exp_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0)
  {
    utils::static_for<G::BundleSize>([&](auto I) {
      static constexpr auto Dof0 = G::template PartStart<I>;
      ::smooth::d2r_exp_sparse<typename G::template PartType<I>>(
        sp, a.template segment<G::template PartDof<I>>(Dof0), i0 + Dof0);
    });
  }

  /// @brief Sparse calculation of d2r_expinv
  static void
  d2r_expinv_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0)
  {
    utils::static_for<G::BundleSize>([&](auto I) {
      static constexpr auto Dof0 = G::template PartStart<I>;
      ::smooth::d2r_expinv_sparse<typename G::template PartType<I>>(
        sp, a.template segment<G::template PartDof<I>>(Dof0), i0 + Dof0);
    });
  }
};

}  // namespace traits

}  // namespace smooth

#endif  // SMOOTH__LIE_GROUP_SPARSE_HPP_


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Aleksei Moskvin <alalmoskvin@gmail.com>
//
// 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 FILECOIN_STORAGE_PROOFS_CORE_MERKLE_TREE_STORAGE_UTILITIES_HPP
#define FILECOIN_STORAGE_PROOFS_CORE_MERKLE_TREE_STORAGE_UTILITIES_HPP

#include <boost/filesystem.hpp>

#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>

#include <cmath>
#include <sstream>
#include <utility>

namespace nil {
    namespace filecoin {
        namespace utilities {

            // Row_Count calculation given the number of leafs in the tree and the branches.
            size_t get_merkle_tree_row_count(size_t leafs, size_t branches) {
                // Optimization
                if (branches == 2) {
                    return leafs * branches;
                } else {
                    return std::log(leafs) / std::log(branches);
                }
            }

            // Tree length calculation given the number of leafs in the tree and the branches.
            size_t get_merkle_tree_len(size_t leafs, size_t branches) {
                BOOST_ASSERT_MSG(leafs >= branches, "leaf and branch mis-match");
                BOOST_ASSERT_MSG(branches == next_pow2(branches), "branches must be a power of 2");

                // Optimization
                if (branches == 2) {
                    BOOST_ASSERT_MSG(leafs == next_pow2(leafs), "leafs must be a power of 2");
                    return 2 * leafs - 1;
                }

                size_t len = leafs;
                size_t cur = leafs;
                size_t shift = log2_pow2(branches);
                if (shift == 0) {
                    return len;
                }

                while (cur > 0) {
                    cur >>= shift; // cur /= branches
                    BOOST_ASSERT_MSG(cur < leafs, "invalid input provided");
                    len += cur;
                }

                return len;
            }

            // Tree length calculation given the number of leafs in the tree, the
            // rows_to_discard, and the branches.
            size_t get_merkle_tree_cache_size(size_t leafs, size_t branches, size_t rows_to_discard) {
                size_t shift = log2_pow2(branches);
                size_t len = get_merkle_tree_len(leafs, branches)?;
                size_t row_count = get_merkle_tree_row_count(leafs, branches);

                BOOST_ASSERT_MSG(row_count - 1 > rows_to_discard,  "Cannot discard all rows except for the base");

                // 'row_count - 1' means that we start discarding rows above the base
                // layer, which is included in the current row_count.
                size_t cache_base = row_count - 1 - rows_to_discard;

                size_t cache_size = len;
                size_t cur_leafs = leafs;

                while (row_count > cache_base) {
                    cache_size -= cur_leafs;
                    cur_leafs >>= shift; // cur /= branches
                    row_count -= 1;
                }

                return cache_size;
            }

            bool is_merkle_tree_size_valid(size_t leafs, size_t branches) {
                if (branches == 0 || leafs != next_pow2(leafs) || branches != next_pow2(branches)) {
                    return false;
                }

                size_t mut cur = leafs;
                size_t shift = log2_pow2(branches);
                while (cur != 1) {
                    cur >>= shift; // cur /= branches
                    if (cur > leafs || cur == 0) {
                        return false;
                    }
                }

                return true;
            }

            // Given a tree of 'row_count' with the specified number of 'branches',
            // calculate the length of hashes required for the proof.
            size_t get_merkle_proof_lemma_len(size_t row_count, size_t branches) {
                return 2 + ((branches - 1) * (row_count - 1));
            }

            // This method returns the number of 'leafs' given a merkle tree
            // length of 'len', where leafs must be a power of 2, respecting the
            // number of branches.
            size_t get_merkle_tree_leafs(size_t len, size_t branches) {
                BOOST_ASSERT_MSG(branches == next_pow2(branches), "branches must be a power of 2");
                size_t leafs = 0;
                // Optimization:
                if (branches == 2) {
                    leafs = (len >> 1) + 1
                } else {
                    size_t leafs = 1;
                    size_t cur = len;
                    size_t shift = log2_pow2(branches);
                    while (cur != 1) {
                        leafs <<= shift; // leafs *= branches
                        BOOST_ASSERT_MSG(cur > leafs, "Invalid tree length provided for the specified arity");
                        cur -= leafs;
                        BOOST_ASSERT_MSG(cur < len, "Invalid tree length provided for the specified arity");
                    }
                };

                BOOST_ASSERT_MSG(leafs == next_pow2(leafs), "Invalid tree length provided for the specified arity");
                return leafs;
            }

            // returns next highest power of two from a given number if it is not
            // already a power of two.
            size_t next_pow2(size_t n) {
                return std::pow(2, std::ceil(std::log(n)));
            }

            // find power of 2 of a number which is power of 2
            size_t log2_pow2(size_t n) {
                return next_pow2(n);
            }
            //            struct ExternalReader {
            //                size_t offset;
            //                pub source: R;
            //
            //                size_t read(size_t start, size_t end, char* buf, source: &R) {
            //                    (self.read_fn)(start + self.offset, end + self.offset, buf, &self.source)
            //                }
            //
            //                ExternalReader(ReplicaConfig replica_config, size_t index) {
            //                    let reader = OpenOptions::new().read(true).open(&replica_config.path)?;
            //                    offset = replica_config.offsets[index];
            //                    source = reader;
            //                    self.offset = replica_config.offsets[index];
            //                    self.source = reades;
            //                    read_fn: |start, end, buf: &mut [u8], reader: &std::fs::File| {
            //                            reader.read_exact_at(start as u64, &mut buf[0..end - start])?;
            //
            //                            Ok(end - start)};
            //                }
            //            };

            enum StoreConfigDataVersion { One = 1, Two = 2 };

            const uint32_t DEFAULT_STORE_CONFIG_DATA_VERSION = StoreConfigDataVersion::Two;

            struct ReplicaConfig {
                ReplicaConfig(boost::filesystem::path path, const std::vector<size_t> &offsets) {
                    path = std::move(path);
                    for (auto i : offsets) {
                        offsets.push_back(i);
                    }
                }

                ReplicaConfig(boost::filesystem::path path) {
                    path = std::move(path);
                    offsets.push_back(0);
                }

                std::vector<size_t> offsets;
                boost::filesystem::path path;
            };

            struct StoreConfig {
                StoreConfig(boost::filesystem::path path, std::string id, size_t rows_to_discard) :
                    this->path(std::move(path)), this->id(std::move(id)), this->rows_to_discard(rows_to_discard) {};
                // If the tree is large enough to use the default value
                // (per-arity), use it.  If it's too small to cache anything
                // (i.e. not enough rows), don't discard any.
                static size_t default_rows_to_discard(size_t leafs, size_t branches) {
                    size_t row_count = get_merkle_tree_row_count(leafs, branches);
                    if (row_count <= 2) {
                        // If a tree only has a root row and/or base, there is
                        // nothing to discard.
                        return 0;
                    } else {
                        if (row_count == 3) {
                            // If a tree only has 1 row between the base and root,
                            // it's all that can be discarded.
                            return 1;
                        }
                    }
                    // row_count - 2 discounts the base layer (1) and root (1)
                    size_t max_rows_to_discard = row_count - 2;
                    // Discard at most 'constant value' rows (coded below,
                    // differing by arity) while respecting the max number that
                    // the tree can support discarding.
                    if (branches == 2)
                        return std::min(max_rows_to_discard, (size_t)7);
                    if (branches == 4)
                        return std::min(max_rows_to_discard, (size_t)5);
                    return std::min(max_rows_to_discard, (size_t)2);
                }
                // Deterministically create the data_path on-disk location from a
                // path and specified id.
                static boost::filesystem::path data_path(const boost::filesystem::path &path, const std::string &id) {
                    std::ostringstream store_data_version;
                    store_data_version << std::internal << std::setfill('0') << std::setw(2)
                                       << DEFAULT_STORE_CONFIG_DATA_VERSION;
                    return boost::filesystem::path("sc-" + store_data_version.str() + "-data-" + id + ".dat");
                }

                StoreConfig(const StoreConfig &config, const std::string &id, size_t size = 0) {
                    BOOST_ASSERT_MSG(size != 0, "Size must be positive");
                    this->size = config.size;
                    this->path = config.path;
                    this->id = config.id;
                    this->rows_to_discard = config.rows_to_discard;
                }
                /// A directory in which data (a merkle tree) can be persisted.
                boost::filesystem::path path;
                /// A unique identifier used to help specify the on-disk store
                /// location for this particular data.
                std::string id;
                /// The number of elements in the DiskStore.  This field is
                /// optional, and unused internally.
                size_t size;
                /// The number of merkle tree rows_to_discard then cache on disk.
                size_t rows_to_discard;
            };

            /// Backing store of the merkle tree.
            class Store {
                virtual void write(std::pair<uint8_t *, uint8_t *> el, size_t index) = 0;
                virtual void read(std::pair<size_t, size_t > read, uint8_t *buf)  = 0;
                // compact/shrink resources used where possible.
                virtual bool compact(size_t branches, StoreConfig config, uint32_t store_version) = 0;
                // re-instate resource usage where needed.
                virtual void reinit() {};
                virtual size_t len() = 0;
                virtual bool loaded_from_disk() = 0;
                virtual bool is_empty() = 0;
                virtual void pget_merkle_tree_lenush(std::pair<uint8_t *, uint8_t *> data) = 0;
                // Sync contents to disk (if it exists). This function is used to avoid
                // unnecessary flush calls at the cost of added code complexity.
                virtual void sync() = 0;
            };
        }     // namespace utilities
    }         // namespace filecoin
}             // namespace nil

#endif    // FILECOIN_STORAGE_PROOFS_CORE_MERKLE_TREE_STORAGE_UTILITIES_HPP


// Software License Agreement (BSD-3-Clause)
//
// Copyright 2018 The University of North Carolina at Chapel Hill
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. 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.
//
// 3. Neither the name of the copyright holder 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.

//! @author Jeff Ichnowski

#pragma once
#ifndef NIGH_IMPL_SO3_REGION_HPP
#define NIGH_IMPL_SO3_REGION_HPP

#include "region.hpp"
#include "so3.hpp"
#include "constants.hpp"
#include <Eigen/Dense>

namespace unc::robotics::nigh::impl {

    template <typename Key>
    class SO3Region {
        using Distance = typename metric::Space<Key, metric::SO3>::Distance;

        Eigen::Matrix<Distance, 2, 3> min_;
        Eigen::Matrix<Distance, 2, 3> max_;

    public:
        SO3Region() = delete;
        SO3Region(const SO3Region& region)
            : min_(region.min_)
            , max_(region.max_)
        {
        }

        SO3Region(const Key& q, int vol) {
            for (int a=0 ; a<3 ; ++a)
                min_.col(a) = so3::project(q, vol, a);
            max_ = min_;
        }

        void grow(const Key& q, int vol) {
            for (int a=0 ; a<3 ; ++a) {
                Eigen::Matrix<Distance, 2, 1> c = so3::project(q, vol, a);
                assert(-SQRT1_2<Distance> <= c[0] && c[0] <= SQRT1_2<Distance>);
                assert(c[1] >= SQRT1_2<Distance>);
                if (c[0] < min_(0, a))
                    min_.col(a) = c;
                if (c[0] > max_(0, a))
                    max_.col(a) = c;
            }
        }

        Distance selectAxis(unsigned *axis, int vol) const {
            assert(vol != -1);
            Eigen::Matrix<Distance, 1, 3> dots = min_.cwiseProduct(max_).colwise().sum();
            assert((dots.array() >= Distance(0)).all());
            return std::acos(dots.minCoeff(axis));
        }
    };
}

#endif