/*
Copyright 2009-2021 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include <math/seginter.hpp>
#include <math/mathtools.hpp>
#include <boost/optional.hpp>

namespace eXl
{
  Intersector::Event::Event(PooledList<uint32_t>::Pool& iPool, Vector2Q const& iPoint, uint32_t iSeg, Type iType)
    : m_SegmentsStart(iPool)
    , m_SegmentsInter(iPool)
    , m_SegmentsEnd(iPool)
    , m_Point(iPoint)
  {
    if(iType == Start)
    {
      m_SegmentsStart.PushBack(iSeg);
    }
    else if (iType == End)
    {
      m_SegmentsEnd.PushBack(iSeg);
    }
  }

  Intersector::Event::Event(PooledList<uint32_t>::Pool& iPool, Vector2Q const& iPoint, uint32_t iSeg1, uint32_t iSeg2)
    : m_SegmentsStart(iPool)
    , m_SegmentsInter(iPool)
    , m_SegmentsEnd(iPool)
    , m_Point(iPoint)
  {
    m_SegmentsInter.PushBack(iSeg1);
    m_SegmentsInter.PushBack(iSeg2);
  }

  Intersector::Event::Event(Event const& iEvt)
    : m_SegmentsStart(iEvt.m_SegmentsStart)
    , m_SegmentsInter(iEvt.m_SegmentsInter)
    , m_SegmentsEnd(iEvt.m_SegmentsEnd)
    , m_Point(iEvt.m_Point)
  {

  }

  Intersector::Event& Intersector::Event::operator=(Event const& iEvt)
  {
    m_SegmentsStart = iEvt.m_SegmentsStart;
    m_SegmentsInter = iEvt.m_SegmentsInter;
    m_SegmentsEnd = iEvt.m_SegmentsEnd;
    m_Point = iEvt.m_Point;

    return *this;
  }

  Intersector::Event::Event(Event&& iEvt)
    : m_SegmentsStart(std::move(iEvt.m_SegmentsStart))
    , m_SegmentsInter(std::move(iEvt.m_SegmentsInter))
    , m_SegmentsEnd(std::move(iEvt.m_SegmentsEnd))
    , m_Point(iEvt.m_Point)
  {

  }

  Intersector::Event& Intersector::Event::operator=(Event&& iEvt)
  {
    m_SegmentsStart = std::move(iEvt.m_SegmentsStart);
    m_SegmentsInter = std::move(iEvt.m_SegmentsInter);
    m_SegmentsEnd = std::move(iEvt.m_SegmentsEnd);
    m_Point = iEvt.m_Point;

    return *this;
  }

  bool Intersector::Event::IsEmpty() const
  {
    return m_SegmentsStart.Begin() == m_SegmentsStart.End()
      && m_SegmentsEnd.Begin() == m_SegmentsEnd.End()
      && m_SegmentsInter.Begin() == m_SegmentsInter.End();
  }

  bool Intersector::Event::operator<(Event const& iOther) const
  {
    return m_Point < iOther.m_Point;
  }

  Intersector::OrderedSeg::OrderedSeg(Segmenti const& iSeg, uint32_t iOrigSeg)
    : m_Start(ToVec2Q(iSeg.m_Ext1))
    , m_End(ToVec2Q(iSeg.m_Ext2))
    , m_OrigSeg(iOrigSeg)
  {
    if(iSeg.m_Ext2 < iSeg.m_Ext1)
    {
      std::swap(m_Start, m_End);
    }
    if(m_End.X() != m_Start.X())
    {
      m_Slope = QType(m_End.Y() - m_Start.Y()) / QType(m_End.X() - m_Start.X());
    }
  }

  QType Intersector::OrderedSeg::GetYAt(QType iX, Event const& iEvt) const
  {
    if(m_Slope)
    {
      return m_Start.Y() + (*m_Slope) * (iX - m_Start.X());
    }
    else
    {
      return iEvt.m_Point.Y();
    }
  }

  bool Intersector::CheckIntersection(Event const& iEvt, OrderedSeg const& iSeg1, OrderedSeg const& iSeg2, boost::optional<Event>& outSeg)
  {
    uint32_t seg1Idx = &iSeg1 - m_Segments.data();
    uint32_t seg2Idx = &iSeg2 - m_Segments.data();

    Vector2Q interPt;
    uint32_t res = Segment<QType>::Intersect(iSeg1.m_Start, iSeg1.m_End, iSeg2.m_Start, iSeg2.m_End, interPt, 0);
    if(res == Segment<QType>::PointOnSegments)
    {
      if(iEvt.m_Point < interPt || iEvt.m_Point == interPt)
      {
        outSeg = Event(m_SegListPool, interPt, seg1Idx, seg2Idx);
      }
    }
    else if(res & Segment<QType>::ConfoundSegments)
    {
      //eXl_ASSERT(iEvt.m_Type == Event::Start);

      if(iSeg1.m_End.m_X > iSeg2.m_End.m_X)
      {
        //Reinsert seg1's start at seg2's end.
        Event newStart(m_SegListPool, iSeg2.m_End, seg1Idx, Event::Start);
        auto lowerBound = std::lower_bound(m_EventQueue.rbegin(), m_EventQueue.rend(), newStart);
        if(lowerBound->m_Point == iSeg2.m_End)
        {
          lowerBound->m_SegmentsStart.PushBack(seg1Idx);
        }
        else
        {
          m_EventQueue.insert(lowerBound.base(), std::move(newStart));
        }

        //m_EventQueue.emplace(std::move(newStart));
      }
      else if(iSeg1.m_End != iEvt.m_Point)
      {
        //Seg1 is useless, remove it.
        Event evt(m_SegListPool, iSeg1.m_End, seg1Idx, Event::End);
        auto toFixup = std::lower_bound(m_EventQueue.rbegin(), m_EventQueue.rend(), evt);

        // remove iSeg1's endpoint from the list.
        auto iterToRemove = std::prev(toFixup.base());
        auto endingSegs = iterToRemove->m_SegmentsEnd.Begin();
        while(*endingSegs != seg1Idx)
        {
          ++endingSegs;
        }

        //eXl_ASSERT(endingSegs != iterToRemove->m_SegmentsEnd.End());
        iterToRemove->m_SegmentsEnd.Erase(endingSegs);

        if(iterToRemove->IsEmpty())
        {
          m_EventQueue.erase(iterToRemove);
        }
      }

      for (auto iter = m_ActiveSegments.begin(); iter != m_ActiveSegments.end(); ++iter)
      {
        if (iter->m_Idx == seg1Idx)
        {
          m_ActiveSegments.erase(iter);
          break;
        }
      }

      return false;
    }

    return true;
  }

  void Intersector::InsertEvent(Event&& iEvt)
  {
    //for(auto iter = iEvt.m_Segments.Begin(); iter != iEvt.m_Segments.End(); ++iter)
    //{
    //  ++m_SegInsertCount[*iter];
    //}

    auto lowerBound = std::lower_bound(m_EventQueue.rbegin(), m_EventQueue.rend(), iEvt);
    //auto lowerBound = m_EventQueue.find(iEvt);
    if(lowerBound != m_EventQueue.rend()
      //lowerBound != m_EventQueue.end()
      //&& lowerBound->m_Type == Event::Intersection
      && lowerBound->m_Point == iEvt.m_Point
      )
    {
      for(auto startSegIter = iEvt.m_SegmentsStart.Begin(); startSegIter != iEvt.m_SegmentsStart.End(); ++startSegIter )
      {
        lowerBound->m_SegmentsStart.PushBack(*startSegIter);
      }
      for(auto endSegIter = iEvt.m_SegmentsEnd.Begin(); endSegIter != iEvt.m_SegmentsEnd.End(); ++endSegIter )
      {
        lowerBound->m_SegmentsEnd.PushBack(*endSegIter);
      }
      for(auto interSegIter = iEvt.m_SegmentsInter.Begin(); interSegIter != iEvt.m_SegmentsInter.End(); ++interSegIter )
      {
        lowerBound->m_SegmentsInter.PushBack(*interSegIter);
      }
    }
    else
    {
      m_EventQueue.insert(lowerBound.base(), std::move(iEvt));
      //m_EventQueue.emplace(std::move(iEvt));
    }

  }

  void Intersector::CheckIntersection(Event const& iEvt, OrderedSeg const& iSeg1, OrderedSeg const& iSeg2)
  {
    boost::optional<Event> evt;
    CheckIntersection(iEvt, iSeg1, iSeg2, evt);
    if(evt && evt->m_Point != iEvt.m_Point)
    {
      InsertEvent(std::move(*evt));
    }
  }

  void Intersector::RemoveEvt(uint32_t iSeg1, uint32_t iSeg2)
  {
    if(iSeg1 != iSeg2)
    {
      //if(m_SegInsertCount[iSeg1] == 0 || m_SegInsertCount[iSeg2] == 0)
      //{
      //  return;
      //}

      for(auto iter = m_EventQueue.begin(); iter!= m_EventQueue.end(); ++iter)
      {
        uint32_t countSeg = 0;
        PooledList<uint32_t>::Iterator segs[] = {iter->m_SegmentsInter.End(), iter->m_SegmentsInter.End()};
        for(auto iterSeg = iter->m_SegmentsInter.Begin(); iterSeg != iter->m_SegmentsInter.End(); ++iterSeg)
        {
          ++countSeg;
          if(*iterSeg == iSeg1 && segs[0] == iter->m_SegmentsInter.End())
          {
            segs[0] = iterSeg;
          }
          if(*iterSeg == iSeg2 && segs[1] == iter->m_SegmentsInter.End())
          {
            segs[1] = iterSeg;
          }
        }

        if(segs[0] != iter->m_SegmentsInter.End()
          && segs[1] != iter->m_SegmentsInter.End())
        {
          if(iter->IsEmpty())
          {
            m_EventQueue.erase(iter);
          }
          else
          {
            iter->m_SegmentsInter.Erase(segs[0]);
            iter->m_SegmentsInter.Erase(segs[1]);
          }
          //m_SegInsertCount[iSeg1]--;
          //m_SegInsertCount[iSeg2]--;

          break;
        }
      }
    }
  };

  Intersector::Intersector()
    //: m_EvtListAlloc(4096)
    //, m_EventQueue(std::less<Event>(), PooledAllocator<Event>(m_EvtListAlloc))
  {

  }

  void Intersector::SortSegmentExtremities()
  {
    for(auto& evt : m_EventQueue)
    {
      auto sortPredicate = [&] (uint32_t const& iSeg1, uint32_t const& iSeg2)
      {
        auto const& seg1 = m_Segments[iSeg1];
        auto const& seg2 = m_Segments[iSeg2];

        if(seg1.m_Slope)
        {
          if(seg2.m_Slope)
          {
            return *seg1.m_Slope < *seg2.m_Slope;
          }
        }

        return seg2.m_Slope && !seg1.m_Slope;

      };

      m_SortArray.clear();
      for(auto startSegIter = evt.m_SegmentsStart.Begin(); startSegIter != evt.m_SegmentsStart.End(); ++startSegIter )
      {
        m_SortArray.push_back(*startSegIter);
      }
      evt.m_SegmentsStart.Clear();

      std::sort(m_SortArray.begin(), m_SortArray.end(), sortPredicate);

      for(uint32_t segIdx : m_SortArray)
      {
        evt.m_SegmentsStart.PushBack(segIdx);
      }

      m_SortArray.clear();
      for(auto endSegIter = evt.m_SegmentsEnd.Begin(); endSegIter != evt.m_SegmentsEnd.End(); ++endSegIter )
      {
        m_SortArray.push_back(*endSegIter);
      }
      evt.m_SegmentsEnd.Clear();

      std::sort(m_SortArray.begin(), m_SortArray.end(), sortPredicate);

      for(uint32_t segIdx : m_SortArray)
      {
        evt.m_SegmentsEnd.PushBack(segIdx);
      }

    }
  }

  Err Intersector::IntersectSegments(Vector<Segmenti> const& iSegments, Vector<std::pair<uint32_t, Segmenti>>& oSegs, Parameters const& iParams)
  {
    //m_EventQueue.~EventQueue();
    //m_EvtListAlloc.Reset();
    //new(&m_EventQueue) EventQueue(std::less<Event>(), PooledAllocator<Event>(m_EvtListAlloc));
    //m_SegListPool.Reserve(1024);

    m_EventQueue.clear();
    m_Segments.clear();
    m_ActiveSegments.clear();
    m_SegInsertCount.clear();

    {
      uint32_t segCounter = 0;
      for(auto const& seg : iSegments)
      {
        if(seg.m_Ext1 != seg.m_Ext2)
        {
          m_Segments.push_back(OrderedSeg(seg, segCounter++));

          //m_EventQueue.emplace_back(Event(m_SegListPool, m_Segments.back().m_Start, m_Segments.size() - 1, Event::Start));
          //m_EventQueue.emplace_back(Event(m_SegListPool, m_Segments.back().m_End, m_Segments.size() - 1, Event::End));

          InsertEvent(Event(m_SegListPool, m_Segments.back().m_Start, m_Segments.size() - 1, Event::Start));
          InsertEvent(Event(m_SegListPool, m_Segments.back().m_End, m_Segments.size() - 1, Event::End));

          //m_EventQueue.emplace(Event(m_SegListPool, m_Segments.back().m_Start, m_Segments.size() - 1, Event::Start));
          //m_EventQueue.emplace(Event(m_SegListPool, m_Segments.back().m_End, m_Segments.size() - 1, Event::End));

          //m_SegInsertCount.push_back(0);
        }
      }
      //if(iParams.m_UsrEvtCb)
      //{
      //  for(uint32_t pos = 0; pos < iParams.m_UsrEvts.size(); ++pos)
      //  {
      //    m_EventQueue.emplace_back(Event(m_SegListPool, ToVec2Q(iParams.m_UsrEvts[pos].m_Position), pos, Event::User));
      //  }
      //}

      //std::sort(m_EventQueue.begin(), m_EventQueue.end());
    }

    //std::reverse(m_EventQueue.begin(), m_EventQueue.end());

    SortSegmentExtremities();

    while(!m_EventQueue.empty())
    {
      Event curEvt = std::move(m_EventQueue.back());
      m_EventQueue.pop_back();

      //Event curEvt = *m_EventQueue.begin();
      //m_EventQueue.erase(m_EventQueue.begin());

      auto curX = curEvt.m_Point.X();

      auto activeSegComp = [this, curX, &curEvt](ActiveSegment const& iSeg1, ActiveSegment const& iSeg2)
      {
        auto const& seg1 = m_Segments[iSeg1.m_Idx];
        auto const& seg2 = m_Segments[iSeg2.m_Idx];

        auto y1 = seg1.GetYAt(curX, curEvt);
        auto y2 = seg2.GetYAt(curX, curEvt);

        if(y1 != y2)
        {
          return y1 < y2;
        }
        else
        {
          if(seg1.m_Slope)
          {
            if(seg2.m_Slope)
            {
              return *seg1.m_Slope < *seg2.m_Slope;
            }
            return true;
          }
          return false;

        }
      };

      //if(curEvt.m_Type == Event::Intersection)
      //{
      //  for(auto iter = curEvt.m_Segments.Begin(); iter != curEvt.m_Segments.End(); ++iter)
      //  {
      //    --m_SegInsertCount[*iter];
      //  }
      //}

      //switch(curEvt.m_Type)
      {

      //case Event::Intersection:
      if(curEvt.m_SegmentsInter.Begin() != curEvt.m_SegmentsInter.End())
      {
        //Set<uint32_t> checkSet;
        int32_t posLow = INT_MAX;
        int32_t posHigh = -INT_MAX;

        for(auto iter = curEvt.m_SegmentsInter.Begin(); iter != curEvt.m_SegmentsInter.End(); ++iter)
        {
          for(int32_t i = 0; i<m_ActiveSegments.size(); ++i)
          {
            if(m_ActiveSegments[i].m_Idx == *iter)
            {
              if(i < posLow)
              {
                posLow = i;
              }
              if(i > posHigh)
              {
                posHigh = i;
              }
              //checkSet.insert(i);
              break;
            }
          }
        }
        //if(checkSet.size() != (posHigh - posLow) + 1)
        //{
        //  //eXl_ASSERT(false);
        //  return Err::Failure;
        //}
        if(!(posLow < m_ActiveSegments.size() && posHigh > posLow))
        {
          //eXl_ASSERT(false);
          return Err::Failure;
        }

        for(int32_t i = posLow; i<=posHigh; ++i)
        {
          ActiveSegment& activeSeg = m_ActiveSegments[i];

          if(activeSeg.m_Point != curEvt.m_Point)
          {
            if(!iParams.m_Filter || iParams.m_Filter(m_ActiveSegments, i, Segment<QType>{activeSeg.m_Point, curEvt.m_Point} ))
            {
              Segmenti interSeg = {FromVec2Q<int>(activeSeg.m_Point), FromVec2Q<int>(curEvt.m_Point)};
              oSegs.push_back(std::make_pair(activeSeg.m_Idx, interSeg));
            }
            activeSeg.m_Point = curEvt.m_Point;
          }
        }

        std::reverse(m_ActiveSegments.begin() + posLow, m_ActiveSegments.begin() + posHigh + 1);

        //Order is reversed now.
        uint32_t lowSegIdx = m_ActiveSegments[posLow].m_Idx;
        uint32_t highSegIdx = m_ActiveSegments[posHigh].m_Idx;
        auto const& lowSeg = m_Segments[lowSegIdx];
        auto const& highSeg = m_Segments[highSegIdx];

        if(posHigh + 1 < m_ActiveSegments.size())
        {
          uint32_t higherSegIdx = m_ActiveSegments[posHigh + 1].m_Idx;

          auto const& higherSeg = m_Segments[higherSegIdx];
          CheckIntersection(curEvt, highSeg, higherSeg);
          RemoveEvt(higherSegIdx, lowSegIdx);
        }
        if(posLow > 0)
        {
          uint32_t lowerSegIdx = m_ActiveSegments[posLow - 1].m_Idx;

          auto const& lowerSeg = m_Segments[lowerSegIdx];
          CheckIntersection(curEvt, lowSeg, lowerSeg);
          RemoveEvt(highSegIdx, lowerSegIdx);
        }
      }

      //case Event::End:
      for(auto iterEnd = curEvt.m_SegmentsEnd.Begin(); iterEnd != curEvt.m_SegmentsEnd.End(); ++iterEnd)
      {
        uint32_t curSegIdx = *iterEnd;
        auto const& curSeg = m_Segments[curSegIdx];

        //ActiveSegment activeSeg = {curEvt.m_Point, curSegIdx};

        //auto lowerBound = std::lower_bound(m_ActiveSegments.begin(), m_ActiveSegments.end(), activeSeg, activeSegComp);
        int32_t curSegPos = -1;

        for(auto& seg : m_ActiveSegments)
        {
          if(seg.m_Idx == curSegIdx)
          {
            curSegPos = &seg - m_ActiveSegments.data();
          }
        }

        if(curSegPos == -1)
        {
          //eXl_ASSERT(false);
          return Err::Failure;
        }

        //Segmenti finalSeg = {FromVec2Q<int>(m_ActiveSegments[curSegPos].m_Point), FromVec2Q<int>(curEvt.m_Point)};
        if(m_ActiveSegments[curSegPos].m_Point != curEvt.m_Point)
        {
          if(!(iParams.m_Filter) || iParams.m_Filter(m_ActiveSegments, curSegPos, Segment<QType>{m_ActiveSegments[curSegPos].m_Point, curEvt.m_Point}))
          {
            Segmenti finalSeg = {FromVec2Q<int>(m_ActiveSegments[curSegPos].m_Point), FromVec2Q<int>(curEvt.m_Point)};
            oSegs.push_back(std::make_pair(curSegIdx, finalSeg));
          }
        }

        m_ActiveSegments.erase(m_ActiveSegments.begin() + curSegPos);

        int32_t lowerSegIdx = -1;
        int32_t higherSegIdx = -1;

        if(curSegPos > 0)
        {
          lowerSegIdx = m_ActiveSegments[curSegPos - 1].m_Idx;
        }
        if(curSegPos < m_ActiveSegments.size())
        {
          higherSegIdx = m_ActiveSegments[curSegPos].m_Idx;
        }

        if(lowerSegIdx != -1 && higherSegIdx != -1)
        {
          auto const& lowerSeg = m_Segments[lowerSegIdx];
          auto const& higherSeg = m_Segments[higherSegIdx];

          CheckIntersection(curEvt, lowerSeg, higherSeg);
        }
      }

      //case Event::Start:
      for(auto iterStart = curEvt.m_SegmentsStart.Begin(); iterStart != curEvt.m_SegmentsStart.End(); ++iterStart)
      {
        uint32_t curSegIdx = *iterStart;
        auto const& curSeg = m_Segments[curSegIdx];

        ActiveSegment newSeg = {curEvt.m_Point, curSegIdx};

        auto lowerBound = std::lower_bound(m_ActiveSegments.begin(), m_ActiveSegments.end(), newSeg, activeSegComp);

        int32_t lowerSegIdx = -1;
        int32_t higherSegIdx = -1;

        boost::optional<Event> evts[2];

        bool acceptSegment = true;

        if(lowerBound > m_ActiveSegments.begin())
        {
          lowerSegIdx = (lowerBound - 1)->m_Idx;
          auto const& lowerSeg = m_Segments[lowerSegIdx];

          acceptSegment &= CheckIntersection(curEvt, curSeg, lowerSeg, evts[0]);
        }
        if(lowerBound != m_ActiveSegments.end())
        {
          higherSegIdx = lowerBound->m_Idx;
          auto const& higherSeg = m_Segments[higherSegIdx];

          acceptSegment &= CheckIntersection(curEvt, curSeg, higherSeg, evts[1]);
        }

        if(acceptSegment)
        {
          for(auto& optEvt : evts)
          {
            if(optEvt)
            {
              if(optEvt->m_Point != curEvt.m_Point)
              {
                InsertEvent(std::move(*optEvt));
              }
              else
              {
                ActiveSegment* activeSeg = nullptr;
                // Only report event.
                for(auto iterSeg = optEvt->m_SegmentsInter.Begin(); iterSeg != optEvt->m_SegmentsInter.End(); ++iterSeg)
                {
                  uint32_t segIdx = *iterSeg;
                  if(segIdx == curSegIdx)
                    continue;

                  for(auto& seg : m_ActiveSegments)
                  {
                    if(seg.m_Idx == segIdx)
                    {
                      activeSeg = &seg;
                      break;
                    }
                  }

                  //eXl_ASSERT(activeSeg);

                  if(activeSeg->m_Point != curEvt.m_Point)
                  {
                    if(!iParams.m_Filter || iParams.m_Filter(m_ActiveSegments, activeSeg - m_ActiveSegments.data(), Segment<QType>{activeSeg->m_Point, curEvt.m_Point}))
                    {
                      Segmenti interSeg = {FromVec2Q<int>(activeSeg->m_Point), FromVec2Q<int>(curEvt.m_Point)};
                      oSegs.push_back(std::make_pair(activeSeg->m_Idx, interSeg));
                    }
                    activeSeg->m_Point = curEvt.m_Point;
                  }
                }
              }
            }
          }

          m_ActiveSegments.insert(lowerBound, newSeg);

          if(lowerSegIdx != -1 && higherSegIdx != -1)
          {
            // Only remove the event when the three segments do not intersect at the same point.
            if(!evts[0]
              || !evts[1]
              || evts[0]->m_Point != evts[1]->m_Point)
            {
              RemoveEvt(lowerSegIdx, higherSegIdx);
            }
          }
        }
      }

      /*
      case Event::User:
      {
        auto userPtComp = [this, curX, &curEvt](ActiveSegment const& iSeg1, ActiveSegment const& iSeg2)
        {
          auto const& seg = m_Segments[iSeg1.m_Idx == -1 ? iSeg2.m_Idx : iSeg1.m_Idx];

          auto y1 = seg.GetYAt(curX, curEvt);

          return y1 < curEvt.m_Point.Y();
        };

        ActiveSegment dummySeg = {curEvt.m_Point, -1};
        auto lowerBound = std::lower_bound(m_ActiveSegments.begin(), m_ActiveSegments.end(), dummySeg, userPtComp);

        int32_t lowSeg = -1;
        int32_t highSeg = -1;

        if(lowerBound != m_ActiveSegments.begin())
        {
          lowSeg = (lowerBound - 1) - m_ActiveSegments.begin();
        }

        if(lowerBound != m_ActiveSegments.end())
        {
          highSeg = lowerBound - m_ActiveSegments.begin();
        }

        iParams.m_UsrEvtCb(m_ActiveSegments, lowSeg, highSeg, iParams.m_UsrEvts[*curEvt.m_Segments.Begin()]);

        break;
      }*/

      }
    }

    return Err::Success;
  }
}


