//------------------------------------------------------------------------------
/// \file
/// \ingroup interpolate_detail
/// \copyright (C) Copyright Aquaveo 2018. Distributed under FreeBSD License
/// (See accompanying file LICENSE or https://aqaveo.com/bsd/license.txt)
//------------------------------------------------------------------------------

//----- Included files ---------------------------------------------------------

// 1. Precompiled header

// 2. My header
#include <xmsinterp/interpolate/detail/InterpNatNeigh.h>

// 3. Standard Library Headers

// 4. External Library Headers
#pragma warning(push)
#pragma warning(disable : 4512) // boost code: no assignment operator
#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/unordered_map.hpp>
#pragma warning(pop)

// 5. Shared Headers
#include <xmscore/math/math.h>
#include <xmscore/misc/XmError.h>
#include <xmscore/stl/utility.h>
#include <xmsgrid/geometry/geoms.h>
#include <xmsgrid/geometry/GmTriSearch.h>
#include <xmsinterp/interpolate/detail/NodalFunc.h>
#include <xmscore/misc/xmstype.h>

// 6. Non-shared Headers

//----- Forward declarations ---------------------------------------------------

//----- External globals -------------------------------------------------------

//----- Namespace declaration --------------------------------------------------
namespace xms
{
//----- Constants / Enumerations -----------------------------------------------

//----- Classes / Structs ------------------------------------------------------
/// \brief convenience struct for natural neighbor interpolation
struct nnOuterEdgeStruct
{
public:
  nnOuterEdgeStruct() { m_triIdx = m_edge[0] = m_edge[1] = XM_NONE; }
  int m_triIdx;   ///< index of this triangle
  int m_edge[2];  ///< pts that make up edge in CC order
  xms::Pt3d m_cc; ///< circumcenter of triangle made by interp pt with edge
};
//----- Internal functions -----------------------------------------------------

//----- Class / Function definitions -------------------------------------------
class InterpNatNeighImpl : public InterpNatNeigh
{
public:
  InterpNatNeighImpl(const std::vector<xms::Pt3d>& a_pts,
                     const std::vector<int>& a_tris,
                     const std::vector<float>& a_scalar,
                     GmTriSearch* a_triSearch);

  virtual float InterpToPt(const xms::Pt3d& a_pt) override;
  virtual void GetNeighbors(int a_ptIdx, std::vector<int>& a_neigh) override;
  virtual void SetNodalFunc(BSHP<NodalFunc> a_) override;
  virtual void RecalcNodalFunc() override;
  virtual void SetBlendWeights(bool a_) override;
  virtual std::string ToString() override;

  //
  void FillEdgeMap();
  void FillCenterVec();
  void EdgesFromTri(int a_triIdx, std::pair<int, int> a_edges[3]);
  void NeighTriFromTriIdx(int a_triIdx, std::vector<int>& a_tris);
  void GetNatNeighTriangles(const xms::Pt3d& a_pt, std::vector<int>& a_tris);
  double BlendFunc(double a_x);
  double ScalarFromNodalFunc(int a_ptIdx, const xms::Pt3d& a_loc);

  double HaleNnInterp(const xms::Pt3d& a_pt);
  void HaleNnVisitNeighbors(int a_tIdx,
                            const xms::Pt3d& a_pt,
                            std::vector<nnOuterEdgeStruct>& a_outerEdges,
                            std::map<int, double>& a_weights);
  int AdjacentTriangle(std::pair<int, int>& a_edge, int a_triIdx);
  bool PtInTriCircumCircle(const xms::Pt3d& a_pt, int a_tri);
  void HaleNnAddOuterEdge(std::vector<nnOuterEdgeStruct>& a_,
                          int a_tIdx,
                          int a_ptIdx0,
                          int a_ptIdx1,
                          const xms::Pt3d& a_pt);
  void HaleNnSortOuterEdges(std::vector<nnOuterEdgeStruct>& a_);
  void HaleNnOuterEdgesToWeights(std::vector<nnOuterEdgeStruct>& a_outerEdges,
                                 std::map<int, double>& a_weights);
  void NormalizeWeights(std::map<int, double>& a_weights);
  void BlendWeights(std::map<int, double>& a_weights);
  double ScalarFromWeights(std::map<int, double>& a_weights, const xms::Pt3d& a_pt);

private:
  /// typedef for long template
  typedef boost::unordered_map<std::pair<int, int>, std::pair<int, int>> MapEdges;