/*ckwg +29
 * Copyright 2015 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any 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 AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * \file
 * \brief core homography template implementations
 */

#include "homography.h"

#include <cmath>

#include <maptk/exceptions/math.h>
#include <maptk/logging_macros.h>

#include <Eigen/LU>

namespace maptk
{

namespace //anonymous
{

/// Private helper method for point transformation via homography matrix
template <typename T>
Eigen::Matrix<T,2,1>
h_map_point( Eigen::Matrix<T,3,3> const &h, Eigen::Matrix<T,2,1> const &p )
{
  Eigen::Matrix<T,3,1> out_pt = h * Eigen::Matrix<T,3,1>(p[0], p[1], 1.0);
  if( fabs(out_pt[2]) <= Eigen::NumTraits<T>::dummy_precision() )
  {
    throw point_maps_to_infinity();
  }
  return Eigen::Matrix<T,2,1>( out_pt[0] / out_pt[2], out_pt[1] / out_pt[2] );
}

} // end anonymous namespace

/// Construct an identity homography
template <typename T>
homography_<T>
::homography_()
  : h_( matrix_t::Identity() )
{
}

/// Construct from a provided transformation matrix
template <typename T>
homography_<T>
::homography_( Eigen::Matrix<T,3,3> const &mat )
  : h_( mat )
{
}

/// Conversion Copy constructor -- float specialization
template <>
template <>
homography_<float>
::homography_( homography_<float> const &other )
  : h_( other.get_matrix() )
{
}

/// Conversion Copy constructor -- double specialization
template <>
template <>
homography_<double>
::homography_( homography_<double> const &other )
  : h_( other.get_matrix() )
{
}

/// Construct from a generic homography
template <typename T>
homography_<T>
::homography_( homography const &base )
  : h_( base.matrix().template cast<T>() )
{
}

/// Construct from a generic homography -- double specialization
template <>
homography_<double>
::homography_( homography const &base )
  : h_( base.matrix() )
{
}

/// Create a clone of outself as a shared pointer
template <typename T>
homography_sptr
homography_<T>
::clone() const
{
  return homography_sptr( new homography_<T>( *this ) );
}

/// Get a double-typed copy of the underlying matrix transformation
template <typename T>
Eigen::Matrix<double,3,3>
homography_<T>
::matrix() const
{
  return this->h_.template cast<double>();
}

/// Specialization for homographies with native double type
template <>
Eigen::Matrix<double,3,3>
homography_<double>
::matrix() const
{
  return this->h_;
}

/// Normalize homography transformation in-place
template <typename T>
homography_sptr
homography_<T>
::normalize() const
{
  matrix_t norm = this->get_matrix();
  if( fabs(norm(2,2)) >= Eigen::NumTraits<T>::dummy_precision() )
  {
    norm /= norm(2,2);
  }
  return homography_sptr( new homography_<T>( norm ) );
}

/// Inverse the homography transformation returning a new transformation
template <typename T>
homography_sptr
homography_<T>
::inverse() const
{
  matrix_t inv;
  bool isvalid;
  this->h_.computeInverseWithCheck( inv, isvalid );
  if( !isvalid )
  {
    throw non_invertible_matrix();
  }
  return homography_sptr( new homography_<T>( inv ) );
}

/// Map a 2D double-type point using this homography
template <typename T>
Eigen::Matrix<double,2,1>
homography_<T>
::map( Eigen::Matrix<double,2,1> const &p ) const
{
  // Explicitly refer to templated version of method so as to not infinitely
  // recurse.
  Eigen::Matrix<double,3,3> m = h_.template cast<double>();
  return h_map_point( m, p );
}

/// Map a 2D double-type point using this homography -- double specialization
template <>
Eigen::Matrix<double,2,1>
homography_<double>
::map( Eigen::Matrix<double,2,1> const &p ) const
{
  return h_map_point( h_, p );
}

/// Get the underlying matrix transformation
template <typename T>
typename homography_<T>::matrix_t&
homography_<T>
::get_matrix()
{
  return this->h_;
}

/// Get a const new copy of the underlying matrix transformation.
template <typename T>
typename homography_<T>::matrix_t const&
homography_<T>
::get_matrix() const
{
  return this->h_;
}

/// Map a 2D point using this homography -- generic version
template <typename T>
Eigen::Matrix<T,2,1>
homography_<T>
::map_point( Eigen::Matrix<T,2,1> const &p ) const
{
  return h_map_point<T>( h_.template cast<T>(), p );
}

/// Map a 2D point using this homography -- float specialization
template <>
Eigen::Matrix<float,2,1>
homography_<float>
::map_point( Eigen::Matrix<float,2,1> const &p ) const
{
  return h_map_point( h_, p );
}

/// Map a 2D point using this homography -- double specialization
template <>
Eigen::Matrix<double,2,1>
homography_<double>
::map_point( Eigen::Matrix<double,2,1> const &p ) const
{
  return h_map_point( h_, p );
}

/// Custom f2f_homography multiplication operator.
template <typename T>
homography_<T>
homography_<T>
::operator*( homography_<T> const &rhs )
{
  return homography_<T>( h_ * rhs.h_ );
}

// ===========================================================================
// Other Functions
// ---------------------------------------------------------------------------

/// homography_<T> output stream operator
template <typename T>
std::ostream&
operator<<( std::ostream &s, homography_<T> const &h )
{
  s << h.get_matrix();
  return s;
}

/// Output stream operator for \p homography instances
std::ostream&
operator<<( std::ostream &s, homography const &h )
{
  s << h.matrix();
  return s;
}

// ===========================================================================
// Template class instantiation
// ---------------------------------------------------------------------------
/// \cond DoxygenSuppress
#define INSTANTIATE_HOMOGRAPHY(T) \
  template class homography_<T>; \
  template std::ostream& operator<<( std::ostream &, \
                                     homography_<T> const & )

INSTANTIATE_HOMOGRAPHY(float);
INSTANTIATE_HOMOGRAPHY(double);
#undef INSTANTIATE_HOMOGRAPHY
/// \endcond

} // end maptk namespace


/*! \class EikonalSolver
    \brief Abstract class that serves as interface for the actual EikonalSolvers implemented.
    It requires (at least) the computeInternal method to be implemented,

    It uses as a main container the nDGridMap class. The nDGridMap template paramenter
    has to be an FMCell or something inherited from it.

    Copyright (C) 2015 Javier V. Gomez
    www.javiervgomez.com

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.
    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef EIKONALSOLVER_H_
#define EIKONALSOLVER_H_

#include <iostream>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <fstream>
#include <array>
#include <chrono>

#include <boost/concept_check.hpp>

#include <fast_methods/fm/solver.hpp>
#include <fast_methods/console/console.h>

template <class grid_t>
class EikonalSolver : public Solver<grid_t>{

    public:
        EikonalSolver() : Solver<grid_t>("EikonalSolver") {}
        EikonalSolver(const std::string& name) : Solver<grid_t>(name) {}

        /** \brief Solves nD Eikonal equation for cell idx. If heuristics are activated, it will add
            the estimated travel time to goal with current velocity. */
        virtual double solveEikonal(const int & idx)
        {
            unsigned int a = grid_t::getNDims(); // a parameter of the Eikonal equation.
            Tvalues_.clear();

            for (unsigned int dim = 0; dim < grid_t::getNDims(); ++dim) {
                double minTInDim = grid_->getMinValueInDim(idx, dim);
                if (!std::isinf(minTInDim) && minTInDim < grid_->getCell(idx).getArrivalTime())
                    Tvalues_.push_back(minTInDim);
                else
                    a -=1;
            }

            if (a == 0)
                return std::numeric_limits<double>::infinity();

            // Sort the neighbor values to make easy the following code.
            /// \todo given that this sorts a small vector, a n^2 methods could be better. Test it.
            std::sort(Tvalues_.begin(), Tvalues_.end());
            double updatedT;
            for (unsigned i = 1; i <= a; ++i) {
                updatedT = solveEikonalNDims(idx, i);
                // If no more dimensions or increasing one dimension will not improve time.
                if (i == a || (updatedT - Tvalues_[i]) < utils::COMP_MARGIN)
                    break;
            }
            return updatedT;
        }

    protected:
        /** \brief Solves the Eikonal equation assuming that Tvalues_
            is sorted. */
        double solveEikonalNDims
        (unsigned int idx, unsigned int dim) {
            // Solve for 1 dimension.
            if (dim == 1)
                return Tvalues_[0] + grid_->getLeafSize() / grid_->getCell(idx).getVelocity();

            // Solve for any number > 1 of dimensions.
            double sumT = 0;
            double sumTT = 0;
            for (unsigned i = 0; i < dim; ++i) {
                sumT += Tvalues_[i];
                sumTT += Tvalues_[i]*Tvalues_[i];
            }

            // These a,b,c values are simplified since leafsize^2, which should be present in the three
            // terms but they are cancelled out when solving the quadratic function.
            double a = dim;
            double b = -2*sumT;
            double c = sumTT - grid_->getLeafSize() * grid_->getLeafSize() / (grid_->getCell(idx).getVelocity()*grid_->getCell(idx).getVelocity());
            double quad_term = b*b - 4*a*c;

            if (quad_term < 0)
                return std::numeric_limits<double>::infinity();
            else
                return (-b + sqrt(quad_term))/(2*a);
        }

        /** \brief Auxiliar vector with values T0,T1...Tn-1 variables in the Discretized Eikonal Equation. */
        std::vector<double>          Tvalues_;

        /** \brief Auxiliar array which stores the neighbor of each iteration of the computeFM() function. */
        std::array <unsigned int, 2*grid_t::getNDims()> neighbors_;

        using Solver<grid_t>::grid_;
};