  const std::vector<xms::Pt3d>& m_pts; ///< points interpolating from
  const std::vector<int>& m_tris;      ///< triangles interpolating from
  const std::vector<float>& m_scalar;  ///< scalar interpolating from
  double m_tol;                        ///< tolerance for geometric floating point comparisons
  MapEdges m_edges;                    ///< map of triangle edges
  std::vector<xms::Pt3d> m_centers;    ///< circumcircle centers of triangles
  BSHP<NodalFunc> m_nf;                ///< Nodal function (constant, gradient plane, quadratic)
  GmTriSearch* m_triSearch;            ///< Spatial index for searching triangles
  bool m_blendWeights;                 ///< flag for blending interpolation weights
};

namespace
{
//------------------------------------------------------------------------------
/// \brief
//------------------------------------------------------------------------------
static std::pair<int, int> iMakePair(int a_first, int a_second)
{
  if (a_first < a_second)
    return std::make_pair(a_first, a_second);
  return std::make_pair(a_second, a_first);
} // iMakePair
} // unnamed namespace
//------------------------------------------------------------------------------
/// \brief Creates a Natural Neighbor Interpolation class
/// \param[in] a_pts Locations that are interpolated from
/// \param[in] a_tris Triangles connecting a_pts and defining neighbors
/// \param[in] a_scalar Scalars that are interpolated from
/// \param[in] a_triSearch Spatial index for searching triangles
/// \return A shared pointer to an instance of this class.
//------------------------------------------------------------------------------
BSHP<InterpNatNeigh> InterpNatNeigh::New(const std::vector<xms::Pt3d>& a_pts,
                                         const std::vector<int>& a_tris,
                                         const std::vector<float>& a_scalar,
                                         GmTriSearch* a_triSearch)
{
  BSHP<InterpNatNeighImpl> r(new InterpNatNeighImpl(a_pts, a_tris, a_scalar, a_triSearch));
  return r;
} // InterpNatNeigh::InterpNatNeigh
//------------------------------------------------------------------------------
/// \brief Constructor
//------------------------------------------------------------------------------
InterpNatNeigh::InterpNatNeigh()
{
} // InterpNatNeigh::InterpNatNeigh
//------------------------------------------------------------------------------
/// \brief Destructor
//------------------------------------------------------------------------------
InterpNatNeigh::~InterpNatNeigh()
{
} // InterpNatNeigh::~InterpNatNeigh

////////////////////////////////////////////////////////////////////////////////
/// \class InterpNatNeighImpl
/// \brief Performs natural neighbor interpolation to a location.
////////////////////////////////////////////////////////////////////////////////
//------------------------------------------------------------------------------
/// \brief Consructor
/// \param[in] a_pts Locations that are interpolated from
/// \param[in] a_tris Triangles connecting a_pts and defining neighbors
/// \param[in] a_scalar Scalars that are interpolated from
/// \param[in] a_triSearch Spatial index for searching triangles
//------------------------------------------------------------------------------
InterpNatNeighImpl::InterpNatNeighImpl(const std::vector<xms::Pt3d>& a_pts,
                                       const std::vector<int>& a_tris,
                                       const std::vector<float>& a_scalar,
                                       GmTriSearch* a_triSearch)
: m_pts(a_pts)
, m_tris(a_tris)
, m_scalar(a_scalar)
, m_tol(1e-9)
, m_centers()
, m_nf()
, m_triSearch(a_triSearch)
, m_blendWeights(false)
{
  FillEdgeMap();
  FillCenterVec();
} // InterpNatNeighImpl::~InterpNatNeighImpl
//------------------------------------------------------------------------------
/// \brief Interpolates to a_pt
/// \param[in] a_pt The location to interpolate to
/// \return The interpolated value at a_pt
//------------------------------------------------------------------------------
float InterpNatNeighImpl::InterpToPt(const xms::Pt3d& a_pt)
{
  return (float)(HaleNnInterp(a_pt));
} // InterpNatNeighImpl::InterpToPt
//------------------------------------------------------------------------------
/// \brief Finds neighboring triangles for the pt at a_ptIdx index in m_pts
/// \param[in] a_ptIdx Index to the point where we want to find its neighbor
/// triangles
/// \param[out] a_neigh Vector of triangle indexes
//------------------------------------------------------------------------------
void InterpNatNeighImpl::GetNeighbors(int a_ptIdx, std::vector<int>& a_neigh)
{
  const xms::Pt3d& p(m_pts[a_ptIdx]);
  a_neigh.resize(0);
  std::vector<int> tris;
  GetNatNeighTriangles(p, tris);

  std::set<int> ptIdxs;
  // now check to see if the point is inside the cirumcircle
  for (size_t i = 0; i < tris.size(); ++i)
  {
    for (int j = 0; j < 3; ++j)
      ptIdxs.insert(m_tris[tris[i] + j]);
  }
  ptIdxs.erase(a_ptIdx);
  a_neigh.assign(ptIdxs.begin(), ptIdxs.end());
} // InterpNatNeighImpl::GetNeighbors
//------------------------------------------------------------------------------
/// \brief Set the precomputed nodal function
/// \param a_ The nodal function class.
//------------------------------------------------------------------------------
void InterpNatNeighImpl::SetNodalFunc(BSHP<NodalFunc> a_)
{
  m_nf = a_;
} // InterpNatNeighImpl::SetNodalFunc
//------------------------------------------------------------------------------
/// \brief Recalculates the nodal function. This happens when the scalars change
//------------------------------------------------------------------------------
void InterpNatNeighImpl::RecalcNodalFunc()
{
  if (m_nf)
    m_nf->ComputeNodalFuncs();
} // InterpNatNeighImpl::RecalcNodalFunc
//------------------------------------------------------------------------------
/// \brief Turns on a flag for using a blending function when calculating weights
/// \param[in] a_ flag to turn on weight blending
//------------------------------------------------------------------------------
void InterpNatNeighImpl::SetBlendWeights(bool a_)
{
  m_blendWeights = a_;
} // InterpNatNeighImpl::SetBlendWeights
//------------------------------------------------------------------------------
/// \brief serializes the class to a string
/// \return The string.
//------------------------------------------------------------------------------
std::string InterpNatNeighImpl::ToString()
{
  std::stringstream ss;
  VecToStream(ss, m_pts, "pts");
  VecToStream(ss, m_tris, "tris");
  VecToStream(ss, m_scalar, "scalar");
  ss << m_tol << "=tol ";
  // typedef boost::unordered_map<std::pair<int, int>, std::pair<int, int>> MapEdges;
  for (MapEdges::iterator it = m_edges.begin(); it != m_edges.end(); ++it)
  {
    ss << it->first.first << " " << it->first.second << " " << it->second.first << " "
       << it->second.second << " ";
  }
  ss << "=edges ";
  VecToStream(ss, m_centers, "centers");
  ss << m_blendWeights << "=blendWeights ";
  return ss.str();
} // InterpNatNeighImpl::ToString
//------------------------------------------------------------------------------
/// \brief Creates a map of edges for natural neighbor calculations
//------------------------------------------------------------------------------
void InterpNatNeighImpl::FillEdgeMap()
{
  if (m_tris.empty())
    return;
#if _DEBUG
  std::pair<int, int> pr;
#endif
  for (size_t i = 0; i < m_tris.size(); i += 3)
  {
    int k = (int)i;
    std::pair<int, int> tp[3];
    EdgesFromTri((int)i, tp);
    for (int j = 0; j < 3; ++j)
    {
      if (m_edges.find(tp[j]) == m_edges.end())
      {
        m_edges[tp[j]].first = k;
        m_edges[tp[j]].second = XM_NONE;
      }
      else
        m_edges[tp[j]].second = k;
#if _DEBUG
      pr = m_edges[tp[j]];
#endif
    }
  }
} // InterpNatNeighImpl::FillEdgeMap
//------------------------------------------------------------------------------
/// \brief calculate the circumcircle centers for all triangles
//------------------------------------------------------------------------------
void InterpNatNeighImpl::FillCenterVec()
{
  if (m_pts.empty() || m_tris.empty())
    return;

  xms::Pt3d pt, ptMin, ptMax;
  ptMin = ptMax = m_pts[0];
  for (size_t i = 1; i < m_pts.size(); ++i)
  {
    ptMin.x = Mmin(m_pts[i].x, ptMin.x);
    ptMin.y = Mmin(m_pts[i].y, ptMin.y);
    ptMax.x = Mmax(m_pts[i].x, ptMax.x);
    ptMax.y = Mmax(m_pts[i].y, ptMax.y);
  }
  m_tol = gmXyDistance(ptMin, ptMax) / 1e9;
  m_centers.assign(m_tris.size() / 3, xms::Pt3d());
  for (size_t i = 0, cnt = 0; i < m_tris.size(); i += 3, ++cnt)
  {
    const xms::Pt3d &p0(m_pts[m_tris[i]]), &p1(m_pts[m_tris[i + 1]]), &p2(m_pts[m_tris[i + 2]]);
    // calculate the center (x,y) and radius squared (z) of the circumcenter
    // of the triangle
    gmCircumcircleWithTol(&p0, &p1, &p2, &pt.x, &pt.y, &pt.z, m_tol);
    m_centers[cnt] = pt;
  }
} // InterpNatNeighImpl::FillCenterVec
//------------------------------------------------------------------------------
/// \brief fill an array of edges from a triangle index
/// \param[in] a_triIdx Index of the triangle
/// \param[out] a_edges Array of triangle edges filled by this method
//------------------------------------------------------------------------------
void InterpNatNeighImpl::EdgesFromTri(int a_triIdx, std::pair<int, int> a_edges[3])
{
  a_edges[0] = iMakePair(m_tris[a_triIdx], m_tris[a_triIdx + 1]);
  a_edges[1] = iMakePair(m_tris[a_triIdx + 1], m_tris[a_triIdx + 2]);
  a_edges[2] = iMakePair(m_tris[a_triIdx + 2], m_tris[a_triIdx]);
} // InterpNatNeighImpl::EdgesFromTri
//------------------------------------------------------------------------------
/// \brief Gets the adjacent triangles to a_triIdx
/// \param[in] a_triIdx Index of the triangle
/// \param[out] a_tris Indexes of adjacent triangles
//------------------------------------------------------------------------------
void InterpNatNeighImpl::NeighTriFromTriIdx(int a_triIdx, std::vector<int>& a_tris)
{
  a_tris.resize(0);
  std::pair<int, int> edges[3];
  EdgesFromTri(a_triIdx, edges);
  std::set<int> setTris;
  for (int i = 0; i < 3; ++i)
  {
    const std::pair<int, int>& pTri = m_edges[edges[i]];
    setTris.insert(pTri.first);
    if (XM_NONE != pTri.second)
      setTris.insert(pTri.second);
  }
  setTris.erase(a_triIdx);
  a_tris.assign(setTris.begin(), setTris.end());
} // InterpNatNeighImpl::NeighTriFromTriIdx
//------------------------------------------------------------------------------
/// \brief  Given a pt this will find the "natural neighbor" triangles. First,
/// we find the triangle that the pt is in. Then we visit the neighbor
/// triangles to see if the point is inside of its circumcircle. If it is then
/// we will also visit the neighbors of that triangle and so on.
/// \param[in] a_pt The location to find the natural neighbor triangles
/// \param[out] a_tris The triangle indexes that are natural neighbors to a_pt
//------------------------------------------------------------------------------
void InterpNatNeighImpl::GetNatNeighTriangles(const xms::Pt3d& a_pt, std::vector<int>& a_tris)
{
  a_tris.resize(0);
  if (!m_triSearch)
    return;
  int tIdx = m_triSearch->TriContainingPt(a_pt);
  if (XM_NONE == tIdx)
    return;

  a_tris.push_back(tIdx);
  std::vector<int> visited(m_tris.size(), 0);
  visited[tIdx] = 1;
  for (size_t j = 0; j < a_tris.size(); ++j)
  {
    tIdx = a_tris[j];
    std::vector<int> vals;
    NeighTriFromTriIdx(tIdx, vals);
    xms::Pt3d center;
    double rSquared, distSquared;
    std::set<int> ptIdxs;
    // now check to see if the point is inside the circumcircle
    for (size_t i = 0; i < vals.size(); ++i)
    {
      size_t tri = vals[i];
      if (visited[tri])
        continue;
      visited[tri] = 1;
      distSquared = gmXyDistanceSquared(m_centers[tri / 3], a_pt);
      rSquared = m_centers[tri / 3].z;
      if (distSquared <= rSquared)
        a_tris.push_back((int)tri);
    }
  }
} // InterpNatNeighImpl::GetNatNeighTriangles
//------------------------------------------------------------------------------
/// \brief Blending function to smooth interpolation weights refactored from GMS
/// \param x interpolation weight
/// \return The blended value.
//------------------------------------------------------------------------------
double InterpNatNeighImpl::BlendFunc(double x)
{
  return ((-2.0 * x * x * x) + (3.0 * x * x));
} // InterpNatNeighImpl::BlendFunc
//------------------------------------------------------------------------------
/// \brief calculates the scalar using the nodal function
/// \param[in] a_ptIdx Index to point in m_pts
/// \param[in] a_loc Location being interpolated to
/// \return the scalar adjusted by the nodal function
//------------------------------------------------------------------------------
double InterpNatNeighImpl::ScalarFromNodalFunc(int a_ptIdx, const xms::Pt3d& a_loc)
{
  double rval((double)m_scalar[a_ptIdx]);
  // get value from the nodal function
  if (m_nf)
    rval = m_nf->ScalarFromPtIdx(a_ptIdx, a_loc);
  return rval;
} // InterpNatNeighImpl::ScalarFromNodalFunc
//------------------------------------------------------------------------------
/// \brief Implementation of natural neighbor interpolation taken from this
/// paper
/// A stable and fast implementation of natural neighbor interpolation
/// Luming Liang & Dave Hale
/// Center for Wave Phenomena, Colorado School of Mines,Golden, CO 80401, USA
/// April 20, 2010
/// \param[in] a_pt The location being interpolated to
/// \return The interpolated value at a_pt
//------------------------------------------------------------------------------
double InterpNatNeighImpl::HaleNnInterp(const xms::Pt3d& a_pt)
{
  double rval(0);
  if (!m_triSearch)
    return rval;
  int tIdx = m_triSearch->TriContainingPt(a_pt);
  if (XM_NONE == tIdx)
    return rval;

  std::map<int, double> weights;
  std::vector<nnOuterEdgeStruct> outerEdges;
  // search through all neighbors
  HaleNnVisitNeighbors(tIdx, a_pt, outerEdges, weights);
  // sort outer edges into a loop
  HaleNnSortOuterEdges(outerEdges);
  // process outer edges
  HaleNnOuterEdgesToWeights(outerEdges, weights);

  NormalizeWeights(weights);
  if (m_blendWeights)
    BlendWeights(weights);
  rval = ScalarFromWeights(weights, a_pt);
  return rval;
} // InterpNatNeighImpl::HaleNnInterp
//------------------------------------------------------------------------------
/// \brief Visit the neighbors to a_pt and calculate the interpolation weight
/// for the natural neighbors
/// \param[in] a_tIdx Starting triangle index
/// \param[in] a_pt Location used to find natural neighbors
/// \param[out] a_outerEdges Outer edges for a_pt
/// \param[out] a_weights Interpolation weight for neighbor triangle
//------------------------------------------------------------------------------
void InterpNatNeighImpl::HaleNnVisitNeighbors(int a_tIdx,
                                              const xms::Pt3d& a_pt,
                                              std::vector<nnOuterEdgeStruct>& a_outerEdges,
                                              std::map<int, double>& a_weights)
{
  std::set<std::pair<int, int>> processedEdges;
  std::vector<int> tris(1, a_tIdx);
  for (size_t t = 0; t < tris.size(); ++t)
  { // visit adjacent triangles
    std::pair<int, int> edge;
    int tIdx = tris[t];
    for (int i = 0; i < 3; ++i)
    {
      int ptIdx = m_tris[tIdx + i];
      int nextTriPtIdx = i == 2 ? 0 : i + 1;
      int ptIdx1 = m_tris[tIdx + nextTriPtIdx];
      edge = iMakePair(ptIdx, ptIdx1);
      // already processed this edge
      if (processedEdges.find(edge) != processedEdges.end())
        continue;

      // see if there is an adjacent triangle and if it is a natural neighbor
      int adjTri = AdjacentTriangle(edge, tIdx);
      if (XM_NONE == adjTri || !PtInTriCircumCircle(a_pt, adjTri))
      { // no adjtri or not a natural neighbor. This is an outer edge.
        HaleNnAddOuterEdge(a_outerEdges, tIdx, ptIdx, ptIdx1, a_pt);
        continue;
      }

      // adjacent triangle is a natural neighbor. Calculate area contribution.
      // cross the circumcenters of the 2 triangles.
      const xms::Pt3d &tCC(m_centers[tIdx / 3]), adjCC(m_centers[adjTri / 3]);
      double cross = (tCC.x * adjCC.y) - (tCC.y * adjCC.x);
      a_weights[ptIdx] -= cross;
      a_weights[ptIdx1] += cross;

      tris.push_back(adjTri);
      // add to processed edges
      processedEdges.insert(edge);
    }
  }
} // InterpNatNeighImpl::HaleNnVisitNeighbors
//------------------------------------------------------------------------------
/// \brief Gets the triangle adjacent to a_edge
/// \param[in] a_edge A triangle edge
/// \param[in] a_triIdx Triangle index
/// \return the index to the adjacent triangle. If there is no adjacent triangle
/// then the return value is XM_NONE.
//------------------------------------------------------------------------------
int InterpNatNeighImpl::AdjacentTriangle(std::pair<int, int>& a_edge, int a_triIdx)
{
  int rval = XM_NONE;
  boost::unordered_map<std::pair<int, int>, std::pair<int, int>>::iterator it(m_edges.find(a_edge));
  if (it != m_edges.end())
  {
    rval = it->second.first;
    if (a_triIdx == rval)
      rval = it->second.second;
  }
  return rval;
} // InterpNatNeighImpl::AdjacentTriangle
//------------------------------------------------------------------------------
/// \brief Determines if a point is inside of a triangle circumcircle
/// \param[in] a_pt The location to test
/// \param[in] a_tri Index to the triangle
/// \return true if the point is inside the circumcircle
//------------------------------------------------------------------------------
bool InterpNatNeighImpl::PtInTriCircumCircle(const xms::Pt3d& a_pt, int a_tri)
{
  const xms::Pt3d& cc = m_centers[a_tri / 3];
  double d2 = gmXyDistanceSquared(a_pt, cc);
  if (d2 < cc.z)
    return true;
  return false;
} // InterpNatNeighImpl::PtInTriCircumCircle
//------------------------------------------------------------------------------
/// \brief Adds a nnOuterEdgeStruct to a vector
/// \param[out] a_ Vector of outer edges
/// \param[in] a_tIdx Triangle index
/// \param[in] a_ptIdx0 Point index for first point on edge of triangle a_tIdx
/// \param[in] a_ptIdx1 Point index for second point on edge of triangle a_tIdx
/// \param[in] a_pt Location used to find this outer edge
//------------------------------------------------------------------------------
void InterpNatNeighImpl::HaleNnAddOuterEdge(std::vector<nnOuterEdgeStruct>& a_,
                                            int a_tIdx,
                                            int a_ptIdx0,
                                            int a_ptIdx1,
                                            const xms::Pt3d& a_pt)
{
  nnOuterEdgeStruct n;
  n.m_triIdx = a_tIdx;
  n.m_edge[0] = a_ptIdx0;
  n.m_edge[1] = a_ptIdx1;
  gmCircumcircleWithTol(&m_pts[a_ptIdx0], &m_pts[a_ptIdx1], &a_pt, &n.m_cc.x, &n.m_cc.y, &n.m_cc.z,
                        m_tol);
  a_.push_back(n);
} // InterpNatNeighImpl::HaleNnAddOuterEdge
//------------------------------------------------------------------------------
/// \brief Sorts the outer edges
/// \param[out] a_ Vector of outer edges
//------------------------------------------------------------------------------
void InterpNatNeighImpl::HaleNnSortOuterEdges(std::vector<nnOuterEdgeStruct>& a_)
{
  std::vector<nnOuterEdgeStruct> e(a_);
  for (size_t i = 1; i < a_.size(); ++i)
  {
    for (size_t j = 1; j < e.size(); ++j)
    {
      if (e[j].m_edge[0] == a_[i - 1].m_edge[1])
        a_[i] = e[j];
    }
  }
} // InterpNatNeighImpl::HaleNnSortOuterEdges
//------------------------------------------------------------------------------
/// \brief Calculates weights from the outer edges
/// \param[in] a_outerEdges The outer edges
/// \param[out] a_weights Weight calculated for each outer edge
//------------------------------------------------------------------------------
void InterpNatNeighImpl::HaleNnOuterEdgesToWeights(std::vector<nnOuterEdgeStruct>& a_outerEdges,
                                                   std::map<int, double>& a_weights)
{
  for (size_t i = 0; i < a_outerEdges.size(); ++i)
  { // get the current edge and the next edge
    nnOuterEdgeStruct& curr(a_outerEdges[i]);
    size_t nextIdx = i + 1 == a_outerEdges.size() ? 0 : i + 1;
    nnOuterEdgeStruct& next(a_outerEdges[nextIdx]);

    int ptIdx = curr.m_edge[1];
    // cross the original tri cc with the tri cc formed by curr edge and the
    // interpolation point
    {
      xms::Pt3d &o1(m_centers[curr.m_triIdx / 3]), &c1(curr.m_cc);
      double cross = (o1.x * c1.y) - (o1.y * c1.x);
      a_weights[ptIdx] += cross;
      a_weights[curr.m_edge[0]] -= cross;
    }

    // cross tri cc formed by curr edge and interp pt with tri cc formed by
    // next edge and interp pt
    {
      xms::Pt3d &o1(curr.m_cc), &c1(next.m_cc);
      double cross = (o1.x * c1.y) - (o1.y * c1.x);
      a_weights[ptIdx] += cross;
    }
  }
} // InterpNatNeighImpl::HaleNnOuterEdgesToWeights
//------------------------------------------------------------------------------
/// \brief Normalizes the weights so that they sum to 1.0
/// \param[out] a_weights The interpolation weights
//------------------------------------------------------------------------------
void InterpNatNeighImpl::NormalizeWeights(std::map<int, double>& a_weights)
{
  double sum(0);
  std::map<int, double>::iterator it(a_weights.begin());
  for (; it != a_weights.end(); ++it)
    sum += it->second;
  for (it = a_weights.begin(); it != a_weights.end(); ++it)
    it->second /= sum;
} // InterpNatNeighImpl::NormalizeWeights
//------------------------------------------------------------------------------
/// \brief Blends the weights using BlendFunc. Smooths the interpolation.
/// \param[out] a_weights The interpolation weights
//------------------------------------------------------------------------------
void InterpNatNeighImpl::BlendWeights(std::map<int, double>& a_weights)
{
  double sum(0);
  std::map<int, double>::iterator it(a_weights.begin());
  for (; it != a_weights.end(); ++it)
  {
    it->second = BlendFunc(it->second);
    sum += it->second;
  }
  for (it = a_weights.begin(); it != a_weights.end(); ++it)
    it->second /= sum;
} // InterpNatNeighImpl::NormalizeWeights
//------------------------------------------------------------------------------
/// \brief Computes a scalar at a_pt based on the interpolation weights
/// \param[in] a_weights The interpolation weights
/// \param[in] a_pt The location interpolated to.
/// \return The interpolated value at a_pt.
//------------------------------------------------------------------------------
double InterpNatNeighImpl::ScalarFromWeights(std::map<int, double>& a_weights,
                                             const xms::Pt3d& a_pt)
{
  double rval(0);
  std::map<int, double>::iterator it(a_weights.begin());
  for (; it != a_weights.end(); ++it)
  {
    rval += it->second * ScalarFromNodalFunc(it->first, a_pt);
  }
  return rval;
} // InterpNatNeighImpl::NormalizeWeights

} // namespace xms