#endif /* EIKONALSOLVER_H_*/


/*
 * This file is part of the statismo library.
 *
 * Author: Marcel Luethi (marcel.luethi@unibas.ch)
 *
 * Copyright (c) 2011 University of Basel
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the project's author 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.
 *
 */

#ifndef __PosteriorModelBuilder_hxx
#define __PosteriorModelBuilder_hxx

#include "PosteriorModelBuilder.h"

#include <iostream>

#include <Eigen/SVD>

#include "CommonTypes.h"
#include "PCAModelBuilder.h"

namespace statismo {

//
// PosteriorModelBuilder
//
//

template <typename T>
PosteriorModelBuilder<T>::PosteriorModelBuilder()
    : Superclass() {
}

template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModel(
    const DataItemListType& sampleDataList,
    const PointValueListType& pointValues,
    double pointValuesNoiseVariance,
    double noiseVariance) const {
    return BuildNewModel(sampleDataList, TrivialPointValueWithCovarianceListWithUniformNoise(pointValues, pointValuesNoiseVariance), noiseVariance);
}

template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModelFromModel(
    const StatisticalModelType* inputModel,
    const PointValueListType& pointValues,
    double pointValuesNoiseVariance,
    bool computeScores) const {

    return BuildNewModelFromModel(inputModel, TrivialPointValueWithCovarianceListWithUniformNoise(pointValues,pointValuesNoiseVariance), computeScores);

}

template <typename T>
typename PosteriorModelBuilder<T>::PointValueWithCovarianceListType
PosteriorModelBuilder<T>::TrivialPointValueWithCovarianceListWithUniformNoise(
    const PointValueListType& pointValues, double pointValueNoiseVariance) const {

    const MatrixType pointCovarianceMatrix = pointValueNoiseVariance * MatrixType::Identity(3,3);
    PointValueWithCovarianceListType pvcList;//(pointValues.size());

    for (typename PointValueListType::const_iterator it = pointValues.begin(); it != pointValues.end(); ++it) {
        pvcList.push_back(PointValueWithCovariancePairType(*it,pointCovarianceMatrix));
    }

    return pvcList;

}

template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModel(
    const DataItemListType& sampleDataList,
    const PointValueWithCovarianceListType& pointValuesWithCovariance,
    double noiseVariance) const {
    typedef PCAModelBuilder<T> PCAModelBuilderType;
    PCAModelBuilderType* modelBuilder = PCAModelBuilderType::Create();
    StatisticalModelType* model = modelBuilder->BuildNewModel(sampleDataList, noiseVariance);
    StatisticalModelType* PosteriorModel = BuildNewModelFromModel(model, pointValuesWithCovariance, noiseVariance);
    delete modelBuilder;
    delete model;
    return PosteriorModel;
}

template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModelFromModel(
    const StatisticalModelType* inputModel,
    const PointValueWithCovarianceListType& pointValuesWithCovariance,
    bool computeScores) const {

    typedef statismo::Representer<T> RepresenterType;

    const RepresenterType* representer = inputModel->GetRepresenter();

    // The naming of the variables correspond to those used in the paper
    // Posterior Shape Models,
    // Thomas Albrecht, Marcel Luethi, Thomas Gerig, Thomas Vetter
    //
    const MatrixType& Q =  inputModel->GetPCABasisMatrix();
    const VectorType& mu = inputModel->GetMeanVector();

    // this method only makes sense for a proper PPCA model (e.g. the noise term is properly defined)
    // if the model has zero noise, we assume a small amount of noise
    double rho2 = std::max((double) inputModel->GetNoiseVariance(), (double) Superclass::TOLERANCE);

    unsigned dim = representer->GetDimensions();

    // build the part matrices with , considering only the points that are fixed
    //
    unsigned numPrincipalComponents = inputModel->GetNumberOfPrincipalComponents();
    MatrixType Q_g(pointValuesWithCovariance.size()* dim, numPrincipalComponents);
    VectorType mu_g(pointValuesWithCovariance.size() * dim);
    VectorType s_g(pointValuesWithCovariance.size() * dim);

    MatrixType LQ_g(pointValuesWithCovariance.size()* dim, numPrincipalComponents);

    unsigned i = 0;
    for (typename PointValueWithCovarianceListType::const_iterator it = pointValuesWithCovariance.begin(); it != pointValuesWithCovariance.end(); ++it) {
        VectorType val = representer->PointSampleToPointSampleVector(it->first.second);
        unsigned pt_id = representer->GetPointIdForPoint(it->first.first);

        // In the formulas, we actually need the precision matrix, which is the inverse of the covariance.
        const MatrixType pointPrecisionMatrix = it->second.inverse();

        // Get the three rows pertaining to this point:
        const MatrixType Qrows_for_pt_id = Q.block(pt_id * dim, 0, dim, numPrincipalComponents);

        Q_g.block(i * dim, 0, dim, numPrincipalComponents) = Qrows_for_pt_id;
        mu_g.block(i * dim, 0, dim, 1) = mu.block(pt_id * dim, 0, dim, 1);
        s_g.block(i * dim, 0, dim, 1) = val;

        LQ_g.block(i * dim, 0, dim, numPrincipalComponents) = pointPrecisionMatrix * Qrows_for_pt_id;
        i++;
    }

    VectorType D2 = inputModel->GetPCAVarianceVector().array();

    const MatrixType& Q_gT = Q_g.transpose();

    MatrixType M = Q_gT * LQ_g;
    M.diagonal() += VectorType::Ones(Q_g.cols());

    MatrixTypeDoublePrecision Minv = M.cast<double>().inverse();

    // the MAP solution for the latent variables (coefficients)
    VectorType coeffs = Minv.cast<ScalarType>() * LQ_g.transpose() * (s_g - mu_g);

    // the MAP solution in the sample space
    VectorType mu_c = inputModel->GetRepresenter()->SampleToSampleVector(inputModel->DrawSample(coeffs));

    const VectorType& pcaVariance = inputModel->GetPCAVarianceVector();
    VectorTypeDoublePrecision pcaSdev = pcaVariance.cast<double>().array().sqrt();

    VectorType D2MinusRho = D2 - VectorType::Ones(D2.rows()) * rho2;
    // the values of D2 can be negative. We need to be careful when taking the root
    for (unsigned i = 0; i < D2MinusRho.rows(); i++) {
        D2MinusRho(i) = std::max((ScalarType) 0, D2(i));
    }
    VectorType D2MinusRhoSqrt = D2MinusRho.array().sqrt();

    typedef Eigen::JacobiSVD<MatrixTypeDoublePrecision> SVDType;
    MatrixTypeDoublePrecision innerMatrix = D2MinusRhoSqrt.cast<double>().asDiagonal() * Minv * D2MinusRhoSqrt.cast<double>().asDiagonal();
    SVDType svd(innerMatrix, Eigen::ComputeThinU);

    // SVD of the inner matrix
    VectorType D_c = svd.singularValues().cast<ScalarType>();

    // Todo: Maybe it is possible to do this with Q, so that we don"t need to get U as well.
    MatrixType U_c = inputModel->GetOrthonormalPCABasisMatrix() * svd.matrixU().cast<ScalarType>();

    StatisticalModelType* PosteriorModel = StatisticalModelType::Create(representer , mu_c, U_c, D_c, rho2);

    // Write the parameters used to build the models into the builderInfo

    typename ModelInfo::BuilderInfoList builderInfoList = inputModel->GetModelInfo().GetBuilderInfoList();

    BuilderInfo::ParameterInfoList bi;
    bi.push_back(BuilderInfo::KeyValuePair("NoiseVariance ", Utils::toString(rho2)));
    bi.push_back(BuilderInfo::KeyValuePair("FixedPointsVariance ", Utils::toString(0.2)));
//
    BuilderInfo::DataInfoList di;

    unsigned pt_no = 0;
    for (typename PointValueWithCovarianceListType::const_iterator it = pointValuesWithCovariance.begin(); it != pointValuesWithCovariance.end(); ++it) {
        VectorType val = representer->PointSampleToPointSampleVector(it->first.second);

        // TODO we looked up the PointId for the same point before. Having it here again is inefficient.
        unsigned pt_id = representer->GetPointIdForPoint(it->first.first);
        std::ostringstream keySStream;
        keySStream << "Point constraint " << pt_no;
        std::ostringstream valueSStream;
        valueSStream << "(" << pt_id << ", (";

        for (unsigned d = 0; d < dim - 1; d++) {
            valueSStream << val[d] << ",";
        }
        valueSStream << val[dim -1];
        valueSStream << "))";
        di.push_back(BuilderInfo::KeyValuePair(keySStream.str(), valueSStream.str()));
        pt_no++;
    }

    BuilderInfo builderInfo("PosteriorModelBuilder", di, bi);
    builderInfoList.push_back(builderInfo);

    MatrixType inputScores = inputModel->GetModelInfo().GetScoresMatrix();
    MatrixType scores = MatrixType::Zero(inputScores.rows(), inputScores.cols());

    if (computeScores == true) {

        // get the scores from the input model
        for (unsigned i = 0; i < inputScores.cols(); i++) {
            // reconstruct the sample from the input model and project it back into the model
            typename RepresenterType::DatasetPointerType ds = inputModel->DrawSample(inputScores.col(i));
            scores.col(i) = PosteriorModel->ComputeCoefficientsForDataset(ds);
            representer->DeleteDataset(ds);
        }
    }
    ModelInfo info(scores, builderInfoList);
    PosteriorModel->SetModelInfo(info);

    return PosteriorModel;

}

} // namespace statismo