#ifdef CXX_TEST
////////////////////////////////////////////////////////////////////////////////

#include <xmsinterp/interpolate/detail/InterpNatNeigh.t.h>

#include <xmscore/testing/TestTools.h>

namespace
{
static void iGetPtsTris(std::vector<xms::Pt3d>& a_pts,
                        std::vector<int>& a_tris,
                        std::vector<float>& a_scalar)
{
  a_pts = {{26, 74, 5},  {15, 31, 8},  {60, -3, 4}, {78, 78, 7},
           {56, 64, 10}, {75, 45, 11}, {43, 22, 2}};
  a_tris = {0, 4, 3, 0, 1, 4, 4, 5, 3, 4, 6, 5, 4, 1, 6, 1, 2, 6, 2, 5, 6};
  a_scalar = {5.0f, 8.0f, 4.0f, 7.0f, 10.0f, 11.0f, 2.0f};
} // iGetPtsTris
} // unnamed namespace

// namespace xms {
using namespace xms;
////////////////////////////////////////////////////////////////////////////////
/// \class InterpNatNeighUnitTests
/// \brief Tester for the InterpNatNeigh class
////////////////////////////////////////////////////////////////////////////////
//------------------------------------------------------------------------------
/// \brief testing class creation
//------------------------------------------------------------------------------
void InterpNatNeighUnitTests::testCreateClass()
{
  std::vector<xms::Pt3d> pts;
  std::vector<int> tris;
  std::vector<float> scalar;
  BSHP<InterpNatNeigh> p = InterpNatNeigh::New(pts, tris, scalar, nullptr);
} // InterpNatNeighUnitTests::testCreateClass
//------------------------------------------------------------------------------
/// \brief testing getting the natural neighbor triangles
//------------------------------------------------------------------------------
void InterpNatNeighUnitTests::testGetTris()
{
  xms::Pt3d loc(46, 27, 0);
  BSHP<std::vector<xms::Pt3d>> pts(new std::vector<xms::Pt3d>());
  BSHP<std::vector<int>> tris(new std::vector<int>());
  std::vector<int> tIdxs;
  std::vector<float> scalar;
  iGetPtsTris(*pts, *tris, scalar);
  BSHP<GmTriSearch> ts = GmTriSearch::New();
  ts->TrisToSearch(pts, tris);
  InterpNatNeighImpl p(*pts, *tris, scalar, ts.get());
  p.GetNatNeighTriangles(loc, tIdxs);
  std::vector<int> nBase = {9, 12, 18, 3};
  TS_ASSERT_EQUALS_VEC(nBase, tIdxs);
} // InterpNatNeighUnitTests::testGetTris
//------------------------------------------------------------------------------
/// \brief testing getting the natural neighbor triangles
//------------------------------------------------------------------------------
void InterpNatNeighUnitTests::testGetNeighbors()
{
  BSHP<std::vector<xms::Pt3d>> pts(new std::vector<xms::Pt3d>());
  BSHP<std::vector<int>> tris(new std::vector<int>());
  std::vector<int> tIdxs;
  std::vector<float> scalar;
  iGetPtsTris(*pts, *tris, scalar);
  BSHP<GmTriSearch> ts = GmTriSearch::New();
  ts->TrisToSearch(pts, tris);
  InterpNatNeighImpl p(*pts, *tris, scalar, ts.get());
  p.GetNeighbors(6, tIdxs);
  std::vector<int> nBase = {1, 2, 4, 5};
  TS_ASSERT_EQUALS_VEC(nBase, tIdxs);
} // InterpNatNeighUnitTests::testGetNeighbors
//------------------------------------------------------------------------------
/// \brief testing sorting outer edges for the Hale NN method
//------------------------------------------------------------------------------
void InterpNatNeighUnitTests::testHaleNnSortOuterEdges()
{
  std::vector<nnOuterEdgeStruct> edges(5);
  edges[0].m_edge[0] = 5;
  edges[0].m_edge[1] = 1;
  edges[1].m_edge[0] = 3;
  edges[1].m_edge[1] = 4;
  edges[2].m_edge[0] = 2;
  edges[2].m_edge[1] = 3;
  edges[3].m_edge[0] = 4;
  edges[3].m_edge[1] = 5;
  edges[4].m_edge[0] = 1;
  edges[4].m_edge[1] = 2;
  BSHP<std::vector<xms::Pt3d>> pts(new std::vector<xms::Pt3d>());
  BSHP<std::vector<int>> tris(new std::vector<int>());
  std::vector<int> tIdxs;
  std::vector<float> scalar;
  iGetPtsTris(*pts, *tris, scalar);
  BSHP<GmTriSearch> ts = GmTriSearch::New();
  ts->TrisToSearch(pts, tris);
  InterpNatNeighImpl p(*pts, *tris, scalar, ts.get());
  p.HaleNnSortOuterEdges(edges);
  int base[5][2] = {{5, 1}, {1, 2}, {2, 3}, {3, 4}, {4, 5}};
  for (size_t i = 0; i < edges.size(); ++i)
  {
    TS_ASSERT_EQUALS(base[i][0], edges[i].m_edge[0]);
  }
} // InterpNatNeighUnitTests::testHaleNnSortOuterEdges
//------------------------------------------------------------------------------
/// \brief testing the Hale NN interpolation
//------------------------------------------------------------------------------
void InterpNatNeighUnitTests::testHaleNnInterp()
{
  BSHP<std::vector<xms::Pt3d>> pts(new std::vector<xms::Pt3d>());
  BSHP<std::vector<int>> tris(new std::vector<int>());
  std::vector<int> tIdxs;
  std::vector<float> scalar;
  iGetPtsTris(*pts, *tris, scalar);
  BSHP<GmTriSearch> ts = GmTriSearch::New();
  ts->TrisToSearch(pts, tris);
  InterpNatNeighImpl p(*pts, *tris, scalar, ts.get());
  std::vector<xms::Pt3d> iPts = {
    {36, 42, 0}, {39, 17, 0}, {64, 27, 0}, {55, 41, 0},
    {72, 64, 0}, {52, 73, 0}, {33, 60, 0}, {65.9404706, 54.0595293, 0}};

  std::vector<double> iVals(iPts.size(), 0);
  for (size_t i = 0; i < iPts.size(); ++i)
    iVals[i] = p.HaleNnInterp(iPts[i]);
  std::vector<double> nBase = {6.0669168950546712, 4.6727605118829985, 7.1629642928188586,
                               6.9812628044431957, 8.9941520467836238, 6.9374999999999991,
                               6.6593131032057444, 10.058329876949339};
  TS_ASSERT_DELTA_VEC(nBase, iVals, 1e-10);
} // InterpNatNeighUnitTests::testHaleNnInterp

//} // namespace xms
#endif // CXX_TEST


#include "Intersectable.hpp"
#include "Light.hpp"
#include "MaterialLoader.hpp"
#include "SceneLoader.hpp"
#include "Util/ReinhardToneMapping.hpp"
#include "Util/Window.hpp"

#include <boost/program_options.hpp>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>

#include <chrono>
#include <iostream>
#include <random>

glm::mat4 generatePixelstoNDC(unsigned width, unsigned height) {
    return glm::ortho(-0.5f, width - 0.5f, height - 0.5f, -0.5f);
}

namespace {
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_real_distribution dis(-0.5f, 0.5f);
}  // namespace

Ray generateRay(unsigned x, unsigned y, unsigned num_bounces, glm::mat4 unproj, glm::vec3 origin) {
    glm::vec4 world = unproj * glm::vec4{x + dis(gen), y + dis(gen), 0.0f, 1.0f};
    glm::vec3 direction = glm::normalize(glm::vec3{world / world.w} - origin);
    return Ray(origin, direction, num_bounces);
}

std::vector<Ray> generateScreenRays(unsigned width, unsigned height, glm::mat4 unproj, glm::vec3 origin) {
    std::vector<Ray> rays;
    rays.reserve(width * height);
    unproj = unproj * generatePixelstoNDC(width, height);
    for (unsigned y = 0; y < height; y++) {
        for (unsigned x = 0; x < width; x++) {
            rays.push_back(generateRay(x, y, 3, unproj, origin));
        }
    }
    return rays;
}

template <ExecutionPolicy P>
void accumulateColors(P policy, const Scene& scene, unsigned width, unsigned height, unsigned num_samples, glm::mat4 proj_view_inv, std::vector<glm::vec3>& accumulated_colors) {
    auto rays = generateScreenRays(width, height, proj_view_inv, scene.camera.position);
    std::transform(
        policy, rays.begin(), rays.end(), accumulated_colors.begin(), accumulated_colors.begin(),
        [&](Ray r, glm::vec3 color) {
            for (unsigned i = 0; i < num_samples; i++) {
                color += sceneIntersectColor(scene, r);
            }
            return color;
        });
}

glm::mat4 getProjViewInv(const Window& window, const Scene& scene) {
    glm::mat4 proj = windowPerspective(window, scene.eps);
    glm::mat4 view = cameraView(scene.camera);
    return glm::inverse(proj * view);
}

template <ExecutionPolicy P>
auto runBenchmark(P policy, const Window& window, const Scene& scene) {
    auto [width, height] = window.dimensions();
    glm::mat4 proj_view_inv = getProjViewInv(window, scene);
    std::vector<glm::vec3> colors{width * height};

    auto start = std::chrono::steady_clock::now();
    accumulateColors(policy, scene, width, height, window.samples, proj_view_inv, colors);
    auto end = std::chrono::steady_clock::now();

    return std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
}

template <ExecutionPolicy P>
void renderFrame(P policy, Window& window, const Scene& scene, std::vector<glm::vec3>& accumulated_colors, unsigned& accumulated_samples) {
    auto [width, height] = window.dimensions();
    glm::mat4 proj_view_inv = getProjViewInv(window, scene);

    accumulateColors(policy, scene, width, height, window.samples, proj_view_inv, accumulated_colors);
    accumulated_samples += window.samples;
    auto pixels = std::views::transform(
        accumulated_colors,
        [&](glm::vec3 pixel) { return pixel / float(accumulated_samples); });
    auto tone_mapped_pixels = reinhardToneMapPixels(policy, pixels.begin(), pixels.end());
    window.setPixels(tone_mapped_pixels.begin(), tone_mapped_pixels.end());
    window.update();
}

template <ExecutionPolicy P>
void runRender(P policy, Window& window, const Scene& scene) {
    auto [width, height] = window.dimensions();
    std::vector<glm::vec3> accumulated_colors{width * height, glm::vec3{0.0f}};
    unsigned accumulated_samples = 0;

    while (!window.shouldClose()) {
        renderFrame(policy, window, scene, accumulated_colors, accumulated_samples);
    }
}

int main(int argc, const char* argv[]) {
    namespace bpo = boost::program_options;

    unsigned width = 1280;
    unsigned height = 720;
    unsigned pixel_samples = 16;
    std::string scene_name = "Default";

    bpo::options_description options;
    // clang-format off
    options.add_options()
        ("help", "show this message")
        ("benchmark", "render a single frame and measure the time it takes")
        ("width,w", bpo::value(&width), "set window width")
        ("height,h", bpo::value(&height), "set window height")
        ("samples,s", bpo::value(&pixel_samples), "the number of samples per pixel")
    ;
    // clang-format on

    bpo::variables_map vm;
    bpo::store(bpo::parse_command_line(argc, argv, options), vm);
    bpo::notify(vm);

    if (vm.contains("help")) {
        std::cout << "Usage: " << argv[0] << " [options]:\n";
        std::cout << options << "\n";
        return 0;
    }

    bool benchmark = vm.contains("benchmark");

    NamedScene scene = loadScene("");

    Window window(width, height);
    window.samples = pixel_samples;
    window.hfov = 45.0f;
    window.setTitle(scene.name);

    auto policy = std::execution::par_unseq;

    if (benchmark) {
        window.hide();
        auto render_time = runBenchmark(policy, window, scene.scene);
        std::cout << "Rendered scene \"" << scene.name << "\" "
                  << "(" << width << "x" << height << ", " << pixel_samples << " samples) "
                  << "in " << render_time << " milliseconds\n";
    } else {
        runRender(policy, window, scene.scene);
    }
}


#include <unordered_set>
#include <numeric>
#include <iostream>
#include <cassert>

#include <boost/timer.hpp>

#include "Conditioning.h"
#include "ConditionedFactorType.h"
#include "FactorGraphObservation.h"
#include "FactorGraphStructurizer.h"
#include "FactorGraphPartialObservation.h"
#include "VAcyclicDecomposition.h"
#include "RandomSource.h"
#include "MaximumCompositeLikelihood.h"