#endif


/**
 * Author	: Michael Fonder
 * Year		: 2016
 **/

#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Sparse>

#include <unordered_map>
#include <unordered_set>
#include <set>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/video.hpp>

#include "dynamicVectorContainer.hpp"
#include "dynamicVectorReference.hpp"
#include "imuState.hpp"
#include "MSCKF.hpp"

#ifndef _CRT_SECURE_NO_WARNINGS
# define _CRT_SECURE_NO_WARNINGS
#endif

using namespace cv;
using namespace std;

int main(int argc, char* argv[])
{
	FileStorage fs;
	Mat distCoeffs;
	Mat cameraMatrix;

	//******************************************************************************

	string filename = "distCoeffs";
	fs.open(filename+".yml", FileStorage::READ);
	fs[filename] >> distCoeffs;

	filename = "cameraMatrix";
	fs.open(filename+".yml", FileStorage::READ);
	fs[filename] >> cameraMatrix;

	filename = "featurePos";
	fs.open(filename+".yml", FileStorage::READ);
	Mat featurePos;
	fs[filename] >> featurePos;
	cout <<featurePos  << endl;

	filename = "data";
	fs.open(filename+".yml", FileStorage::READ);
	Mat data;
	fs[filename] >> data;
	cout << filename+".yml" << endl;
	data = data.t();

	Mat ba, bg, IMU_noise, p_CI, q_CI, MSCKF_params, init_uncertainty;
	filename = "parameters";
	fs.open(filename+".yml", FileStorage::READ);
	fs["ba"] >> ba;
	fs["bg"] >> bg;
	fs["IMU_noise"] >> IMU_noise;
	fs["init_uncertainty"] >> init_uncertainty;
	fs["p_CI"] >> p_CI;
	fs["q_CI"] >> q_CI;
	fs["MSCKF_params"] >> MSCKF_params;
	cout << filename+".yml" << endl;
	cout << ba << bg << IMU_noise << p_CI << q_CI << MSCKF_params << endl;

	Measurement newMeasurement;
	IMUstate imustate;
	cout << "datatestbegin *********************************" << endl;

	VideoCapture capture("flight_cut.avi"); // attempt to open it as a video file or image sequence
//	VideoCapture capture("test_syn.avi"); // attempt to open it as a video file or image sequence

	if (!capture.isOpened()){  // returns true if video capturing has been initialized already
		cerr << "Failed to open the video file or image sequence...\n" << endl;
		return 1;
	}
	Mat frame;
	bool finished = false;
	size_t timeIMU=0, timeVideo=0;
	MSCKF filter;
	filter.trueFeaturesPos = featurePos;
	filter.camera.distCoeffs = distCoeffs;
	filter.camera.intrisic = cameraMatrix;
	filter.setCameraParams(p_CI, q_CI);
	filter.setFilterParams(MSCKF_params);
	ba.copyTo(filter.imustate->ba);
	bg.copyTo(filter.imustate->bg);
	filter.imustate->setQ(IMU_noise);
	filter.imustate->resetCovar(init_uncertainty);
	capture >> frame;

	Mat log_p = Mat::zeros(1,6,CV_32FC1);
	Mat local = Mat::zeros(1,6,CV_32FC1);

	Mat log_q = Mat::zeros(1,7,CV_32FC1);
	Mat localq = Mat::zeros(1,7,CV_32FC1);

	while(!finished)
	{
		capture >> frame;
		if (frame.empty())
		{
			finished = true;
			continue;
		}
// 		if(timeVideo > 150)
// 			break;

		while(float(timeIMU)*0.01 <= float(timeVideo)/30)
		{
//			data.rowRange(6,9).col(timeIMU).copyTo(filter.truePos);
//			data.rowRange(9,13).col(timeIMU).copyTo(filter.trueQuat);

			data.rowRange(0,3).col(timeIMU).copyTo(newMeasurement.a_I);
			//newMeasurement.a_I.row(0) *= -1;
			data.rowRange(3,6).col(timeIMU).copyTo(newMeasurement.omega);
			//newMeasurement.omega.rowRange(0,2)*=-1.0;
			filter.propagateIMUStateAndCovar(newMeasurement, float(timeIMU)*0.01);
			local.colRange(0,3) = filter.imustate->p_G.t();
			local.at<float>(0,3) = 	filter.imustate->covar.at<float>(12,12);
			local.at<float>(0,4) = 	filter.imustate->covar.at<float>(13,13);
			local.at<float>(0,5) = 	filter.imustate->covar.at<float>(14,14);
			log_p.push_back(local);

			localq.colRange(0,4) = filter.imustate->q_IG.t();
			localq.at<float>(0,4) = 	filter.imustate->covar.at<float>(0,0);
			localq.at<float>(0,5) = 	filter.imustate->covar.at<float>(1,1);
			localq.at<float>(0,6) = 	filter.imustate->covar.at<float>(2,2);
			log_q.push_back(localq);
			++timeIMU;
		}

		cvtColor(frame, frame, CV_BGR2GRAY);
		if(!(timeVideo%2))
		{
			cout << "augment " << timeVideo << endl;
			filter.augmentState(frame, float(timeVideo)/30);
			cout << "correct " << timeVideo << endl;
			filter.update();
		}
		++timeVideo;
		FileStorage logFilep("log_p.yml", FileStorage::WRITE);
		logFilep << "log_p" << log_p;
		FileStorage logFileq("log_q.yml", FileStorage::WRITE);
		logFileq << "log_q" << log_q;
		FileStorage covar_IMU("covar_IMU.yml", FileStorage::WRITE);
		covar_IMU << "covar_IMU" << filter.imustate->covar;

		cout << "Estimated attitude : " << filter.getOrientation().t() << endl;
		cout << "Estimated position : " << filter.getPosition().t() << endl;

	}

	cout << filter.getPosition() << endl;

    return 0;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"  // Precompiled headers

#include <mrpt/core/round.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/vision/CDifodo.h>
#include <Eigen/Dense>
#include <iostream>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::math;
using namespace std;
using namespace Eigen;
using mrpt::round;
using mrpt::square;

CDifodo::CDifodo()
{
	rows = 60;
	cols = 80;
	fovh = M_PIf * 58.6f / 180.0f;
	fovv = M_PIf * 45.6f / 180.0f;
	cam_mode = 1;  // (1 - 640 x 480, 2 - 320 x 240, 4 - 160 x 120)
	downsample = 1;
	ctf_levels = 1;
	m_width = 640 / (cam_mode * downsample);
	m_height = 480 / (cam_mode * downsample);
	fast_pyramid = true;

	// Resize pyramid
	const unsigned int pyr_levels =
		round(log(float(m_width / cols)) / log(2.f)) + ctf_levels;
	depth.resize(pyr_levels);
	depth_old.resize(pyr_levels);
	depth_inter.resize(pyr_levels);
	depth_warped.resize(pyr_levels);
	xx.resize(pyr_levels);
	xx_inter.resize(pyr_levels);
	xx_old.resize(pyr_levels);
	xx_warped.resize(pyr_levels);
	yy.resize(pyr_levels);
	yy_inter.resize(pyr_levels);
	yy_old.resize(pyr_levels);
	yy_warped.resize(pyr_levels);
	transformations.resize(pyr_levels);

	for (unsigned int i = 0; i < pyr_levels; i++)
	{
		unsigned int s = pow(2.f, int(i));
		cols_i = m_width / s;
		rows_i = m_height / s;
		depth[i].resize(rows_i, cols_i);
		depth_inter[i].resize(rows_i, cols_i);
		depth_old[i].resize(rows_i, cols_i);
		depth[i].fill(0.0f);
		depth_old[i].fill(0.0f);
		xx[i].resize(rows_i, cols_i);
		xx_inter[i].resize(rows_i, cols_i);
		xx_old[i].resize(rows_i, cols_i);
		xx[i].fill(0.0f);
		xx_old[i].fill(0.0f);
		yy[i].resize(rows_i, cols_i);
		yy_inter[i].resize(rows_i, cols_i);
		yy_old[i].resize(rows_i, cols_i);
		yy[i].fill(0.0f);
		yy_old[i].fill(0.0f);
		transformations[i].resize(4, 4);

		if (cols_i <= cols)
		{
			depth_warped[i].resize(rows_i, cols_i);
			xx_warped[i].resize(rows_i, cols_i);
			yy_warped[i].resize(rows_i, cols_i);
		}
	}

	depth_wf.setSize(m_height, m_width);

	fps = 30.f;  // In Hz

	previous_speed_const_weight = 0.05f;
	previous_speed_eig_weight = 0.5f;
	kai_loc_old = TTwist3D();
	num_valid_points = 0;

	// Compute gaussian mask
	VectorXf v_mask(4);
	v_mask(0) = 1.f;
	v_mask(1) = 2.f;
	v_mask(2) = 2.f;
	v_mask(3) = 1.f;
	for (unsigned int i = 0; i < 4; i++)
		for (unsigned int j = 0; j < 4; j++)
			f_mask(i, j) = v_mask(i) * v_mask(j) / 36.f;

	// Compute gaussian mask
	float v_mask2[5] = {1, 4, 6, 4, 1};
	for (unsigned int i = 0; i < 5; i++)
		for (unsigned int j = 0; j < 5; j++)
			g_mask[i][j] = v_mask2[i] * v_mask2[j] / 256.f;
}

void CDifodo::buildCoordinatesPyramid()
{
	const float max_depth_dif = 0.1f;

	// Push coordinates back
	depth_old.swap(depth);
	xx_old.swap(xx);
	yy_old.swap(yy);

	// The number of levels of the pyramid does not match the number of levels
	// used
	// in the odometry computation (because we might want to finish with lower
	// resolutions)

	unsigned int pyr_levels =
		round(log(float(m_width / cols)) / log(2.f)) + ctf_levels;

	// Generate levels
	for (unsigned int i = 0; i < pyr_levels; i++)
	{
		unsigned int s = pow(2.f, int(i));
		cols_i = m_width / s;
		rows_i = m_height / s;
		const int rows_i2 = 2 * rows_i;
		const int cols_i2 = 2 * cols_i;
		const int i_1 = i - 1;

		if (i == 0) depth[i].swap(depth_wf);

		//                              Downsampling
		//-----------------------------------------------------------------------------
		else
		{
			for (unsigned int u = 0; u < cols_i; u++)
				for (unsigned int v = 0; v < rows_i; v++)
				{
					const int u2 = 2 * u;
					const int v2 = 2 * v;
					const float dcenter = depth[i_1](v2, u2);

					// Inner pixels
					if ((v > 0) && (v < rows_i - 1) && (u > 0) &&
						(u < cols_i - 1))
					{
						if (dcenter > 0.f)
						{
							float sum = 0.f;
							float weight = 0.f;

							for (int l = -2; l < 3; l++)
								for (int k = -2; k < 3; k++)
								{
									const float abs_dif = abs(
										depth[i_1](v2 + k, u2 + l) - dcenter);
									if (abs_dif < max_depth_dif)
									{
										const float aux_w =
											g_mask[2 + k][2 + l] *
											(max_depth_dif - abs_dif);
										weight += aux_w;
										sum +=
											aux_w * depth[i_1](v2 + k, u2 + l);
									}
								}
							depth[i](v, u) = sum / weight;
						}
						else
						{
							float min_depth = 10.f;
							for (int l = -2; l < 3; l++)
								for (int k = -2; k < 3; k++)
								{
									const float d = depth[i_1](v2 + k, u2 + l);
									if ((d > 0.f) && (d < min_depth))
										min_depth = d;
								}

							if (min_depth < 10.f)
								depth[i](v, u) = min_depth;
							else
								depth[i](v, u) = 0.f;
						}
					}

					// Boundary
					else
					{
						if (dcenter > 0.f)
						{
							float sum = 0.f;
							float weight = 0.f;

							for (int l = -2; l < 3; l++)
								for (int k = -2; k < 3; k++)
								{
									const int indv = v2 + k, indu = u2 + l;
									if ((indv >= 0) && (indv < rows_i2) &&
										(indu >= 0) && (indu < cols_i2))
									{
										const float abs_dif = abs(
											depth[i_1](indv, indu) - dcenter);
										if (abs_dif < max_depth_dif)
										{
											const float aux_w =
												g_mask[2 + k][2 + l] *
												(max_depth_dif - abs_dif);
											weight += aux_w;
											sum +=
												aux_w * depth[i_1](indv, indu);
										}
									}
								}
							depth[i](v, u) = sum / weight;
						}
						else
						{
							float min_depth = 10.f;
							for (int l = -2; l < 3; l++)
								for (int k = -2; k < 3; k++)
								{
									const int indv = v2 + k, indu = u2 + l;
									if ((indv >= 0) && (indv < rows_i2) &&
										(indu >= 0) && (indu < cols_i2))
									{
										const float d = depth[i_1](indv, indu);
										if ((d > 0.f) && (d < min_depth))
											min_depth = d;
									}
								}

							if (min_depth < 10.f)
								depth[i](v, u) = min_depth;
							else
								depth[i](v, u) = 0.f;
						}
					}
				}
		}

		// Calculate coordinates "xy" of the points
		const float inv_f_i = 2.f * tan(0.5f * fovh) / float(cols_i);
		const float disp_u_i = 0.5f * (cols_i - 1);
		const float disp_v_i = 0.5f * (rows_i - 1);

		for (unsigned int u = 0; u < cols_i; u++)
			for (unsigned int v = 0; v < rows_i; v++)
				if (depth[i](v, u) > 0.f)
				{
					xx[i](v, u) = (u - disp_u_i) * depth[i](v, u) * inv_f_i;
					yy[i](v, u) = (v - disp_v_i) * depth[i](v, u) * inv_f_i;
				}
				else
				{
					xx[i](v, u) = 0.f;
					yy[i](v, u) = 0.f;
				}
	}
}

void CDifodo::buildCoordinatesPyramidFast()
{
	const float max_depth_dif = 0.1f;

	// Push coordinates back
	depth_old.swap(depth);
	xx_old.swap(xx);
	yy_old.swap(yy);

	// The number of levels of the pyramid does not match the number of levels
	// used
	// in the odometry computation (because we might want to finish with lower
	// resolutions)

	unsigned int pyr_levels =
		round(log(float(m_width / cols)) / log(2.f)) + ctf_levels;

	// Generate levels
	for (unsigned int i = 0; i < pyr_levels; i++)
	{
		unsigned int s = pow(2.f, int(i));
		cols_i = m_width / s;
		rows_i = m_height / s;
		// const int rows_i2 = 2*rows_i;
		// const int cols_i2 = 2*cols_i;
		const int i_1 = i - 1;

		if (i == 0) depth[i].swap(depth_wf);

		//                              Downsampling
		//-----------------------------------------------------------------------------
		else
		{
			for (unsigned int u = 0; u < cols_i; u++)
				for (unsigned int v = 0; v < rows_i; v++)
				{
					const int u2 = 2 * u;
					const int v2 = 2 * v;

					// Inner pixels
					if ((v > 0) && (v < rows_i - 1) && (u > 0) &&
						(u < cols_i - 1))
					{
						const Matrix4f d_block =
							depth[i_1].block<4, 4>(v2 - 1, u2 - 1);
						float depths[4] = {d_block(5), d_block(6), d_block(9),
										   d_block(10)};
						float dcenter;

						// Sort the array (try to find a good/representative
						// value)
						for (signed char k = 2; k >= 0; k--)
							if (depths[k + 1] < depths[k])
								std::swap(depths[k + 1], depths[k]);
						for (unsigned char k = 1; k < 3; k++)
							if (depths[k] > depths[k + 1])
								std::swap(depths[k + 1], depths[k]);
						if (depths[2] < depths[1])
							dcenter = depths[1];
						else
							dcenter = depths[2];

						if (dcenter > 0.f)
						{
							float sum = 0.f;
							float weight = 0.f;

							for (unsigned char k = 0; k < 16; k++)
							{
								const float abs_dif = abs(d_block(k) - dcenter);
								if (abs_dif < max_depth_dif)
								{
									const float aux_w =
										f_mask[k] * (max_depth_dif - abs_dif);
									weight += aux_w;
									sum += aux_w * d_block(k);
								}
							}
							if (weight > 0) depth[i](v, u) = sum / weight;
						}
						else
							depth[i](v, u) = 0.f;
					}

					// Boundary
					else
					{
						const Matrix2f d_block = depth[i_1].block<2, 2>(v2, u2);
						const float new_d = 0.25f * d_block.array().sum();
						if (new_d < 0.4f)
							depth[i](v, u) = 0.f;
						else
							depth[i](v, u) = new_d;
					}
				}
		}

		// Calculate coordinates "xy" of the points
		const float inv_f_i = 2.f * tan(0.5f * fovh) / float(cols_i);
		const float disp_u_i = 0.5f * (cols_i - 1);
		const float disp_v_i = 0.5f * (rows_i - 1);

		for (unsigned int u = 0; u < cols_i; u++)
			for (unsigned int v = 0; v < rows_i; v++)
				if (depth[i](v, u) > 0.f)
				{
					xx[i](v, u) = (u - disp_u_i) * depth[i](v, u) * inv_f_i;
					yy[i](v, u) = (v - disp_v_i) * depth[i](v, u) * inv_f_i;
				}
				else
				{
					xx[i](v, u) = 0.f;
					yy[i](v, u) = 0.f;
				}
	}
}

void CDifodo::performWarping()
{
	// Camera parameters (which also depend on the level resolution)
	const float f = float(cols_i) / (2.f * tan(0.5f * fovh));
	const float disp_u_i = 0.5f * float(cols_i - 1);
	const float disp_v_i = 0.5f * float(rows_i - 1);

	// Rigid transformation estimated up to the present level
	Matrix4f acu_trans;
	acu_trans.setIdentity();
	for (unsigned int i = 1; i <= level; i++)
		acu_trans = transformations[i - 1].asEigen() * acu_trans;

	MatrixXf wacu(rows_i, cols_i);
	wacu.fill(0.f);
	depth_warped[image_level].fill(0.f);

	const auto cols_lim = float(cols_i - 1);
	const auto rows_lim = float(rows_i - 1);

	//						Warping loop
	//---------------------------------------------------------
	for (unsigned int j = 0; j < cols_i; j++)
		for (unsigned int i = 0; i < rows_i; i++)
		{
			const float z = depth[image_level](i, j);

			if (z > 0.f)
			{
				// Transform point to the warped reference frame
				const float depth_w = acu_trans(0, 0) * z +
									  acu_trans(0, 1) * xx[image_level](i, j) +
									  acu_trans(0, 2) * yy[image_level](i, j) +
									  acu_trans(0, 3);
				const float x_w = acu_trans(1, 0) * z +
								  acu_trans(1, 1) * xx[image_level](i, j) +
								  acu_trans(1, 2) * yy[image_level](i, j) +
								  acu_trans(1, 3);
				const float y_w = acu_trans(2, 0) * z +
								  acu_trans(2, 1) * xx[image_level](i, j) +
								  acu_trans(2, 2) * yy[image_level](i, j) +
								  acu_trans(2, 3);

				// Calculate warping
				const float uwarp = f * x_w / depth_w + disp_u_i;
				const float vwarp = f * y_w / depth_w + disp_v_i;

				// The warped pixel (which is not integer in general)
				// contributes to all the surrounding ones
				if ((uwarp >= 0.f) && (uwarp < cols_lim) && (vwarp >= 0.f) &&
					(vwarp < rows_lim))
				{
					const int uwarp_l = uwarp;
					const int uwarp_r = uwarp_l + 1;
					const int vwarp_d = vwarp;
					const int vwarp_u = vwarp_d + 1;
					const float delta_r = float(uwarp_r) - uwarp;
					const float delta_l = uwarp - float(uwarp_l);
					const float delta_u = float(vwarp_u) - vwarp;
					const float delta_d = vwarp - float(vwarp_d);

					// Warped pixel very close to an integer value
					if (abs(round(uwarp) - uwarp) + abs(round(vwarp) - vwarp) <
						0.05f)
					{
						depth_warped[image_level](round(vwarp), round(uwarp)) +=
							depth_w;
						wacu(round(vwarp), round(uwarp)) += 1.f;
					}
					else
					{
						const float w_ur = square(delta_l) + square(delta_d);
						depth_warped[image_level](vwarp_u, uwarp_r) +=
							w_ur * depth_w;
						wacu(vwarp_u, uwarp_r) += w_ur;

						const float w_ul = square(delta_r) + square(delta_d);
						depth_warped[image_level](vwarp_u, uwarp_l) +=
							w_ul * depth_w;
						wacu(vwarp_u, uwarp_l) += w_ul;

						const float w_dr = square(delta_l) + square(delta_u);
						depth_warped[image_level](vwarp_d, uwarp_r) +=
							w_dr * depth_w;
						wacu(vwarp_d, uwarp_r) += w_dr;

						const float w_dl = square(delta_r) + square(delta_u);
						depth_warped[image_level](vwarp_d, uwarp_l) +=
							w_dl * depth_w;
						wacu(vwarp_d, uwarp_l) += w_dl;
					}
				}
			}
		}

	// Scale the averaged depth and compute spatial coordinates
	const float inv_f_i = 1.f / f;
	for (unsigned int u = 0; u < cols_i; u++)
		for (unsigned int v = 0; v < rows_i; v++)
		{
			if (wacu(v, u) > 0.f)
			{
				depth_warped[image_level](v, u) /= wacu(v, u);
				xx_warped[image_level](v, u) =
					(u - disp_u_i) * depth_warped[image_level](v, u) * inv_f_i;
				yy_warped[image_level](v, u) =
					(v - disp_v_i) * depth_warped[image_level](v, u) * inv_f_i;
			}
			else
			{
				depth_warped[image_level](v, u) = 0.f;
				xx_warped[image_level](v, u) = 0.f;
				yy_warped[image_level](v, u) = 0.f;
			}
		}
}

void CDifodo::calculateCoord()
{
	null.resize(rows_i, cols_i);
	null.fill(false);
	num_valid_points = 0;

	for (unsigned int u = 0; u < cols_i; u++)
		for (unsigned int v = 0; v < rows_i; v++)
		{
			if ((depth_old[image_level](v, u)) == 0.f ||
				(depth_warped[image_level](v, u) == 0.f))
			{
				depth_inter[image_level](v, u) = 0.f;
				xx_inter[image_level](v, u) = 0.f;
				yy_inter[image_level](v, u) = 0.f;
				null(v, u) = true;
			}
			else
			{
				depth_inter[image_level](v, u) =
					0.5f * (depth_old[image_level](v, u) +
							depth_warped[image_level](v, u));
				xx_inter[image_level](v, u) =
					0.5f *
					(xx_old[image_level](v, u) + xx_warped[image_level](v, u));
				yy_inter[image_level](v, u) =
					0.5f *
					(yy_old[image_level](v, u) + yy_warped[image_level](v, u));
				null(v, u) = false;
				if ((u > 0) && (v > 0) && (u < cols_i - 1) && (v < rows_i - 1))
					num_valid_points++;
			}
		}
}

void CDifodo::calculateDepthDerivatives()
{
	dt.resize(rows_i, cols_i);
	dt.fill(0.f);
	du.resize(rows_i, cols_i);
	du.fill(0.f);
	dv.resize(rows_i, cols_i);
	dv.fill(0.f);

	// Compute connectivity
	MatrixXf rx_ninv(rows_i, cols_i);
	MatrixXf ry_ninv(rows_i, cols_i);
	rx_ninv.fill(1.f);
	ry_ninv.fill(1.f);

	for (unsigned int u = 0; u < cols_i - 1; u++)
		for (unsigned int v = 0; v < rows_i; v++)
			if (null(v, u) == false)
			{
				rx_ninv(v, u) = sqrtf(
					square(
						xx_inter[image_level](v, u + 1) -
						xx_inter[image_level](v, u)) +
					square(
						depth_inter[image_level](v, u + 1) -
						depth_inter[image_level](v, u)));
			}

	for (unsigned int u = 0; u < cols_i; u++)
		for (unsigned int v = 0; v < rows_i - 1; v++)
			if (null(v, u) == false)
			{
				ry_ninv(v, u) = sqrtf(
					square(
						yy_inter[image_level](v + 1, u) -
						yy_inter[image_level](v, u)) +
					square(
						depth_inter[image_level](v + 1, u) -
						depth_inter[image_level](v, u)));
			}

	// Spatial derivatives
	for (unsigned int v = 0; v < rows_i; v++)
	{
		for (unsigned int u = 1; u < cols_i - 1; u++)
			if (null(v, u) == false)
				du(v, u) =
					(rx_ninv(v, u - 1) * (depth_inter[image_level](v, u + 1) -
										  depth_inter[image_level](v, u)) +
					 rx_ninv(v, u) * (depth_inter[image_level](v, u) -
									  depth_inter[image_level](v, u - 1))) /
					(rx_ninv(v, u) + rx_ninv(v, u - 1));

		du(v, 0) = du(v, 1);
		du(v, cols_i - 1) = du(v, cols_i - 2);
	}

	for (unsigned int u = 0; u < cols_i; u++)
	{
		for (unsigned int v = 1; v < rows_i - 1; v++)
			if (null(v, u) == false)
				dv(v, u) =
					(ry_ninv(v - 1, u) * (depth_inter[image_level](v + 1, u) -
										  depth_inter[image_level](v, u)) +
					 ry_ninv(v, u) * (depth_inter[image_level](v, u) -
									  depth_inter[image_level](v - 1, u))) /
					(ry_ninv(v, u) + ry_ninv(v - 1, u));

		dv(0, u) = dv(1, u);
		dv(rows_i - 1, u) = dv(rows_i - 2, u);
	}

	// Temporal derivative
	for (unsigned int u = 0; u < cols_i; u++)
		for (unsigned int v = 0; v < rows_i; v++)
			if (null(v, u) == false)
				dt(v, u) = fps * (depth_warped[image_level](v, u) -
								  depth_old[image_level](v, u));
}

void CDifodo::computeWeights()
{
	weights.resize(rows_i, cols_i);
	weights.fill(0.f);

	// Obtain the velocity associated to the rigid transformation estimated up
	// to the present level
	CVectorFixedFloat<6> kai_level;
	kai_level.setFromMatrixLike(kai_loc_old);

	Matrix4f acu_trans;
	acu_trans.setIdentity();
	for (unsigned int i = 0; i < level; i++)
		acu_trans = transformations[i].asEigen() * acu_trans;

	// Alternative way to compute the log
	auto mat_aux = CMatrixDouble44(acu_trans.cast<double>().eval());

	auto trans_vec = poses::Lie::SE<3>::log(poses::CPose3D(mat_aux));
	trans_vec *= fps;
	const auto kai_level_acu = CVectorFixedDouble<6>(trans_vec);

	kai_level -= kai_level_acu.cast_float();

	// Parameters for the measurement error
	const float f_inv = float(cols_i) / (2.f * tan(0.5f * fovh));
	const float kz2 = 8.122e-12f;  // square(1.425e-5) / 25

	// Parameters for linearization error
	const float kduv = 20e-5f;
	const float kdt = kduv / square(fps);
	const float k2dt = 5e-6f;
	const float k2duv = 5e-6f;

	for (unsigned int u = 1; u < cols_i - 1; u++)
		for (unsigned int v = 1; v < rows_i - 1; v++)
			if (null(v, u) == false)
			{
				//					Compute measurment error (simplified)
				//-----------------------------------------------------------------------
				const float z = depth_inter[image_level](v, u);
				const float inv_d = 1.f / z;
				// const float dycomp = du2(v,u)*f_inv_y*inv_d;
				// const float dzcomp = dv2(v,u)*f_inv_z*inv_d;
				const float z2 = z * z;
				const float z4 = z2 * z2;

				// const float var11 = kz2*z4;
				// const float var12 =
				// kz2*xx_inter[image_level](v,u)*z2*depth_inter[image_level](v,u);
				// const float var13 =
				// kz2*yy_inter[image_level](v,u)*z2*depth_inter[image_level](v,u);
				// const float var22 =
				// kz2*square(xx_inter[image_level](v,u))*z2;
				// const float var23 =
				// kz2*xx_inter[image_level](v,u)*yy_inter[image_level](v,u)*z2;
				// const float var33 =
				// kz2*square(yy_inter[image_level](v,u))*z2;
				const float var44 = kz2 * z4 * square(fps);
				const float var55 = kz2 * z4 * 0.25f;
				const float var66 = var55;

				// const float j1 =
				// -2.f*inv_d*inv_d*(xx_inter[image_level](v,u)*dycomp +
				// yy_inter[image_level](v,u)*dzcomp)*(kai_level[0] +
				// yy_inter[image_level](v,u)*kai_level[4] -
				// xx_inter[image_level](v,u)*kai_level[5])
				//				+ inv_d*dycomp*(kai_level[1] -
				// yy_inter[image_level](v,u)*kai_level[3]) +
				// inv_d*dzcomp*(kai_level[2] +
				// xx_inter[image_level](v,u)*kai_level[3]);
				// const float j2 = inv_d*dycomp*(kai_level[0] +
				// yy_inter[image_level](v,u)*kai_level[4] -
				// 2.f*xx_inter[image_level](v,u)*kai_level[5]) -
				// dzcomp*kai_level[3];
				// const float j3 = inv_d*dzcomp*(kai_level[0] +
				// 2.f*yy_inter[image_level](v,u)*kai_level[4] -
				// xx_inter[image_level](v,u)*kai_level[5]) +
				// dycomp*kai_level[3];

				const float j4 = 1.f;
				const float j5 =
					xx_inter[image_level](v, u) * inv_d * inv_d * f_inv *
						(kai_level[0] +
						 yy_inter[image_level](v, u) * kai_level[4] -
						 xx_inter[image_level](v, u) * kai_level[5]) +
					inv_d * f_inv *
						(-kai_level[1] - z * kai_level[5] +
						 yy_inter[image_level](v, u) * kai_level[3]);
				const float j6 =
					yy_inter[image_level](v, u) * inv_d * inv_d * f_inv *
						(kai_level[0] +
						 yy_inter[image_level](v, u) * kai_level[4] -
						 xx_inter[image_level](v, u) * kai_level[5]) +
					inv_d * f_inv *
						(-kai_level[2] + z * kai_level[4] -
						 xx_inter[image_level](v, u) * kai_level[3]);

				// error_measurement(v,u) = j1*(j1*var11+j2*var12+j3*var13) +
				// j2*(j1*var12+j2*var22+j3*var23)
				//						+j3*(j1*var13+j2*var23+j3*var33) +
				// j4*j4*var44
				//+
				// j5*j5*var55 + j6*j6*var66;

				const float error_m =
					j4 * j4 * var44 + j5 * j5 * var55 + j6 * j6 * var66;

				//					Compute linearization error
				//-----------------------------------------------------------------------
				const float ini_du = depth_old[image_level](v, u + 1) -
									 depth_old[image_level](v, u - 1);
				const float ini_dv = depth_old[image_level](v + 1, u) -
									 depth_old[image_level](v - 1, u);
				const float final_du = depth_warped[image_level](v, u + 1) -
									   depth_warped[image_level](v, u - 1);
				const float final_dv = depth_warped[image_level](v + 1, u) -
									   depth_warped[image_level](v - 1, u);

				const float dut = ini_du - final_du;
				const float dvt = ini_dv - final_dv;
				const float duu = du(v, u + 1) - du(v, u - 1);
				const float dvv = dv(v + 1, u) - dv(v - 1, u);
				const float dvu =
					dv(v, u + 1) -
					dv(v, u - 1);  // Completely equivalent to compute duv

				const float error_l =
					kdt * square(dt(v, u)) +
					kduv * (square(du(v, u)) + square(dv(v, u))) +
					k2dt * (square(dut) + square(dvt)) +
					k2duv * (square(duu) + square(dvv) + square(dvu));

				// Weight
				weights(v, u) = sqrt(1.f / (error_m + error_l));
			}

	// Normalize weights in the range [0,1]
	const float inv_max = 1.f / weights.maxCoeff();
	weights *= inv_max;
}

void CDifodo::solveOneLevel()
{
	MatrixXf A(num_valid_points, 6);
	MatrixXf B(num_valid_points, 1);
	unsigned int cont = 0;

	// Fill the matrix A and the vector B
	// The order of the unknowns is (vz, vx, vy, wz, wx, wy)
	// The points order will be (1,1), (1,2)...(1,cols-1), (2,1),
	// (2,2)...(row-1,cols-1).

	const float f_inv = float(cols_i) / (2.f * tan(0.5f * fovh));

	for (unsigned int u = 1; u < cols_i - 1; u++)
		for (unsigned int v = 1; v < rows_i - 1; v++)
			if (null(v, u) == false)
			{
				// Precomputed expressions
				const float d = depth_inter[image_level](v, u);
				const float inv_d = 1.f / d;
				const float x = xx_inter[image_level](v, u);
				const float y = yy_inter[image_level](v, u);
				const float dycomp = du(v, u) * f_inv * inv_d;
				const float dzcomp = dv(v, u) * f_inv * inv_d;
				const float tw = weights(v, u);

				// Fill the matrix A
				A(cont, 0) =
					tw * (1.f + dycomp * x * inv_d + dzcomp * y * inv_d);
				A(cont, 1) = tw * (-dycomp);
				A(cont, 2) = tw * (-dzcomp);
				A(cont, 3) = tw * (dycomp * y - dzcomp * x);
				A(cont, 4) = tw * (y + dycomp * inv_d * y * x +
								   dzcomp * (y * y * inv_d + d));
				A(cont, 5) = tw * (-x - dycomp * (x * x * inv_d + d) -
								   dzcomp * inv_d * y * x);
				B(cont, 0) = tw * (-dt(v, u));

				cont++;
			}

	// Solve the linear system of equations using weighted least squares
	const MatrixXf AtA = A.transpose() * A;
	const MatrixXf AtB = A.transpose() * B;
	VectorXf Var = AtA.ldlt().solve(AtB);

	// Covariance matrix calculation
	MatrixXf res = -B;
	for (unsigned int k = 0; k < 6; k++) res += Var(k) * A.col(k);

	est_cov =
		(1.f / float(num_valid_points - 6)) * AtA.inverse() * res.squaredNorm();

	// Update last velocity in local coordinates
	// (vx, vy, vz, wx, wy, wz)
	kai_loc_level.fromVector(Var);
}

void CDifodo::odometryCalculation()
{
	// Clock to measure the runtime
	mrpt::system::CTicTac clock;
	clock.Tic();

	// Build the gaussian pyramid
	if (fast_pyramid)
		buildCoordinatesPyramidFast();
	else
		buildCoordinatesPyramid();

	// Coarse-to-fines scheme
	for (unsigned int i = 0; i < ctf_levels; i++)
	{
		// Previous computations
		transformations[i].setIdentity();

		level = i;
		unsigned int s = pow(2.f, int(ctf_levels - (i + 1)));
		cols_i = cols / s;
		rows_i = rows / s;
		image_level =
			ctf_levels - i + round(log(float(m_width / cols)) / log(2.f)) - 1;

		// 1. Perform warping
		if (i == 0)
		{
			depth_warped[image_level] = depth[image_level];
			xx_warped[image_level] = xx[image_level];
			yy_warped[image_level] = yy[image_level];
		}
		else
			performWarping();

		// 2. Calculate inter coords and find null measurements
		calculateCoord();

		// 3. Compute derivatives
		calculateDepthDerivatives();

		// 4. Compute weights
		computeWeights();

		// 5. Solve odometry
		if (num_valid_points > 6) solveOneLevel();

		// 6. Filter solution
		filterLevelSolution();
	}

	// Update poses
	poseUpdate();

	// Save runtime
	execution_time = 1000.f * clock.Tac();
}

void CDifodo::filterLevelSolution()
{
	//		Calculate Eigenvalues and Eigenvectors
	//----------------------------------------------------------
	CMatrixFloat66 Bii;
	std::vector<float> eigenVals;

	if (est_cov.eig_symmetric(Bii, eigenVals))
	{
		std::cerr
			<< "\n Eigensolver couldn't find a solution. Pose is not updated\n";
		return;
	}

	// First, we have to describe both the new linear and angular velocities in
	// the "eigenvector" basis
	//-------------------------------------------------------------------------------------------------
	auto kai_b = CVectorFixedFloat<6>(Bii.asEigen().colPivHouseholderQr().solve(
		kai_loc_level.asVector<Matrix<float, 6, 1>>()));

	// Second, we have to describe both the old linear and angular velocities in
	// the "eigenvector" basis
	//-------------------------------------------------------------------------------------------------
	auto kai_loc_sub = kai_loc_old.asVector<Eigen::Matrix<float, 6, 1>>();

	// Important: we have to substract the previous levels' solutions from the
	// old velocity.
	Matrix4f acu_trans;
	acu_trans.setIdentity();
	for (unsigned int i = 0; i < level; i++)
		acu_trans = transformations[i].asEigen() * acu_trans;

	auto mat_aux = CMatrixDouble44(acu_trans.cast<double>());
	auto acu_trans_vec = poses::Lie::SE<3>::log(poses::CPose3D(mat_aux));
	acu_trans_vec *= fps;
	const auto kai_level_acu = CVectorFixedDouble<6>(acu_trans_vec);

	kai_loc_sub -= kai_level_acu.asEigen().cast<float>();

	// Matrix<float, 4, 4> log_trans = fps*acu_trans.log();
	// kai_loc_sub(0) -= log_trans(0,3); kai_loc_sub(1) -= log_trans(1,3);
	// kai_loc_sub(2) -= log_trans(2,3);
	// kai_loc_sub(3) += log_trans(1,2); kai_loc_sub(4) -= log_trans(0,2);
	// kai_loc_sub(5) += log_trans(0,1);

	// Transform that local representation to the "eigenvector" basis
	const Matrix<float, 6, 1> kai_b_old =
		Bii.asEigen().colPivHouseholderQr().solve(kai_loc_sub);

	//									Filter velocity
	//--------------------------------------------------------------------------------
	const float cf = previous_speed_eig_weight * expf(-int(level)),
				df = previous_speed_const_weight * expf(-int(level));
	Matrix<float, 6, 1> kai_b_fil;
	for (unsigned int i = 0; i < 6; i++)
		kai_b_fil(i) =
			(kai_b.asEigen()(i) + (cf * eigenVals[i] + df) * kai_b_old(i)) /
			(1.f + cf * eigenVals[i] + df);

	// Transform filtered velocity to the local reference frame
	Matrix<float, 6, 1> kai_loc_fil =
		Bii.asEigen().inverse().colPivHouseholderQr().solve(kai_b_fil);

	// Compute the rigid transformation
	auto aux_vel =
		mrpt::math::CVectorFixedDouble<6>(kai_loc_fil.cast<double>() / fps);
	const poses::CPose3D aux2 = mrpt::poses::Lie::SE<3>::exp(aux_vel);

	CMatrixDouble44 trans;
	aux2.getHomogeneousMatrix(trans);
	transformations[level] = trans.cast_float();
}

void CDifodo::poseUpdate()
{
	// First, compute the overall transformation
	//---------------------------------------------------
	Matrix4f acu_trans;
	acu_trans.setIdentity();
	for (unsigned int i = 1; i <= ctf_levels; i++)
		acu_trans = transformations[i - 1].asEigen() * acu_trans;

	// Compute the new estimates in the local and absolutes reference frames
	//---------------------------------------------------------------------
	auto mat_aux = CMatrixDouble44(acu_trans.cast<double>().eval());
	auto acu_trans_vec = poses::Lie::SE<3>::log(poses::CPose3D(mat_aux));
	acu_trans_vec *= fps;
	const CVectorFixedDouble<6> kai_level_acu(acu_trans_vec);
	kai_loc.fromVector(kai_level_acu.cast_float());

	//---------------------------------------------------------------------------------------------
	// Directly from Eigen:
	//- Eigen 3.1.0 needed for Matrix::log()
	//- The line "#include <unsupported/Eigen/MatrixFunctions>" should be
	// uncommented (CDifodo.h)
	//
	// Matrix<float, 4, 4> log_trans = fps*acu_trans.log();
	// kai_loc(0) = log_trans(0,3); kai_loc(1) = log_trans(1,3); kai_loc(2) =
	// log_trans(2,3);
	// kai_loc(3) = -log_trans(1,2); kai_loc(4) = log_trans(0,2); kai_loc(5) =
	// -log_trans(0,1);
	//---------------------------------------------------------------------------------------------

	CMatrixDouble33 inv_trans;

	cam_pose.getRotationMatrix(inv_trans);
	const auto v_abs =
		(inv_trans.asEigen() *
		 (kai_loc.asVector<Eigen::Matrix<double, 6, 1>>().topRows(3)))
			.eval();
	const auto w_abs =
		(inv_trans.asEigen() *
		 (kai_loc.asVector<Eigen::Matrix<double, 6, 1>>().bottomRows(3)))
			.eval();
	kai_abs.vx = v_abs.x();
	kai_abs.vy = v_abs.y();
	kai_abs.vz = v_abs.z();

	kai_abs.wx = w_abs.x();
	kai_abs.wy = w_abs.y();
	kai_abs.wz = w_abs.z();

	//						Update poses
	//-------------------------------------------------------
	cam_oldpose = cam_pose;
	const auto pose_aux = poses::CPose3D(CMatrixDouble44(acu_trans));
	cam_pose = cam_pose + pose_aux;

	// Compute the velocity estimate in the new ref frame (to be used by the
	// filter in the next iteration)
	//---------------------------------------------------------------------------------------------------
	cam_pose.getRotationMatrix(inv_trans);
	const auto old_vtrans =
		(inv_trans.asEigen().inverse() *
		 (kai_abs.asVector<Eigen::Matrix<double, 6, 1>>().topRows(3)))
			.eval();
	const auto old_w =
		(inv_trans.asEigen().inverse() *
		 (kai_abs.asVector<Eigen::Matrix<double, 6, 1>>().bottomRows(3)))
			.eval();

	kai_loc_old.vx = old_vtrans.x();
	kai_loc_old.vy = old_vtrans.y();
	kai_loc_old.vz = old_vtrans.z();

	kai_loc_old.wx = old_w.x();
	kai_loc_old.wy = old_w.y();
	kai_loc_old.wz = old_w.z();
}

void CDifodo::setFOV(float new_fovh, float new_fovv)
{
	fovh = M_PI * new_fovh / 180.0;
	fovv = M_PI * new_fovv / 180.0;
}

void CDifodo::getPointsCoord(CMatrixFloat& x, CMatrixFloat& y, CMatrixFloat& z)
{
	x.resize(rows, cols);
	y.resize(rows, cols);
	z.resize(rows, cols);

	z = depth_inter[0];
	x = xx_inter[0];
	y = yy_inter[0];
}

void CDifodo::getDepthDerivatives(
	CMatrixFloat& cur_du, CMatrixFloat& cur_dv, CMatrixFloat& cur_dt)
{
	cur_du.resize(rows, cols);
	cur_dv.resize(rows, cols);
	cur_dt.resize(rows, cols);

	cur_du = du;
	cur_dv = dv;
	cur_dt = dt;
}

void CDifodo::getWeights(CMatrixFloat& w)
{
	w.resize(rows, cols);
	w = weights;
}