namespace Grante {

MaximumCompositeLikelihood::MaximumCompositeLikelihood(
	FactorGraphModel* fg_model, int decomp)
	: ParameterEstimationMethod(fg_model), decomp(decomp), mle(fg_model) {
	assert(decomp >= -1);
}

MaximumCompositeLikelihood::~MaximumCompositeLikelihood() {
	assert(comp_training_data.size() == comp_inference_methods.size());
	for (unsigned int cn = 0; cn < comp_training_data.size(); ++cn) {
		delete (comp_training_data[cn].first);
		delete (comp_training_data[cn].second);
		delete (comp_inference_methods[cn]);
	}
}

void MaximumCompositeLikelihood::SetOptimizationMethod(
	MaximumLikelihood::MLEOptimizationMethod opt_method) {
	mle.SetOptimizationMethod(opt_method);
}

MaximumLikelihood::MLEProblem* MaximumCompositeLikelihood::GetLearnProblem() {
	return (mle.GetLearnProblem());
}

void MaximumCompositeLikelihood::SetupTrainingData(
	const std::vector<labeled_instance_type>& training_data,
	const std::vector<InferenceMethod*> inference_methods) {
	assert(comp_training_data.size() == 0);
	assert(comp_inference_methods.size() == 0);
	assert(inference_methods.size() == training_data.size());

	// Number of times each component will be covered
	unsigned int cover_count = 1;
	assert(decomp >= -1);
	if (decomp == DecomposePseudolikelihood) {
		cover_count = 1;
	} else if (decomp > 0) {
		cover_count = decomp;
	}

	// Produce composite factor graphs
	boost::timer decomp_timer;
	int training_data_size = static_cast<int>(training_data.size());
	fg_cc_var_label.resize(cover_count * training_data_size);
	fg_cc_count.resize(cover_count * training_data_size);
	fg_orig_index.resize(cover_count * training_data_size);
	std::fill(fg_cc_count.begin(), fg_cc_count.end(), 0);
	unsigned int cn = 0;
	for (int n = 0; n < training_data_size; ++n) {
		FactorGraph* fg = training_data[n].first;
		size_t var_count = fg->Cardinalities().size();

		// Get observation
		const FactorGraphObservation* obs = training_data[n].second;

		// Obtain one or more decomposition(s)
		for (unsigned int cover_iter = 0; cover_iter < cover_count;
			++cover_iter) {
			VAcyclicDecomposition vac(fg);
			std::vector<bool> factor_is_removed;

			if (decomp == DecomposePseudolikelihood) {
				factor_is_removed.resize(fg->Factors().size());
				std::fill(factor_is_removed.begin(),
					factor_is_removed.end(), true);
			} else {
				std::vector<double> factor_weight(fg->Factors().size(), 0.0);
				if (decomp == DecomposeUniform) {
					// Use constant weights
					std::fill(factor_weight.begin(), factor_weight.end(), 1.0);
				} else {
					// Use uniform random weights
					boost::uniform_real<double> uniform_dist(0.0, 1.0);
					boost::variate_generator<boost::mt19937&,
						boost::uniform_real<double> >
						rgen(RandomSource::GlobalRandomSampler(), uniform_dist);

					for (unsigned int fi = 0; fi < factor_weight.size(); ++fi)
						factor_weight[fi] = rgen();
				}
				vac.ComputeDecompositionSP(factor_weight, factor_is_removed);
			}

			// Shatter factor graph into trees
			fg_cc_count[cn] += FactorGraphStructurizer::ConnectedComponents(
				fg, factor_is_removed, fg_cc_var_label[cn]);
#if 0
			std::cout << "MCL, instance " << n << " decomposed into " << cc_count
				<< " components" << std::endl;
#endif

			// Add each component as separate factor graph
			for (unsigned int ci = 0; ci < fg_cc_count[cn]; ++ci) {
				std::vector<unsigned int> cond_var_set;
				cond_var_set.reserve(var_count);

				// Add all variables not in this component to the conditioning set
				for (size_t vi = 0; vi < var_count; ++vi) {
					if (fg_cc_var_label[cn][vi] != ci)
						cond_var_set.push_back(static_cast<unsigned int>(vi));
				}
				AddTrainingComponentCond(fg, obs, inference_methods[n],
					cond_var_set);
			}
			fg_orig_index[cn] = n;
			cn += 1;
		}
	}
	std::cout << "MCL, decomposed " << training_data.size() << " instances "
		<< "into " << comp_training_data.size() << " instances "
		<< (decomp == DecomposeUniform ? "(uniform)" : "(randomized)")
		<< " in " << decomp_timer.elapsed() << "s." << std::endl;

	// Initialize MLE training data from created components
	SetupMLETrainingData();
}

void MaximumCompositeLikelihood::SetupMLETrainingData() {
	mle.SetupTrainingData(comp_training_data, comp_inference_methods);
}

void MaximumCompositeLikelihood::AddTrainingComponentUncond(
	const FactorGraph* fg, const FactorGraphObservation* obs,
	InferenceMethod* inference_method,
	const std::vector<unsigned int>& uncond_var_set) {
	// Produce conditioning variable set A = V\B, where B is uncond_var_set.
	std::vector<unsigned int> cond_var_set;
	size_t var_count = fg->Cardinalities().size();
	cond_var_set.reserve(var_count);

	// Add all variables not in this component to the conditioning set
	unsigned int uncond_var_count = 0;
	std::unordered_set<unsigned int> uncond_var_set_h(
		uncond_var_set.begin(), uncond_var_set.end());
	for (size_t vi = 0; vi < var_count; ++vi) {
		if (uncond_var_set_h.count(static_cast<unsigned int>(vi)) > 0) {
			uncond_var_count += 1;
			continue;
		}
		cond_var_set.push_back(static_cast<unsigned int>(vi));
	}

	// Add component based on conditioning variable set
	AddTrainingComponentCond(fg, obs, inference_method, cond_var_set);
}

void MaximumCompositeLikelihood::AddTrainingComponentCond(
	const FactorGraph* fg, const FactorGraphObservation* obs,
	InferenceMethod* inference_method,
	const std::vector<unsigned int>& cond_var_set) {
	// Create partial observation from full observation
	FactorGraphPartialObservation* pobs =
		CreatePartialObservationCond(fg, obs, cond_var_set);

	// Condition
	std::vector<unsigned int> var_new_to_orig;
	std::vector<unsigned int> fac_new_to_orig;
	FactorGraph* fg_cond = Conditioning::ConditionFactorGraph(
		&ftab, fg, pobs, var_new_to_orig, fac_new_to_orig);
	delete (pobs);

	FactorGraphObservation* new_obs =
		CreatePartialObservationUncond(fg, fg_cond, obs,
			var_new_to_orig, fac_new_to_orig);

	// Build derived composite training set
	comp_training_data.push_back(labeled_instance_type(fg_cond, new_obs));
	assert(inference_method != 0);
	comp_inference_methods.push_back(inference_method->Produce(fg_cond));
}

void MaximumCompositeLikelihood::UpdateTrainingComponentCond(
	const FactorGraph* fg, const FactorGraphObservation* obs,
	const std::vector<unsigned int>& cond_var_set,
	unsigned int cti) {
	assert(cti < comp_training_data.size());

	// Create partial observation from full observation
	FactorGraphPartialObservation* pobs =
		CreatePartialObservationCond(fg, obs, cond_var_set);

	// Obtain variable and factor maps
	std::vector<unsigned int> var_new_to_orig;
	std::vector<unsigned int> fac_new_to_orig;
	FactorGraph* fg_cond = Conditioning::ConditionFactorGraph(
		&ftab, fg, pobs, var_new_to_orig, fac_new_to_orig);
	delete (pobs);

	FactorGraphObservation* new_obs =
		CreatePartialObservationUncond(fg, fg_cond, obs,
			var_new_to_orig, fac_new_to_orig);
	delete (fg_cond);

	// Update observation
	assert(comp_training_data[cti].second->Type() == new_obs->Type());
	if (new_obs->Type() == FactorGraphObservation::DiscreteLabelingType) {
		assert(comp_training_data[cti].second->State().size() ==
			new_obs->State().size());
	} else {
		assert(comp_training_data[cti].second->Expectation().size() ==
			new_obs->Expectation().size());
	}
	delete (comp_training_data[cti].second);
	comp_training_data[cti].second = new_obs;
}

// This method's responsibility is to create the information the subgraph is
// conditioned on.  For discrete observations this is simply the conditioning
// variable state adjacent to cross-factors.  For expectation observations
// this is the factor expectation with the unconditioned variables
// marginalized out.
FactorGraphPartialObservation*
MaximumCompositeLikelihood::CreatePartialObservationCond(
	const FactorGraph* fg, const FactorGraphObservation* obs,
	const std::vector<unsigned int>& cond_var_set) const {
	if (obs->Type() == FactorGraphObservation::DiscreteLabelingType) {
		size_t cond_var_count = cond_var_set.size();
		std::vector<unsigned int> cond_var_state;
		cond_var_state.reserve(cond_var_count);

		// Add all variables not in this component to the conditioning set
		const std::vector<unsigned int>& obs_state = obs->State();
		for (size_t cvi = 0; cvi < cond_var_count; ++cvi) {
			// Condition
			unsigned int vi = cond_var_set[cvi];
			cond_var_state.push_back(obs_state[vi]);
		}

		// Create conditioning observation
		return (new FactorGraphPartialObservation(cond_var_set,
			cond_var_state));
	} else {
		assert(obs->Type() == FactorGraphObservation::ExpectationType);
		std::vector<unsigned int> fac_subset;
		std::vector<std::vector<double> > obs_e;
		const std::vector<std::vector<double> >& expect = obs->Expectation();
		const std::vector<Factor*>& factors = fg->Factors();
		std::unordered_set<unsigned int> cond_var_set_u(
			cond_var_set.begin(), cond_var_set.end());

		// Sum out the unconditioned variable elements.
		//
		// For discrete observations we can identify the conditioning
		// distribution uniquely.  For expectation observations we can not.
		// But as the expectation is assumed to be realizable by the model
		// (ideally a product distribution), we sum out the unconditioned
		// variables to obtain the per-site conditioning expectations.
		for (unsigned int fi = 0; fi < factors.size(); ++fi) {
			// Check whether this factor is a cross factor
			const std::vector<unsigned int>& fac_vars =
				factors[fi]->Variables();
			const std::vector<unsigned int>& fac_card =
				factors[fi]->Cardinalities();

			bool has_cond = false;
			bool has_uncond = false;
			std::vector<unsigned int> cond_fvar;
			unsigned int cond_card = 1;
			for (unsigned int fvi = 0; fvi < fac_vars.size(); ++fvi) {
				if (cond_var_set_u.count(fac_vars[fvi]) > 0) {
					has_cond = true;
					cond_fvar.push_back(fvi);
					cond_card *= fac_card[fvi];
				} else {
					has_uncond = true;
				}
			}
			if (has_cond == false || has_uncond == false)
				continue;

			// This factor is a cross-factor
			fac_subset.push_back(fi);
			const std::vector<double>& m_e = expect[fi];
			std::vector<double> obs_e_fi(cond_card, 0.0);
			for (unsigned int oei = 0; oei < m_e.size(); ++oei) {
				// oei: index in original factor expectations,
				// cei: index into conditioning expectations.
				unsigned int cei =
					FactorConditioningTable::IndexMapConditioned(
						factors[fi]->Type(), cond_fvar, oei);
				assert(cei < obs_e_fi.size());

				// Marginalize out unconditioned variables
				obs_e_fi[cei] += m_e[oei];
			}
			assert(std::fabs(std::accumulate(obs_e_fi.begin(),
				obs_e_fi.end(), 0.0) - 1.0) <= 1.0e-8);
			obs_e.push_back(obs_e_fi);
		}
		return (new FactorGraphPartialObservation(cond_var_set,
			fac_subset, obs_e));
	}
	assert(0);
	return (0);
}

// This method's responsibility is to create the new target ground truth for
// the subgraphs.  For discrete observations this is the variable subset
// covered by the graph, whereas for expectation observations this are the
// factor observations with the conditioned-on variables marginalized out.
FactorGraphObservation*
MaximumCompositeLikelihood::CreatePartialObservationUncond(
	const FactorGraph* fg, const FactorGraph* fg_cond,
	const FactorGraphObservation* obs,
	const std::vector<unsigned int>& var_new_to_orig,
	const std::vector<unsigned int>& fac_new_to_orig) const {
	if (obs->Type() == FactorGraphObservation::DiscreteLabelingType) {
		// Create matching observation
		size_t uncond_var_count = var_new_to_orig.size();
		const std::vector<unsigned int>& obs_state = obs->State();
		std::vector<unsigned int> new_obs_state(uncond_var_count);
		for (size_t nvi = 0; nvi < uncond_var_count; ++nvi) {
			assert(nvi < var_new_to_orig.size());
			assert(var_new_to_orig[nvi] < obs_state.size());
			new_obs_state[nvi] = obs_state[var_new_to_orig[nvi]];
		}
		return (new FactorGraphObservation(new_obs_state));
	} else {
		assert(obs->Type() == FactorGraphObservation::ExpectationType);
		size_t new_fac_count = fac_new_to_orig.size();

		// For each conditioned factor
		const std::vector<std::vector<double> >& expect = obs->Expectation();
		const std::vector<Factor*>& factors = fg->Factors();
		const std::vector<Factor*>& cond_factors = fg_cond->Factors();
		std::vector<std::vector<double> > obs_e;
		for (size_t nfi = 0; nfi < new_fac_count; ++nfi) {
			const ConditionedFactorType* cft =
				dynamic_cast<const ConditionedFactorType*>(
					cond_factors[nfi]->Type());
			assert(cft != 0);
			const std::vector<unsigned int>& cond_fvar =
				cft->ConditionedVariableIndices();

			// Create unconditioned expectations
			std::vector<double> obs_e_fi(cft->ProdCardinalities(), 0.0);
			unsigned int ofi = fac_new_to_orig[nfi];
			const std::vector<double>& m_e = expect[ofi];
			if (cond_fvar.empty()) {
				// Fully unconditioned factor: copy ground truth expectations
				assert(m_e.size() == obs_e_fi.size());
				std::copy(m_e.begin(), m_e.end(), obs_e_fi.begin());
			} else {
				// Conditioned factor
				for (unsigned int oei = 0; oei < m_e.size(); ++oei) {
					// oei: index in original factor expectations,
					// nei: index in remaining unconditioned variable
					//    'ground-truth' expectations.
					unsigned int nei =
						FactorConditioningTable::IndexMapUnconditioned(
							factors[ofi]->Type(), cond_fvar, oei);
					assert(nei < obs_e_fi.size());

					// Marginalize out conditioned variables
					obs_e_fi[nei] += m_e[oei];
				}
			}
			assert(std::fabs(std::accumulate(obs_e_fi.begin(),
				obs_e_fi.end(), 0.0) - 1.0) <= 1.0e-8);
			obs_e.push_back(obs_e_fi);
		}
		return (new FactorGraphObservation(obs_e));
	}
	assert(0);
	return (0);
}

void MaximumCompositeLikelihood::AddPrior(const std::string& factor_type,
	Prior* prior) {
	mle.AddPrior(factor_type, prior);
}

double MaximumCompositeLikelihood::Train(double conv_tol, unsigned int max_iter)
{
	return (mle.Train(conv_tol, max_iter));
}

void MaximumCompositeLikelihood::UpdateTrainingLabeling(
	const std::vector<labeled_instance_type>& training_update) {
	assert(fg_orig_index.size() == comp_training_data.size());

	// For all decomposed components
	for (unsigned int cn = 0; cn < fg_orig_index.size(); ++cn) {
		// Original factor graph index
		unsigned int n = fg_orig_index[cn];
		assert(n < training_update.size());

		FactorGraph* fg = training_update[n].first;
		const FactorGraphObservation* obs = training_update[n].second;
		size_t var_count = fg->Cardinalities().size();

		// Update each component of the current decomposition
		for (unsigned int ci = 0; ci < fg_cc_count[cn]; ++ci) {
			std::vector<unsigned int> cond_var_set;
			cond_var_set.reserve(var_count);

			// Add all variables not in this component to the conditioning set
			for (size_t vi = 0; vi < var_count; ++vi) {
				if (fg_cc_var_label[cn][vi] != ci)
					cond_var_set.push_back(static_cast<unsigned int>(vi));
			}
			UpdateTrainingComponentCond(fg, obs, cond_var_set, cn);
		}
	}

	// Update fully observed components
	mle.UpdateTrainingLabeling(comp_training_data);
}

}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    LevenbergMarquardtOptimizer.cpp
 * @brief   A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme
 * @author  Richard Roberts
 * @author  Frank Dellaert
 * @author  Luca Carlone
 * @date    Feb 26, 2012
 */

#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/internal/LevenbergMarquardtState.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/timing.h>

#include <boost/format.hpp>
#include <boost/optional.hpp>
#include <boost/range/adaptor/map.hpp>

#include <cmath>
#include <fstream>
#include <iostream>
#include <limits>
#include <string>

using namespace std;

namespace gtsam {

using boost::adaptors::map_values;
typedef internal::LevenbergMarquardtState State;

/* ************************************************************************* */
LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph,
                                                         const Values& initialValues,
                                                         const LevenbergMarquardtParams& params)
    : NonlinearOptimizer(
          graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues),
                                                  params.lambdaInitial, params.lambdaFactor))),
      params_(LevenbergMarquardtParams::EnsureHasOrdering(params, graph)) {}

LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph,
                                                         const Values& initialValues,
                                                         const Ordering& ordering,
                                                         const LevenbergMarquardtParams& params)
    : NonlinearOptimizer(
          graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues),
                                                  params.lambdaInitial, params.lambdaFactor))),
      params_(LevenbergMarquardtParams::ReplaceOrdering(params, ordering)) {}

/* ************************************************************************* */
void LevenbergMarquardtOptimizer::initTime() {
  startTime_ = boost::posix_time::microsec_clock::universal_time();
}

/* ************************************************************************* */
double LevenbergMarquardtOptimizer::lambda() const {
  auto currentState = static_cast<const State*>(state_.get());
  return currentState->lambda;
}

/* ************************************************************************* */
int LevenbergMarquardtOptimizer::getInnerIterations() const {
  auto currentState = static_cast<const State*>(state_.get());
  return currentState->totalNumberInnerIterations;
}

/* ************************************************************************* */
GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const {
  return graph_.linearize(state_->values);
}

/* ************************************************************************* */
GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
    const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal) const {
  gttic(damp);
  auto currentState = static_cast<const State*>(state_.get());

  if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED)
    std::cout << "building damped system with lambda " << currentState->lambda << std::endl;

  if (params_.diagonalDamping)
    return currentState->buildDampedSystem(linear, sqrtHessianDiagonal);
  else
    return currentState->buildDampedSystem(linear);
}

/* ************************************************************************* */
// Log current error/lambda to file
inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){
  auto currentState = static_cast<const State*>(state_.get());

  if (!params_.logFile.empty()) {
    ofstream os(params_.logFile.c_str(), ios::app);
    boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time();
    os << /*inner iterations*/ currentState->totalNumberInnerIterations << ","
        << 1e-6 * (currentTime - startTime_).total_microseconds() << ","
        << /*current error*/ currentError << "," << currentState->lambda << ","
        << /*outer iterations*/ currentState->iterations << endl;
  }
}

/* ************************************************************************* */
bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
                                            const VectorValues& sqrtHessianDiagonal) {
  auto currentState = static_cast<const State*>(state_.get());
  bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA);

#ifdef GTSAM_USING_NEW_BOOST_TIMERS
  boost::timer::cpu_timer lamda_iteration_timer;
  lamda_iteration_timer.start();
#else
  boost::timer lamda_iteration_timer;
  lamda_iteration_timer.restart();
#endif

  if (verbose)
    cout << "trying lambda = " << currentState->lambda << endl;

  // Build damped system for this lambda (adds prior factors that make it like gradient descent)
  auto dampedSystem = buildDampedSystem(linear, sqrtHessianDiagonal);

  // Try solving
  double modelFidelity = 0.0;
  bool step_is_successful = false;
  bool stopSearchingLambda = false;
  double newError = numeric_limits<double>::infinity(), costChange;
  Values newValues;
  VectorValues delta;

  bool systemSolvedSuccessfully;
  try {
    // ============ Solve is where most computation happens !! =================
    delta = solve(dampedSystem, params_);
    systemSolvedSuccessfully = true;
  } catch (const IndeterminantLinearSystemException&) {
    systemSolvedSuccessfully = false;
  }

  if (systemSolvedSuccessfully) {
    if (verbose)
      cout << "linear delta norm = " << delta.norm() << endl;
    if (params_.verbosityLM >= LevenbergMarquardtParams::TRYDELTA)
      delta.print("delta");

    // cost change in the linearized system (old - new)
    double newlinearizedError = linear.error(delta);

    double linearizedCostChange = currentState->error - newlinearizedError;
    if (verbose)
      cout << "newlinearizedError = " << newlinearizedError
           << "  linearizedCostChange = " << linearizedCostChange << endl;

    if (linearizedCostChange >= 0) {  // step is valid
      // update values
      gttic(retract);
      // ============ This is where the solution is updated ====================
      newValues = currentState->values.retract(delta);
      // =======================================================================
      gttoc(retract);

      // compute new error
      gttic(compute_error);
      if (verbose)
        cout << "calculating error:" << endl;
      newError = graph_.error(newValues);
      gttoc(compute_error);

      if (verbose)
        cout << "old error (" << currentState->error << ") new (tentative) error (" << newError
             << ")" << endl;

      // cost change in the original, nonlinear system (old - new)
      costChange = currentState->error - newError;

      if (linearizedCostChange >
          1e-20) {  // the (linear) error has to decrease to satisfy this condition
        // fidelity of linearized model VS original system between
        modelFidelity = costChange / linearizedCostChange;
        // if we decrease the error in the nonlinear system and modelFidelity is above threshold
        step_is_successful = modelFidelity > params_.minModelFidelity;
        if (verbose)
          cout << "modelFidelity: " << modelFidelity << endl;
      }  // else we consider the step non successful and we either increase lambda or stop if error
         // change is small

      double minAbsoluteTolerance = params_.relativeErrorTol * currentState->error;
      // if the change is small we terminate
      if (std::abs(costChange) < minAbsoluteTolerance) {
        if (verbose)
          cout << "abs(costChange)=" << std::abs(costChange)
               << "  minAbsoluteTolerance=" << minAbsoluteTolerance
               << " (relativeErrorTol=" << params_.relativeErrorTol << ")" << endl;
        stopSearchingLambda = true;
      }
    }
  } // if (systemSolvedSuccessfully)

  if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) {
// do timing
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
    double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall;
#else
    double iterationTime = lamda_iteration_timer.elapsed();
#endif
    if (currentState->iterations == 0)
      cout << "iter      cost      cost_change    lambda  success iter_time" << endl;

    cout << boost::format("% 4d % 8e   % 3.2e   % 3.2e  % 4d   % 3.2e") % currentState->iterations %
                newError % costChange % currentState->lambda % systemSolvedSuccessfully %
                iterationTime << endl;
  }

  if (step_is_successful) {
    // we have successfully decreased the cost and we have good modelFidelity
    // NOTE(frank): As we return immediately after this, we move the newValues
    // TODO(frank): make Values actually support move. Does not seem to happen now.
    state_ = currentState->decreaseLambda(params_, modelFidelity, std::move(newValues), newError);
    return true;
  } else if (!stopSearchingLambda) {  // we failed to solved the system or had no decrease in cost
    if (verbose)
      cout << "increasing lambda" << endl;
    State* modifiedState = static_cast<State*>(state_.get());
    modifiedState->increaseLambda(params_); // TODO(frank): make this functional with Values move

    // check if lambda is too big
    if (modifiedState->lambda >= params_.lambdaUpperBound) {
      if (params_.verbosity >= NonlinearOptimizerParams::TERMINATION ||
          params_.verbosityLM == LevenbergMarquardtParams::SUMMARY)
        cout << "Warning:  Levenberg-Marquardt giving up because "
                "cannot decrease error with maximum lambda" << endl;
      return true;
    } else {
      return false;  // only case where we will keep trying
    }
  } else {  // the change in the cost is very small and it is not worth trying bigger lambdas
    if (verbose)
      cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl;
    return true;
  }
}

/* ************************************************************************* */
GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::iterate() {
  auto currentState = static_cast<const State*>(state_.get());

  gttic(LM_iterate);

  // Linearize graph
  if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED)
    cout << "linearizing = " << endl;
  GaussianFactorGraph::shared_ptr linear = linearize();

  if(currentState->totalNumberInnerIterations==0) { // write initial error
    writeLogFile(currentState->error);

    if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) {
      cout << "Initial error: " << currentState->error
           << ", values: " << currentState->values.size() << std::endl;
    }
  }

  // Only calculate diagonal of Hessian (expensive) once per outer iteration, if we need it
  VectorValues sqrtHessianDiagonal;
  if (params_.diagonalDamping) {
    sqrtHessianDiagonal = linear->hessianDiagonal();
    for (Vector& v : sqrtHessianDiagonal | map_values) {
      v = v.cwiseMax(params_.minDiagonal).cwiseMin(params_.maxDiagonal).cwiseSqrt();
    }
  }

  // Keep increasing lambda until we make make progress
  while (!tryLambda(*linear, sqrtHessianDiagonal)) {
    auto newState = static_cast<const State*>(state_.get());
    writeLogFile(newState->error);
  }

  return linear;
}

} /* namespace gtsam */


/*
 * kaskadeBridge.hh
 *
 *  Created on: 06.12.2013
 *      Author: Lars Lubkoll, Anton Schiela
 */

#ifndef KASKADE_BRIDGE_HH_
#define KASKADE_BRIDGE_HH_

#include <boost/signals2.hpp>
#include <boost/bind.hpp>

#include "algorithm/abstract_interface.hh"
#include "algorithm/dune_bridge.hh"
#include "algorithm/newton_bridge.hh"

namespace Kaskade
{
  template <class Value>
  void assignIfNegative(Value& val, Value newVal)
  {
    if(val < 0) val = newVal;
  }

  namespace Bridge
  {
    template <class Linearization>
    class ConnectedLinearization : public Linearization, public AbstractFlushConnection
    {
    public:
      template <typename... Args>
      ConnectedLinearization(const Args&... args) : Linearization(args...)
      {}

      virtual ~ConnectedLinearization() { changed(); flushconn.disconnect(); }

      virtual void flush() { changed(); Linearization::flush(); }

      virtual void connectToSignalForFlush(boost::signals2::signal<void ()>& sig)
      {
        if(flushconn.connected()) flushconn.disconnect();
        flushconn=sig.connect(boost::bind(&ConnectedLinearization<Linearization>::flush, this));
      }

      boost::signals2::signal<void ()> changed;
      boost::signals2::connection flushconn;
    };

    /// Bridge::Linearization class that uses a VariationalFunctionalAssembler to create linear systems
    /** Implements AbstractLinearization */
    template<class Functional>
    class KaskadeLinearization : public AbstractLinearization, public SparseLinearSystem
    {
    public:

      static const int nThreads = 32;

      typedef typename Functional::AnsatzVars::VariableSet DomainElement;
      typedef typename Functional::TestVars::VariableSet ImageElement;
      typedef typename Functional::Scalar Scalar;
      typedef LinearizationAt<Functional> Implementation;
      typedef VariationalFunctionalAssembler<Implementation> Assembler;
      typedef typename DomainElement::Descriptions::template CoefficientVectorRepresentation<>::type CoefficientVector;
      typedef Dune::LinearOperator<CoefficientVector, CoefficientVector> OperatorType;
      typedef OperatorType Operator;

      /*      KaskadeLinearization()
      : x(0), fu(nullptr), lin(*fu,x), ass(nullptr), xptr(nullptr)
      {
        flush();
      }

      /// Creation of a linearization for a functional fu at x_
      KaskadeLinearization(Functional const& fu_)
      : x(0), fu(&fu_), lin(*fu,x), ass(nullptr), xptr(nullptr)
      {
        flush();
      }*/

      /// Creation of a linearization for a functional fu at x_
      KaskadeLinearization(Functional const& fu_, DomainElement const& x_)
        : x(x_), fu(&fu_), lin(*fu,x), ass(new Assembler(x.descriptions.spaces)), xptr(new Vector<DomainElement>(x))
      {
        flush();
      }

      /// Creation of a linearization for a functional fu at x_
      KaskadeLinearization(Functional const& fu_, DomainElement const& x_, std::shared_ptr<Assembler> const& ass_)
        : x(x_), fu(&fu_), lin(*fu,x), ass(ass_), xptr(new Vector<DomainElement>(x))
      {
        flush();
      }

      KaskadeLinearization(KaskadeLinearization const& other)
        : x(other.x), fu(other.fu), lin(*fu,x), ass(other.ass), xptr(new Vector<DomainElement>(x))
      {
      }

      virtual ~KaskadeLinearization() {}

      /// Number of columns of components [cbegin, cend)
      int cols(int cbegin=0, int cend=-1) const
      {
        assignIfNegative(cend,nColBlocks());
        assert(cbegin<cend);
        return x.descriptions.degreesOfFreedom(cbegin,cend);
      }

      /// Number of rows of components [rbegin, rend)
      int rows(int rbegin=0, int rend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assert(rbegin < rend);
        return x.descriptions.degreesOfFreedom(rbegin,rend);
      }

      void precompute() {
        doAssemble(Assembler::VALUE | Assembler::RHS | Assembler::MATRIX);
      }

      /// write blocks of the hessian matrix into mat
      void getMatrixBlocks(MatrixAsTriplet<Scalar>& mat, int rbegin, int rend, int cbegin, int cend) const
      {
        doAssemble(Assembler::VALUE | Assembler::RHS | Assembler::MATRIX);
        mat = ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,rend,cbegin,cend);

        if(DiscreteBlockTraits<Functional>::anyPresent)
        {
          MatrixAsTriplet<Scalar> matD;
          getDiscreteMatrixBlocks(matD,rbegin,rend,cbegin,cend);
          mat+=matD;
        }
      }

      /// write components of the gradient into rhs
      void getRHSBlocks(std::vector<Scalar>& rhs, int rbegin, int rend) const
      {
        doAssemble(Assembler::VALUE | Assembler::RHS);
        rhs.resize(rows(rbegin,rend),0.0);
        ass->toSequence(rbegin,rend,rhs.begin());
        if(DiscreteBlockTraits<Functional>::anyPresent)
        {
          addDiscreteRHSBlocks(rhs,rbegin,rend);
        }
      }

      /// return number of columns
      int nColBlocks() const { return Functional::AnsatzVars::noOfVariables; }

      /// return number of rows
      int nRowBlocks() const { return Functional::TestVars::noOfVariables; }

      /// return point of linearization
      AbstractFunctionSpaceElement const& getOrigin() const
      {
        return *xptr;
      }

      void setOrigin(AbstractFunctionSpaceElement const& x_)
      {
        x = Bridge::getImpl<DomainElement>(x_);
        xptr.reset(new Vector<DomainElement>(x));

        if(ass == nullptr)
        {
          ass.reset(new Assembler(x.descriptions.spaces));
        }
      }

      /// return the implementation
      Implementation const& getLinImpl() const {return lin; }

      /// flush all data, gathered so far
      void flush() { ass->flush( Assembler::VALUE | Assembler::RHS | Assembler::MATRIX ); }

      /// return whether x is in the domain of definition
      bool inDomain(DomainElement const& x) { return InDomainTraits<Functional,DomainElement>::inDomain(x); }

      /// return the current value Functional(Origin)
      double eval() const
      {
        doAssemble(Assembler::VALUE);
        Scalar value =ass->functional();
        if(DiscreteBlockTraits<Functional>::anyPresent)
          value += DiscreteBlockTraits<Functional>::getValue(*fu,x);
        return value;
      }

      double getValue() const { return eval(); }

      void evald(AbstractFunctionSpaceElement& v, int rbegin, int rend) const
      {
        assignIfNegative(rend,nRowBlocks());
        assert(rbegin<=rend);
        std::vector<Scalar> rhs(rows(rbegin,rend),0.0);
        getRHSBlocks(rhs,rbegin,rend);
        dynamic_cast<Vector<ImageElement>& >(v).read(rhs,rbegin,rend);
      }

      void ddxpy(AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        d2axpy(1.0,y,x,rbegin,rend,cbegin,cend);
      }

      void d2axpy(double a, AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        assert( cbegin <= cend );
        assert( rbegin <= rend );
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const&>(x).write(argument,cbegin,cend);
        dynamic_cast<Vector<ImageElement> const&>(y).write(result,rbegin,rend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.axpy(result, argument, a);
        dynamic_cast<Vector<ImageElement>&>(y).read(result,rbegin,rend);
      }

      void ddtxpy(AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        d2taxpy(1.0,y,x,rbegin,rend,cbegin,cend);
      }

      void d2taxpy(double a, AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());

        assert( cbegin <= cend );
        assert( rbegin <= rend );
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const&>(x).write(argument,rbegin,rend);
        dynamic_cast<Vector<ImageElement> const&>(y).write(result,cbegin,cend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.transpose().axpy(result, argument, a);
        dynamic_cast<Vector<ImageElement>&>(y).read(result,cbegin,cend);
      }

      double evalL1norm() const
      {
        doAssemble(Assembler::VALUE);
        Scalar value =0;//ass.fL1norm();
        if(DiscreteBlockTraits<Functional>::anyPresent)
          value += std::fabs(DiscreteBlockTraits<Functional>::getValue(*fu,x));
        return value;
      }

      Assembler const& getValidAssembler() const
      {
        doAssemble((Assembler::VALUE | Assembler::RHS | Assembler::MATRIX));
        return *ass;
      }

      Functional const& getFunctional() const
      {
        return *fu;
      }

    protected:
      void addDiscreteRHSBlocks(std::vector<Scalar>& rhs, int rbegin, int rend) const
      {
        for(int i=rbegin; i<rend; ++i)
        {
          std::vector<Scalar> rhsD;
          DiscreteBlockTraits<Functional>::getRHSBlock(rhsD,*fu,x,i);
          if(rhsD.size() > 0)
          {
            assert(rhsD.size() == rows(i,i+1));
            for(int k=rows(rbegin,i), l=0;k<rows(rbegin,i+1);++k, ++l)
              rhs[k]+=rhsD[l];
          }
        }
      }

      void getDiscreteMatrixBlocks(MatrixAsTriplet<Scalar>& mat, int rbegin, int rend, int cbegin, int cend) const
      {
        for(int i=rbegin; i<rend; ++i)
          for(int j=cbegin; j<cend; ++j)
          {
            MatrixAsTriplet<Scalar> matD;
            DiscreteBlockTraits<Functional>::getMatrixBlock(matD, *fu,x,i,j);
            matD.shiftIndices(rows(rbegin,i),cols(cbegin,j));
            mat+=matD;
          }
      }

      void doAssemble(int flags) const
      {
        int toDoFlag= ((~ass->valid()) & flags);
        if(toDoFlag!=0) ass->assemble(lin,toDoFlag,nThreads);
      }

      DomainElement x;
      Functional const* fu;
      Implementation lin;
      mutable std::shared_ptr<Assembler> ass;
      std::unique_ptr<Vector<DomainElement> > xptr;
    };

    template <class Functional> using ConnectedKaskadeLinearization = ConnectedLinearization<KaskadeLinearization<Functional> >;

    template<class Functional, int stateId=1, int adjointId=2>
    class NormalStepLinearization : public ConnectedKaskadeLinearization<Functional>
    {
      typedef ConnectedKaskadeLinearization<Functional> Base;
      typedef typename Base::Scalar Scalar;
      typedef typename Base::Assembler Assembler;
      typedef typename Base::DomainElement DomainElement;
      typedef typename Base::ImageElement ImageElement;
      using Base::nRowBlocks;
      using Base::nColBlocks;
    public:
      NormalStepLinearization() : Base() {}

      /// Creation of a linearization for a functional fu at x_
      NormalStepLinearization(Functional const& fu, DomainElement const& x) : Base(fu,x) {}

      /// Creation of a linearization for a functional fu at x_
      NormalStepLinearization(Functional const& fu, DomainElement const& x, std::shared_ptr<Assembler> const& assembler) : Base(fu,x,assembler) {}

      NormalStepLinearization(Base const& other) : Base(other) {}

      /// write blocks of the hessian matrix into mat
      void getMatrixBlocks(MatrixAsTriplet<Scalar>& mat, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if(rbegin==adjointId && rend==adjointId+1 && cbegin==adjointId && cend==adjointId+1) return;
        this->doAssemble(Assembler::VALUE | Assembler::RHS | Assembler::MATRIX);
        if(rbegin==stateId && rend==stateId+1 && cbegin==stateId && cend==stateId+1)
        {
          mat = this->ass->template get<MatrixAsTriplet<Scalar> >(false,adjointId,adjointId+1,adjointId,adjointId+1);
          return;
        }

        if(rbegin<=stateId && rend>stateId && cbegin<=stateId && cend > stateId)
        {
          size_t rows0 = 0, cols0 = 0, rows1 = 0, cols1 = 0;
          MatrixAsTriplet<Scalar> tmp;
          if(rbegin < stateId)
          {
            tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,stateId,cbegin,cend);
            rows0 = tmp.nrows();
            mat += tmp;
          }
          if(cbegin < stateId)
          {
            tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,stateId,stateId+1,cbegin,stateId);
            cols0 = tmp.ncols();
            tmp.shiftIndices(rows0,0);
            mat += tmp;
          }
          tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,adjointId,adjointId+1,adjointId,adjointId+1);
          rows1 = tmp.nrows(), cols1 = tmp.ncols();
          tmp.shiftIndices(rows0,cols0);
          mat += tmp;
          if(cend > stateId+1)
          {
            tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,stateId,stateId+1,stateId+1,cend);
            tmp.shiftIndices(rows0,cols0+cols1);
            mat += tmp;
          }
          if(rend > stateId+1)
          {
            tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,stateId+1,rend,cbegin,cend);
            tmp.shiftIndices(rows0+rows1,0);
            mat += tmp;
          }

        }
        else
          mat = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,rend,cbegin,cend);

        if(DiscreteBlockTraits<Functional>::anyPresent)
        {
          MatrixAsTriplet<Scalar> matD;
          this->getDiscreteMatrixBlocks(matD,rbegin,rend,cbegin,cend);
          mat+=matD;
        }
      }

      void ddxpy(AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if(rbegin==adjointId && rend==adjointId+1 && cbegin==adjointId && cend==adjointId+1) return;
        assert(cbegin<=cend);
        assert(rbegin<=rend);
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const& >(x).write(argument,cbegin,cend);
        dynamic_cast<Vector<ImageElement>& >(y).write(result,rbegin,rend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.axpy(result, argument);
        dynamic_cast<Vector<ImageElement>& >(y).read(result,rbegin,rend);
      }

      void d2axpy(double a, AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if( rbegin == adjointId && rend == adjointId+1 && cbegin == adjointId && cend == adjointId+1 ) return;
        assert( cbegin <= cend );
        assert( rbegin <= rend );
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const&>(x).write(argument,cbegin,cend);
        dynamic_cast<Vector<ImageElement> const&>(y).write(result,rbegin,rend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.axpy(result, argument, a);
        dynamic_cast<Vector<ImageElement>&>(y).read(result, rbegin, rend);
      }
    };

    template<class Functional, int stateId=1, int adjointId=2>
    class TangentialStepLinearization : public ConnectedKaskadeLinearization<Functional>
    {
      typedef ConnectedKaskadeLinearization<Functional> Base;
      typedef typename Base::Scalar Scalar;
      typedef typename Base::Assembler Assembler;
      typedef typename Base::DomainElement DomainElement;
      typedef typename Base::ImageElement ImageElement;
      using Base::nRowBlocks;
      using Base::nColBlocks;
    public:
      TangentialStepLinearization() : Base() {}

      /// Creation of a linearization for a functional fu at x_
      TangentialStepLinearization(Functional const& fu, DomainElement const& x) : Base(fu,x) {}

      /// Creation of a linearization for a functional fu at x_
      TangentialStepLinearization(Functional const& fu, DomainElement const& x, std::shared_ptr<Assembler> const& assembler) : Base(fu,x,assembler) {}

      TangentialStepLinearization(Base const& other) : Base(other) {}

      /// write blocks of the hessian matrix into mat
      void getMatrixBlocks(MatrixAsTriplet<Scalar>& mat, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if(rbegin==adjointId && rend==adjointId+1 && cbegin==adjointId && cend==adjointId+1) return;
        this->doAssemble(Assembler::VALUE | Assembler::RHS | Assembler::MATRIX);

        if(rend > adjointId && cend > adjointId)
        {
          mat = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,rend-1,cbegin,cend-1);
          size_t rows = mat.nrows(), cols = mat.ncols();
          auto tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rend-1,rend,cbegin,cend-1);
          tmp.shiftIndices(rows,0);
          mat += tmp;
          tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,rend-1,cend-1,cend);
          tmp.shiftIndices(0,cols);
          mat += tmp;
        }
        else
          mat = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,rend,cbegin,cend);

        if(DiscreteBlockTraits<Functional>::anyPresent)
        {
          MatrixAsTriplet<Scalar> matD;
          this->getDiscreteMatrixBlocks(matD,rbegin,rend,cbegin,cend);
          mat+=matD;
        }
      }

      void ddxpy(AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if(rbegin==adjointId && rend==adjointId+1 && cbegin==adjointId && cend==adjointId+1) return;
        assert(cbegin<=cend);
        assert(rbegin<=rend);
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const& >(x).write(argument,cbegin,cend);
        dynamic_cast<Vector<ImageElement>& >(y).write(result,rbegin,rend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.axpy(result, argument);
        dynamic_cast<Vector<ImageElement>& >(y).read(result,rbegin,rend);
      }

      void d2axpy(double a, AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if( rbegin == adjointId && rend == adjointId+1 && cbegin == adjointId && cend == adjointId+1 ) return;
        assert( cbegin <= cend );
        assert( rbegin <= rend );
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const&>(x).write(argument,cbegin,cend);
        dynamic_cast<Vector<ImageElement> const&>(y).write(result,rbegin,rend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.axpy(result, argument, a);
        dynamic_cast<Vector<ImageElement>&>(y).read(result,rbegin,rend);
      }
    };

    //template <class Functional, int stateId=1, int adjointId=2> using NormalStepLinearization = ConnectedLinearization<NormalStepKaskadeLinearization<Functional,stateId,adjointId> >;
    //template <class Functional, int stateId=1, int adjointId=2> using TangentialStepLinearization = ConnectedLinearization<TangentialStepKaskadeLinearization<Functional,stateId,adjointId> >;

  }
}

#endif /* BRIDGE_HH_ */


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

  @copyright 2017 NumScale SAS

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

#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/pedantic.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/assert.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( clamp_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::unspecified_<A0> >
                          , bd::scalar_< bd::unspecified_<A0> >
                          , bd::scalar_< bd::unspecified_<A0> >
                          )
  {
    BOOST_FORCEINLINE
    A0 operator() ( A0 v, A0 lo, A0 hi) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(lo <= hi, "lo is not less or equal to hi");
      return (v < lo) ? lo : (hi < v) ? hi : v;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( clamp_
                          , (typename A0)
                          , bd::cpu_
                          , bs::pedantic_tag
                          , bd::scalar_< bd::unspecified_<A0> >
                          , bd::scalar_< bd::unspecified_<A0> >
                          , bd::scalar_< bd::unspecified_<A0> >
                          )
  {
    BOOST_FORCEINLINE
    A0 operator() ( pedantic_tag const &
                  , A0 v, A0 lo, A0 hi) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(lo <=  hi, "lo is not less or equal to hi");
      return (v < lo) ? lo : (hi < v) ? hi : v;
    }
  };

} } }

#endif


/***************************************************************************
 *   Software License Agreement (BSD License)                              *
 *   Copyright (C) 2017 by Florian Beck <florian.beck@tuwien.ac.at>        *
 *                                                                         *
 *   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.                                           *
 ***************************************************************************/

#include <boost/date_time/posix_time/posix_time_duration.hpp>
#include <boost/date_time/posix_time/time_formatters.hpp>
#include <limits>
#include "munkres.h"
#include "object_tracker_separate.h"

ObjectTrackerSeparate::ObjectTrackerSeparate(std::shared_ptr<ParticleFilterConfig> pf_config,
                                             std::shared_ptr<TrackerConfig> t_config)
  : ObjectTracker(pf_config, t_config)
{
}

bool ObjectTrackerSeparate::calcAssignments(const MeasurementObjectConstPtr& detection, std::vector<int>& assignment,
                                            Eigen::MatrixXd& D)
{
  if (detection->size() == 0)
  {
    return false;
  }

  int n = tracks_.size() > detection->size() ? tracks_.size() : detection->size();

  D.resize(n, n);

  int track_idx = 0;
  size_t detection_idx = 0;

  for (auto&& track : tracks_)
  {
    for (detection_idx = 0; detection_idx < (*detection).size(); detection_idx++)
    {
      Eigen::Matrix<double, 2, 2> S = (*detection)[detection_idx].covariance.block<2, 2>(0, 0);

      Eigen::Matrix<double, 2, 1> detection_position;
      detection_position(0, 0) = (*detection)[detection_idx].pose2d.x();
      detection_position(1, 0) = (*detection)[detection_idx].pose2d.y();

      Eigen::Matrix<double, 2, 1> track_position = track.second.estimatedState().block<2, 1>(0, 0);

      if (t_config_->use_particle_mahalanobis && t_config_->use_mahalanobis && detection->sensor_type() != SENSOR_TYPE_GENERIC_MONOCULAR_VISION)
      {
        D(track_idx, detection_idx) = 0.0;

        // alternative per particle distance
        for (auto&& p : (*track.second.particles()))
        {
          D(track_idx, detection_idx) +=
              sqrt((detection_position - p.state.block<2, 1>(0, 0)).transpose() * ((S.block<2, 2>(0, 0)).inverse()) *
                   (detection_position - p.state.block<2, 1>(0, 0)));
        }

        if (track.second.particles()->size() > 0)
        {
          D(track_idx, detection_idx) /= track.second.particles()->size();
        }
      }
      else if (t_config_->use_mahalanobis && detection->sensor_type() != SENSOR_TYPE_GENERIC_MONOCULAR_VISION)
      {
        S = S + track.second.stateCovariance()->block<2, 2>(0, 0);

        // calculate mahalanobis distance between detection and track
        D(track_idx, detection_idx) = sqrt((detection_position - track_position).transpose() * S.inverse() *
                                           (detection_position - track_position));
      }
      else if (detection->sensor_type() == SENSOR_TYPE_GENERIC_MONOCULAR_VISION)
      {
        Eigen::Vector2d P1;
        Eigen::Vector2d P2;
        Eigen::Vector2d P0;

        if ((*detection)[detection_idx].shape_variables.size() >= 6)
        {
          P1 << (*detection)[detection_idx].shape_variables[0], (*detection)[detection_idx].shape_variables[1],
              (*detection);

          P2 << (*detection)[detection_idx].shape_variables[3], (*detection)[detection_idx].shape_variables[4],
              (*detection);

          P0 = track.second.estimatedState().head(2);  // first two elements of state, i.e. x and y

          D(track_idx, detection_idx) = acos((P2 - P1).normalized().dot((P0 - P1).normalized()));
        }
        else
        {
          std::cout << "error, shape variables not present" << std::endl;
          return false;
        }
      }
      else
      {
        D(track_idx, detection_idx) = (detection_position - track_position).norm();
      }
    }
    track_idx++;
  }

  // augment matrix to square size
  for (int i = track_idx; i < n; i++)
  {
    for (int j = 0; j < n; j++)
    {
      D(i, j) = 0;
    }
  }
  for (int i = detection_idx; i < n; i++)
  {
    for (int j = 0; j < n; j++)
    {
      D(j, i) = 0;
    }
  }

  // elements mean assign track 1 to meas assignment[1] etc.
  // hungarian algorithm / munkres algorithm O(n^3)
  std::vector<std::pair<int, int>> assignment_pairs = tuw::Munkres::find_minimum_assignment(D);

  assignment.resize(n);

  for (auto&& a = assignment_pairs.begin(); a != assignment_pairs.end(); a++)
  {
    assignment.at(a->first) = a->second;
  }

  return true;
}

// data association, Nearest Neighbor
// create new tracks if required,
// delete unnecessary tracks if required
void ObjectTrackerSeparate::update()
{
  if (detections_.empty())
  {
    MeasurementObject::Object empty_dummy;

    // deleting from map while looping is not a good idea ...
    auto tmp_tracks = tracks_;

    for (auto&& track_it : tmp_tracks)
    {
      // use .at here since [] creates an element if it does not exist
      if ((track_it.second.getVisibility() && track_it.second.getDelete() > t_config_->deletion_cycles) ||
          (!track_it.second.getVisibility() && track_it.second.getDelete() > t_config_->deletion_cycles_inv))
      {
        tracks_.erase(track_it.first);
      }
      else
      {
        tracks_.at(track_it.first).update(empty_dummy, true);
        tracks_.at(track_it.first).incDelete();
      }
    }
  }
  else
  {
    for (auto&& det : detections_)
    {
      Eigen::MatrixXd D;
      std::vector<int> assignment;

      bool assigned = calcAssignments(det, assignment, D);

      // update tracks with measurements
      int track_idx = 0;
      std::vector<int> not_updated_tracks;
      std::vector<int> unused_detections;
      unused_detections.resize(det->size());
      for (size_t i = 0; i < det->size(); i++)
      {
        unused_detections[i] = i;
      }

      for (auto&& track_it : tracks_)
      {
        if (assigned && D(track_idx, assignment[track_idx]) > 0 &&
            D(track_idx, assignment[track_idx]) < t_config_->max_dist_for_association)
        {
          track_it.second.update((*det)[assignment[track_idx]], false);
          track_it.second.incPromote();
          track_it.second.resetDelete();
          if (det->sensor_type() == SENSOR_TYPE_GENERIC_MONOCULAR_VISION ||
              det->sensor_type() == SENSOR_TYPE_GENERIC_RGBD)
          {
            // track_it.second.setVisualConfirmation(true);
            track_it.second.incVisualConfirmation();
          }
          else
          {
            track_it.second.decVisualConfirmation();
          }
          // std::cout << "updated track[" << track_idx << "], id = " << track_it.first << " with detection[" <<
          // assignment[track_idx] << "]" << std::endl;
          unused_detections[assignment[track_idx]] = -1;
        }
        else
        {
          not_updated_tracks.emplace_back(track_it.first);
        }

        // set tracks with more than 1 promotions to visible
        if (track_it.second.getPromote() > t_config_->promotion_cycles)
        {
          track_it.second.setVisibility(true);
        }

        track_idx++;
      }
      // std::cout << std::endl;

      // consider measurements not assigned to a track for track creation
      // if visually confirmed is active, only create if from visual detector
      for (size_t i = 0; i < unused_detections.size(); i++)
      {
        // simply create tracks for unassigned measurements
        if (unused_detections[i] != -1)
        {
          Eigen::Matrix<double, STATE_SIZE, 1> init_state;

          init_state(static_cast<int>(State::X)) = (*det)[unused_detections[i]].pose2d.x();
          init_state(static_cast<int>(State::Y)) = (*det)[unused_detections[i]].pose2d.y();
          init_state(static_cast<int>(State::VX)) = 0;
          init_state(static_cast<int>(State::VY)) = 0;
          init_state(static_cast<int>(State::AX)) = 0;
          init_state(static_cast<int>(State::AY)) = 0;
          init_state(static_cast<int>(State::OMEGA)) = 0;

          if (det->sensor_type() != SENSOR_TYPE_GENERIC_MONOCULAR_VISION)
          {
            int created_id = createTrack(init_state);

            if (det->sensor_type() == SENSOR_TYPE_GENERIC_RGBD)
            {
              tracks_.at(created_id).incVisualConfirmation();
            }
          }
        }
      }

      // consider tracks without measurement for deletion
      // delete if no measurement occured within a certain number of cycles
      // otherwise forward predict w/o weighting

      MeasurementObject::Object empty_dummy;
      empty_dummy.stamp = det->stamp();

      for (auto&& to_delete : not_updated_tracks)
      {
        // use .at here since [] creates an element if it does not exist
        if ((tracks_.at(to_delete).getVisibility() && tracks_.at(to_delete).getDelete() > t_config_->deletion_cycles) ||
            (!tracks_.at(to_delete).getVisibility() &&
             tracks_.at(to_delete).getDelete() > t_config_->deletion_cycles_inv))
        {
          tracks_.erase(to_delete);
        }
        else
        {
          // std::cout << "forward predict w/ dummy" << std::endl;
          tracks_.at(to_delete).update(empty_dummy, true);
          tracks_.at(to_delete).incDelete();
        }
      }
    }
  }
}