/*
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;
  }
}


#include <miopen/conv/invokers/impl_gemm_dynamic.hpp>
#include <miopen/conv/data_invoke_params.hpp>
#include <miopen/algorithm.hpp>
#include <miopen/handle.hpp>
#include <miopen/tensor_ops.hpp>

#include <boost/any.hpp>

namespace miopen {
namespace conv {

float CallImplicitGemmDynamic(const miopen::Handle& handle,
                              const ConvolutionContext& ctx,
                              ConstData_t src,
                              Data_t dst,
                              ConstData_t wei,
                              const std::vector<KernelInvoke>& kernels)
{
    float elapsed = 0.0f;

    auto kernel = kernels[0];
    MIOPEN_LOG_I(kernel.GetName());
    bool kernel_is_1x1 = (kernel.GetName().find("igemm_v4r1_1x1_dynamic") == 0);
    // clang-format off
    int hi          = ctx.in_height;
    int wi          = ctx.in_width;
    int n           = ctx.batch_sz;
    int k           = ctx.n_outputs;
    int c           = ctx.n_inputs;
    int ho          = ctx.out_height;
    int wo          = ctx.out_width;
    int stride_h    = ctx.kernel_stride_h;
    int stride_w    = ctx.kernel_stride_w;
    int dilation_h  = ctx.kernel_dilation_h;
    int dilation_w  = ctx.kernel_dilation_w;
    int pad_h       = ctx.pad_h;
    int pad_w       = ctx.pad_w;
    int y           = ctx.kernel_size_h;
    int x           = ctx.kernel_size_w;
    int __pack0     = 0;
    // clang-format on
    std::vector<OpKernelArg> opArgs;
    opArgs.emplace_back(src);
    opArgs.emplace_back(wei);
    opArgs.emplace_back(dst);
    opArgs.emplace_back(hi);
    opArgs.emplace_back(wi);
    opArgs.emplace_back(n);
    opArgs.emplace_back(k);
    opArgs.emplace_back(c);
    opArgs.emplace_back(ho);
    opArgs.emplace_back(wo);
    opArgs.emplace_back(stride_h);
    opArgs.emplace_back(stride_w);
    opArgs.emplace_back(dilation_h);
    opArgs.emplace_back(dilation_w);
    opArgs.emplace_back(pad_h);
    opArgs.emplace_back(pad_w);
    if(kernel_is_1x1)
    {
        opArgs.emplace_back(__pack0);
    }
    else
    {
        opArgs.emplace_back(y);
        opArgs.emplace_back(x);
        opArgs.emplace_back(__pack0);
    }
    kernel(opArgs);

    if(handle.IsProfilingEnabled())
        elapsed += handle.GetKernelTime();
    return elapsed;
}

InvokerFactory MakeImplGemmDynamicDataInvokerFactory(const ConvolutionContext& ctx)
{
    if(ctx.direction.IsForward())
    {
        return [ctx](const std::vector<Kernel>& kernels) {
            return [=](const Handle& handle, const boost::any& primitive_parameters) {
                const auto data_ctx = boost::any_cast<conv::DataInvokeParams>(primitive_parameters);
                const auto& tensors = data_ctx.tensors;
                auto kernel         = handle.Run(kernels[0]);
                if(kernel.GetName().find("igemm_v4r1_dynamic") == 0 ||
                   kernel.GetName().find("igemm_v4r1_1x1_dynamic") == 0)
                {
                    std::vector<KernelInvoke> ks;
                    std::transform(kernels.begin(),
                                   kernels.end(),
                                   std::back_inserter(ks),
                                   [&](const Kernel& k) { return handle.Run(k); });
                    float elapsed = 0;
                    elapsed       = CallImplicitGemmDynamic(
                        handle, ctx, tensors.in, tensors.out, tensors.w, ks);
                    if(handle.IsProfilingEnabled())
                    {
                        handle.ResetKernelTime();
                        handle.AccumKernelTime(elapsed);
                    }
                }
                else
                {
                    MIOPEN_THROW(
                        "Error running dynamic implicit GEMM convolution (invalid kernel name?)");
                }
            };
        };
    }
    else
    {
        MIOPEN_THROW(
            "Error running dynamic implicit GEMM convolution (currently only support forward)");
    }
}

} // namespace conv
} // namespace miopen


#include "create_sphere.h"

#include "dynamic_value/get_value_as.h"
#include "geometry_component.h"
#include "geometry_operations/create_sphere.h"
#include "geometry_system.h"
#include "hierarchical_component.h"
#include "hierarchical_system.h"
#include "procedural_component.h"
#include "procedural_object_system.h"

#include <boost/qvm/map_vec_mat.hpp>

namespace pagoda
{
const std::string CreateSphereGeometry::outputGeometry("out");
const char* CreateSphereGeometry::name = "CreateSphereGeometry";

CreateSphereGeometry::CreateSphereGeometry(ProceduralObjectSystemPtr objectSystem) : ProceduralOperation(objectSystem)
{
	START_PROFILE;

	RegisterValues({{"radius", std::make_shared<FloatValue>(1.0f)},
	                {"slices", std::make_shared<Integer>(10)},
	                {"stacks", std::make_shared<Integer>(10)}});

	CreateOutputInterface(outputGeometry);
}

CreateSphereGeometry::~CreateSphereGeometry() {}

void CreateSphereGeometry::DoWork()
{
	START_PROFILE;

	float radius = get_value_as<float>(*GetValue("radius"));
	int slices = get_value_as<int>(*GetValue("slices"));
	int stacks = get_value_as<int>(*GetValue("stacks"));

	auto geometrySystem = m_proceduralObjectSystem->GetComponentSystem<GeometrySystem>();
	auto hierarchicalSystem = m_proceduralObjectSystem->GetComponentSystem<HierarchicalSystem>();

	CreateSphere<Geometry> createSphere(radius, slices, stacks);
	auto geometry = std::make_shared<Geometry>();
	createSphere.Execute(geometry);

	ProceduralObjectPtr object = CreateOutputProceduralObject(outputGeometry);
	std::shared_ptr<GeometryComponent> geometry_component =
	    geometrySystem->CreateComponentAs<GeometryComponent>(object);

	geometry_component->SetGeometry(geometry);
	geometry_component->SetScope(
	    Scope::FromGeometryAndConstrainedRotation(geometry, Mat3x3F(boost::qvm::diag_mat(Vec3F{1.0f, 1.0f, 1.0f}))));

	std::shared_ptr<HierarchicalComponent> hierarchical_component =
	    hierarchicalSystem->CreateComponentAs<HierarchicalComponent>(object);
}
}  // namespace pagoda


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_DETAIL_RADIX_SORT_HPP
#define BOOST_COMPUTE_ALGORITHM_DETAIL_RADIX_SORT_HPP

#include <iterator>

#include <boost/assert.hpp>
#include <boost/type_traits/is_signed.hpp>
#include <boost/type_traits/is_floating_point.hpp>

#include <boost/compute/kernel.hpp>
#include <boost/compute/program.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/exclusive_scan.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>
#include <boost/compute/type_traits/is_fundamental.hpp>
#include <boost/compute/type_traits/is_vector_type.hpp>
#include <boost/compute/type_traits/type_name.hpp>
#include <boost/compute/utility/program_cache.hpp>

namespace boost {
namespace compute {
namespace detail {

// meta-function returning true if type T is radix-sortable
template<class T>
struct is_radix_sortable :
    boost::mpl::and_<
        typename ::boost::compute::is_fundamental<T>::type,
        typename boost::mpl::not_<typename is_vector_type<T>::type>::type
    >
{
};

template<size_t N>
struct radix_sort_value_type
{
};

template<>
struct radix_sort_value_type<1>
{
    typedef uchar_ type;
};

template<>
struct radix_sort_value_type<2>
{
    typedef ushort_ type;
};

template<>
struct radix_sort_value_type<4>
{
    typedef uint_ type;
};

template<>
struct radix_sort_value_type<8>
{
    typedef ulong_ type;
};

template<typename T>
inline const char* enable_double()
{
    return " -DT2_double=0";
}

template<>
inline const char* enable_double<double>()
{
    return " -DT2_double=1";
}

const char radix_sort_source[] =
"#if T2_double\n"
"#pragma OPENCL EXTENSION cl_khr_fp64 : enable\n"
"#endif\n"
"#define K2_BITS (1 << K_BITS)\n"
"#define RADIX_MASK ((((T)(1)) << K_BITS) - 1)\n"
"#define SIGN_BIT ((sizeof(T) * CHAR_BIT) - 1)\n"

"inline uint radix(const T x, const uint low_bit)\n"
"{\n"
"#if defined(IS_FLOATING_POINT)\n"
"    const T mask = -(x >> SIGN_BIT) | (((T)(1)) << SIGN_BIT);\n"
"    return ((x ^ mask) >> low_bit) & RADIX_MASK;\n"
"#elif defined(IS_SIGNED)\n"
"    return ((x ^ (((T)(1)) << SIGN_BIT)) >> low_bit) & RADIX_MASK;\n"
"#else\n"
"    return (x >> low_bit) & RADIX_MASK;\n"
"#endif\n"
"}\n"

"__kernel void count(__global const T *input,\n"
"                    const uint input_offset,\n"
"                    const uint input_size,\n"
"                    __global uint *global_counts,\n"
"                    __global uint *global_offsets,\n"
"                    __local uint *local_counts,\n"
"                    const uint low_bit)\n"
"{\n"
     // work-item parameters
"    const uint gid = get_global_id(0);\n"
"    const uint lid = get_local_id(0);\n"

     // zero local counts
"    if(lid < K2_BITS){\n"
"        local_counts[lid] = 0;\n"
"    }\n"
"    barrier(CLK_LOCAL_MEM_FENCE);\n"

     // reduce local counts
"    if(gid < input_size){\n"
"        T value = input[input_offset+gid];\n"
"        uint bucket = radix(value, low_bit);\n"
"        atomic_inc(local_counts + bucket);\n"
"    }\n"
"    barrier(CLK_LOCAL_MEM_FENCE);\n"

     // write block-relative offsets
"    if(lid < K2_BITS){\n"
"        global_counts[K2_BITS*get_group_id(0) + lid] = local_counts[lid];\n"

         // write global offsets
"        if(get_group_id(0) == (get_num_groups(0) - 1)){\n"
"            global_offsets[lid] = local_counts[lid];\n"
"        }\n"
"    }\n"
"}\n"

"__kernel void scan(__global const uint *block_offsets,\n"
"                   __global uint *global_offsets,\n"
"                   const uint block_count)\n"
"{\n"
"    __global const uint *last_block_offsets =\n"
"        block_offsets + K2_BITS * (block_count - 1);\n"

     // calculate and scan global_offsets
"    uint sum = 0;\n"
"    for(uint i = 0; i < K2_BITS; i++){\n"
"        uint x = global_offsets[i] + last_block_offsets[i];\n"
"        global_offsets[i] = sum;\n"
"        sum += x;\n"
"    }\n"
"}\n"

"__kernel void scatter(__global const T *input,\n"
"                      const uint input_offset,\n"
"                      const uint input_size,\n"
"                      const uint low_bit,\n"
"                      __global const uint *counts,\n"
"                      __global const uint *global_offsets,\n"
"#ifndef SORT_BY_KEY\n"
"                      __global T *output,\n"
"                      const uint output_offset)\n"
"#else\n"
"                      __global T *keys_output,\n"
"                      const uint keys_output_offset,\n"
"                      __global T2 *values_input,\n"
"                      const uint values_input_offset,\n"
"                      __global T2 *values_output,\n"
"                      const uint values_output_offset)\n"
"#endif\n"
"{\n"
     // work-item parameters
"    const uint gid = get_global_id(0);\n"
"    const uint lid = get_local_id(0);\n"

     // copy input to local memory
"    T value;\n"
"    uint bucket;\n"
"    __local uint local_input[BLOCK_SIZE];\n"
"    if(gid < input_size){\n"
"        value = input[input_offset+gid];\n"
"        bucket = radix(value, low_bit);\n"
"        local_input[lid] = bucket;\n"
"    }\n"

     // copy block counts to local memory
"    __local uint local_counts[(1 << K_BITS)];\n"
"    if(lid < K2_BITS){\n"
"        local_counts[lid] = counts[get_group_id(0) * K2_BITS + lid];\n"
"    }\n"

     // wait until local memory is ready
"    barrier(CLK_LOCAL_MEM_FENCE);\n"

"    if(gid >= input_size){\n"
"        return;\n"
"    }\n"

     // get global offset
"    uint offset = global_offsets[bucket] + local_counts[bucket];\n"

     // calculate local offset
"    uint local_offset = 0;\n"
"    for(uint i = 0; i < lid; i++){\n"
"        if(local_input[i] == bucket)\n"
"            local_offset++;\n"
"    }\n"

"#ifndef SORT_BY_KEY\n"
     // write value to output
"    output[output_offset + offset + local_offset] = value;\n"
"#else\n"
     // write key and value if doing sort_by_key
"    keys_output[keys_output_offset+offset + local_offset] = value;\n"
"    values_output[values_output_offset+offset + local_offset] =\n"
"        values_input[values_input_offset+gid];\n"
"#endif\n"
"}\n";

template<class T, class T2>
inline void radix_sort_impl(const buffer_iterator<T> first,
                            const buffer_iterator<T> last,
                            const buffer_iterator<T2> values_first,
                            command_queue &queue)
{

    typedef T value_type;
    typedef typename radix_sort_value_type<sizeof(T)>::type sort_type;

    const context &context = queue.get_context();

    size_t count = detail::iterator_range_size(first, last);

    // sort parameters
    const uint_ k = 4;
    const uint_ k2 = 1 << k;
    const uint_ block_size = 128;

    uint_ block_count = static_cast<uint_>(count / block_size);
    if(block_count * block_size != count){
        block_count++;
    }

    // if we have a valid values iterator then we are doing a
    // sort by key and have to set up the values buffer
    bool sort_by_key = (values_first.get_buffer().get() != 0);

    // load (or create) radix sort program
    std::string cache_key =
        std::string("__boost_radix_sort_") + type_name<value_type>();

    if(sort_by_key){
        cache_key += std::string("_with_") + type_name<T2>();
    }

    std::stringstream options;
    options << "-DK_BITS=" << k;
    options << " -DT=" << type_name<sort_type>();
    options << " -DBLOCK_SIZE=" << block_size;

    if(boost::is_floating_point<value_type>::value){
        options << " -DIS_FLOATING_POINT";
    }

    if(boost::is_signed<value_type>::value){
        options << " -DIS_SIGNED";
    }

    if(sort_by_key){
        options << " -DSORT_BY_KEY";
        options << " -DT2=" << type_name<T2>();
        options << enable_double<T2>();
    }

    // load (or create) radix sort program
    boost::shared_ptr<program_cache> cache =
        program_cache::get_global_cache(context);

    program radix_sort_program =
        cache->get_or_build(cache_key, options.str(), radix_sort_source, context);

    kernel count_kernel(radix_sort_program, "count");
    kernel scan_kernel(radix_sort_program, "scan");
    kernel scatter_kernel(radix_sort_program, "scatter");

    // setup temporary buffers
    vector<value_type> output(count, context);
    vector<T2> values_output(sort_by_key ? count : 0, context);
    vector<uint_> offsets(k2, context);
    vector<uint_> counts(block_count * k2, context);

    const buffer *input_buffer = &first.get_buffer();
    uint_ input_offset = first.get_index();
    const buffer *output_buffer = &output.get_buffer();
    uint_ output_offset = 0;
    const buffer *values_input_buffer = &values_first.get_buffer();
    uint_ values_input_offset = values_first.get_index();
    const buffer *values_output_buffer = &values_output.get_buffer();
    uint_ values_output_offset = 0;

    for(uint_ i = 0; i < sizeof(sort_type) * CHAR_BIT / k; i++){
        // write counts
        count_kernel.set_arg(0, *input_buffer);
        count_kernel.set_arg(1, input_offset);
        count_kernel.set_arg(2, static_cast<uint_>(count));
        count_kernel.set_arg(3, counts);
        count_kernel.set_arg(4, offsets);
        count_kernel.set_arg(5, block_size * sizeof(uint_), 0);
        count_kernel.set_arg(6, i * k);
        queue.enqueue_1d_range_kernel(count_kernel,
                                      0,
                                      block_count * block_size,
                                      block_size);

        // scan counts
        if(k == 1){
            typedef uint2_ counter_type;
            ::boost::compute::exclusive_scan(
                make_buffer_iterator<counter_type>(counts.get_buffer(), 0),
                make_buffer_iterator<counter_type>(counts.get_buffer(), counts.size() / 2),
                make_buffer_iterator<counter_type>(counts.get_buffer()),
                queue
            );
        }
        else if(k == 2){
            typedef uint4_ counter_type;
            ::boost::compute::exclusive_scan(
                make_buffer_iterator<counter_type>(counts.get_buffer(), 0),
                make_buffer_iterator<counter_type>(counts.get_buffer(), counts.size() / 4),
                make_buffer_iterator<counter_type>(counts.get_buffer()),
                queue
            );
        }
        else if(k == 4){
            typedef uint16_ counter_type;
            ::boost::compute::exclusive_scan(
                make_buffer_iterator<counter_type>(counts.get_buffer(), 0),
                make_buffer_iterator<counter_type>(counts.get_buffer(), counts.size() / 16),
                make_buffer_iterator<counter_type>(counts.get_buffer()),
                queue
            );
        }
        else {
            BOOST_ASSERT(false && "unknown k");
            break;
        }

        // scan global offsets
        scan_kernel.set_arg(0, counts);
        scan_kernel.set_arg(1, offsets);
        scan_kernel.set_arg(2, block_count);
        queue.enqueue_task(scan_kernel);

        // scatter values
        scatter_kernel.set_arg(0, *input_buffer);
        scatter_kernel.set_arg(1, input_offset);
        scatter_kernel.set_arg(2, static_cast<uint_>(count));
        scatter_kernel.set_arg(3, i * k);
        scatter_kernel.set_arg(4, counts);
        scatter_kernel.set_arg(5, offsets);
        scatter_kernel.set_arg(6, *output_buffer);
        scatter_kernel.set_arg(7, output_offset);
        if(sort_by_key){
            scatter_kernel.set_arg(8, *values_input_buffer);
            scatter_kernel.set_arg(9, values_input_offset);
            scatter_kernel.set_arg(10, *values_output_buffer);
            scatter_kernel.set_arg(11, values_output_offset);
        }
        queue.enqueue_1d_range_kernel(scatter_kernel,
                                      0,
                                      block_count * block_size,
                                      block_size);

        // swap buffers
        std::swap(input_buffer, output_buffer);
        std::swap(values_input_buffer, values_output_buffer);
        std::swap(input_offset, output_offset);
        std::swap(values_input_offset, values_output_offset);
    }
}

template<class Iterator>
inline void radix_sort(Iterator first,
                       Iterator last,
                       command_queue &queue)
{
    radix_sort_impl(first, last, buffer_iterator<int>(), queue);
}

template<class KeyIterator, class ValueIterator>
inline void radix_sort_by_key(KeyIterator keys_first,
                              KeyIterator keys_last,
                              ValueIterator values_first,
                              command_queue &queue)
{
    radix_sort_impl(keys_first, keys_last, values_first, queue);
}

} // end detail namespace
} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_DETAIL_RADIX_SORT_HPP


/*
==============================================================================
KratosStructuralApplication
A library based on:
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).

Copyright 2007
Pooyan Dadvand, Riccardo Rossi, Janosch Stascheit, Felix Nagel
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
janosch.stascheit@rub.de
nagel@sd.rub.de
- CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain
- Ruhr-University Bochum, Institute for Structural Mechanics, Germany

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 condition:

Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNERS.

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.

==============================================================================
*/

//
//   Project Name:        Kratos
//   Last Modified by:    $Author: Nelson $
//   Date:                $Date: 2009-02-02 14:03:23 $
//   Revision:            $Revision: 1.5 $
//

// System includes
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <boost/timer.hpp>

// External includes
#include "boost/smart_ptr.hpp"

// Project includes
#include "includes/define.h"
#include "python/pointer_vector_set_python_interface.h"
#include "python/variable_indexing_python.h"
#include "custom_python/add_fluency_criteria_to_python.h"
#include "fluency_criteria/fluency_criteria.h"
#include "python/vector_python_interface.h"
#include "fluency_criteria/energy_yield_function.h"
#include "fluency_criteria/isotropic_rankine_yield_function.h"
//#include "fluency_criteria/tresca_yield_function.h"
#include "fluency_criteria/von_mises_yield_function.h"
#include "fluency_criteria/modified_morh_coulomb_yield_function.h"
#include "fluency_criteria/morh_coulomb_yield_function.h"
#include "fluency_criteria/standard_morh_coulomb_yield_function.h"

//#include "fluency_criteria/rankine_yield_function.h"
//#include "fluency_criteria/drucker_prager_yield_function.h"

#include "soft_hard_behavior/softening_hardening_criteria.h"
#include "soft_hard_behavior/exponencial_softening.h"
#include "soft_hard_behavior/linear_softening.h"
#include "soft_hard_behavior/cohesion_softening.h"
#include "soft_hard_behavior/friction_softening.h"
#include "soft_hard_behavior/dilatancy_softening.h"

#include "spaces/ublas_space.h"

namespace Kratos
{

namespace Python
{
using namespace boost::python;
typedef FluencyCriteria                   FluencyCriteriaType;
typedef SofteningHardeningCriteria        SofteningHardeningType;
typedef Morh_Coulomb_Yield_Function       MorhCoulombType;
typedef Isotropic_Rankine_Yield_Function  RankineType;

typedef FluencyCriteria::Pointer            FluencyPointerType;
typedef SofteningHardeningCriteria::Pointer SofteningHardeningPointerType;
typedef MorhCoulombType::Pointer            MorhCoulombPointerType;
typedef RankineType::Pointer                RankinePointerType;

void  AddFluencyCriteriaToPython()
{

    class_< FluencyCriteriaType, boost::noncopyable >
    ("FluencyCriteriaType",
     init<>() )
    ;

    class_< SofteningHardeningType, boost::noncopyable >
    ("SofteningHardeningType",
     init<>() )
    ;

    enum_<myState>("State")
    .value("Plane_Stress", Plane_Stress)
    .value("Plane_Strain", Plane_Strain)
    .value("Tri_D", Tri_D)
    ;

    enum_<myPotencialPlastic>("PotencialPlastic")
    .value("Not_Associated", Not_Associated)
    .value("Associated", Associated)
    ;

    /*
    class_<Rankine_Yield_Function, bases< FluencyCriteriaType >, boost::noncopyable >
    ("RankineYieldFunction",
    init<myState> () )
    ;
    */

    class_<Isotropic_Rankine_Yield_Function, bases< FluencyCriteriaType >, boost::noncopyable >
    ("IsotropicRankineYieldFunction",
     init<SofteningHardeningPointerType,
     myState> () )
    ;

    /*
    class_<Tresca_Yield_Function, bases< FluencyCriteriaType >, boost::noncopyable >
    ("TrescaYieldFunction",
    init<myState> () )
    ;
    */

    class_<Von_Mises_Yield_Function, bases< FluencyCriteriaType >, boost::noncopyable >
    ("VonMisesYieldFunction",
     init<const myState&, const SofteningHardeningPointerType&> () )
    //init<myState, myPotencialPlastic> () )
    ;

    class_<Modified_Morh_Coulomb_Yield_Function, bases< FluencyCriteriaType >, boost::noncopyable >
    ("ModifiedMorhCoulombYieldFunction",
     init<myState, MorhCoulombPointerType, RankinePointerType> () )
    /* init<const SofteningHardeningPointerType&,
         const SofteningHardeningPointerType&,
         const SofteningHardeningPointerType&,
         const SofteningHardeningPointerType&,
         const myState,
         const myPotencialPlastic> () )
         */
    ;

    class_<Morh_Coulomb_Yield_Function, bases< FluencyCriteriaType >, boost::noncopyable >
    ("MorhCoulombYieldFunction",
     init<const SofteningHardeningPointerType&,
     const SofteningHardeningPointerType&,
     const SofteningHardeningPointerType&,
     const myState,
     const myPotencialPlastic> () )
    ;

    class_<Standard_Morh_Coulomb_Yield_Function, bases< FluencyCriteriaType >, boost::noncopyable >
    ("StandardMorhCoulombYieldFunction",
     init<const SofteningHardeningPointerType&,
     const myState> () )
    ;

    /*
    class_<Drucker_Prager_Yield_Function, bases< FluencyCriteriaType >, boost::noncopyable >
    ("DruckerPragerYieldFunction",
    init<myState> () )
    ;
                 */

    class_<Energy_Yield_Function, bases< FluencyCriteriaType >, boost::noncopyable >
    ("EnergyYieldFunction",
     init<myState> () )
    ;

    class_<Exponential_Softening, bases< SofteningHardeningType>, boost::noncopyable >
    ("ExponentialSoftening",
     init<> () )
    ;

    class_<Linear_Softening, bases< SofteningHardeningType>, boost::noncopyable >
    ("LinearSoftening",
     init<> () )
    ;

    class_<Cohesion_Softening, bases< SofteningHardeningType>, boost::noncopyable >
    ("CohesionSoftening",
     init<> () )
    ;

    class_<Friction_Softening, bases< SofteningHardeningType>, boost::noncopyable >
    ("FrictionSoftening",
     init<> () )
    ;

    class_<Dilatancy_Softening, bases< SofteningHardeningType>, boost::noncopyable >
    ("DilatancySoftening",
     init<> () )
    ;

}

}  // namespace Python.

}  // namespace Kratos.


#pragma once

#include <boost/unordered_map.hpp>
#include <math/Matrix.hpp>
#include <math/Vec.hpp>
#include <math/Ray.hpp>
#include <memory>
#include <renderer/Camera.hpp>
#include <renderer/Program.hpp>
#include <renderer/Renderable.hpp>
#include <vector>

class RenderingEngine {
	public:
		typedef std::vector<Renderable*> RenderList;
		typedef boost::unordered_map<Program*, RenderList> RenderMap;

		static RenderingEngine * construct(float width, float height) {
			std::auto_ptr<RenderingEngine> ptr(new RenderingEngine(width, height));
			if (!ptr->init()) { return 0; }
			return ptr.release();
		}

		void submit(Renderable * renderable) {
			Program * program = renderable->get_program();
			render_map_[program].push_back(renderable);
		}

		void render();

		float get_view_width() const { return view_width_; }
		float get_view_height() const { return view_height_; }

		Vec2 project_to_screen(Vec4 const & v) const {
			return (Vec2(.5f, .5f) + camera_.project_to_normalized(v) / 2.0f)
				* Vec2(view_width_, view_height_);
		}

		Camera const & get_camera() const { return camera_; }
		void set_camera(Camera const & camera) { camera_ = camera; }

	private:
		RenderingEngine(float view_width, float view_height) :
			view_width_(view_width),
			view_height_(view_height)
		{ }

		bool init();

		Camera camera_;

		RenderMap render_map_;
		float view_width_, view_height_;
};


//  io_ex2.cpp  ----------------------------------------------------------//

//  Copyright 2010 Howard Hinnant
//  Copyright 2010 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*
This code was adapted by Vicente J. Botet Escriba from Hinnant's html documentation.
Many thanks to Howard for making his code available under the Boost license.
*/

#include <iostream>
#include <boost/ratio/ratio_io.hpp>

int main()
{
    using namespace std;
    using namespace boost;

    cout << "ratio_string<deca, char>::prefix() = "
             <<  ratio_string<boost::deca, char>::prefix() << '\n';
    cout << "ratio_string<deca, char>::symbol() = "
             <<  ratio_string<boost::deca, char>::symbol() << '\n';

    cout << "ratio_string<giga, char>::prefix() = "
             <<  ratio_string<boost::giga, char>::prefix() << '\n';
    cout << "ratio_string<giga, char>::symbol() = "
             <<  ratio_string<boost::giga, char>::symbol() << '\n';

    cout << "ratio_string<ratio<4, 6>, char>::prefix() = "
             <<  ratio_string<boost::ratio<4, 6>, char>::prefix() << '\n';
    cout << "ratio_string<ratio<4, 6>, char>::symbol() = "
             <<  ratio_string<boost::ratio<4, 6>, char>::symbol() << '\n';

    return 0;
}


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

 * 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    NonlinearFactorGraph.cpp
 * @brief   Factor Graph Consisting of non-linear factors
 * @author  Frank Dellaert
 * @author  Carlos Nieto
 * @author  Christian Potthast
 */

#include <cmath>
#include <limits>
#include <boost/foreach.hpp>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/inference.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
double NonlinearFactorGraph::probPrime(const Values& c) const {
  return exp(-0.5 * error(c));
}

/* ************************************************************************* */
void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
  cout << str << "size: " << size() << endl;
  for (size_t i = 0; i < factors_.size(); i++) {
    stringstream ss;
    ss << "factor " << i << ": ";
    if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
  }
}

/* ************************************************************************* */
void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
    const GraphvizFormatting& graphvizFormatting,
    const KeyFormatter& keyFormatter) const
{
  stm << "graph {\n";
  stm << "  size=\"" << graphvizFormatting.figureWidthInches << "," <<
    graphvizFormatting.figureHeightInches << "\";\n\n";

  FastSet<Key> keys = this->keys();

  // Local utility function to extract x and y coordinates
  struct { boost::optional<Point2> operator()(
    const Value& value, const GraphvizFormatting& graphvizFormatting)
  {
    if(const Pose2* p = dynamic_cast<const Pose2*>(&value)) {
      double x, y;
      switch (graphvizFormatting.paperHorizontalAxis) {
      case GraphvizFormatting::X: x = p->x(); break;
      case GraphvizFormatting::Y: x = p->y(); break;
      case GraphvizFormatting::Z: x = 0.0; break;
      case GraphvizFormatting::NEGX: x = -p->x(); break;
      case GraphvizFormatting::NEGY: x = -p->y(); break;
      case GraphvizFormatting::NEGZ: x = 0.0; break;
      default: throw std::runtime_error("Invalid enum value");
      }
      switch (graphvizFormatting.paperVerticalAxis) {
      case GraphvizFormatting::X: y = p->x(); break;
      case GraphvizFormatting::Y: y = p->y(); break;
      case GraphvizFormatting::Z: y = 0.0; break;
      case GraphvizFormatting::NEGX: y = -p->x(); break;
      case GraphvizFormatting::NEGY: y = -p->y(); break;
      case GraphvizFormatting::NEGZ: y = 0.0; break;
      default: throw std::runtime_error("Invalid enum value");
      }
      return Point2(x,y);
    } else if(const Pose3* p = dynamic_cast<const Pose3*>(&value)) {
      double x, y;
      switch (graphvizFormatting.paperHorizontalAxis) {
      case GraphvizFormatting::X: x = p->x(); break;
      case GraphvizFormatting::Y: x = p->y(); break;
      case GraphvizFormatting::Z: x = p->z(); break;
      case GraphvizFormatting::NEGX: x = -p->x(); break;
      case GraphvizFormatting::NEGY: x = -p->y(); break;
      case GraphvizFormatting::NEGZ: x = -p->z(); break;
      default: throw std::runtime_error("Invalid enum value");
      }
      switch (graphvizFormatting.paperVerticalAxis) {
      case GraphvizFormatting::X: y = p->x(); break;
      case GraphvizFormatting::Y: y = p->y(); break;
      case GraphvizFormatting::Z: y = p->z(); break;
      case GraphvizFormatting::NEGX: y = -p->x(); break;
      case GraphvizFormatting::NEGY: y = -p->y(); break;
      case GraphvizFormatting::NEGZ: y = -p->z(); break;
      default: throw std::runtime_error("Invalid enum value");
      }
     return Point2(x,y);
    } else {
      return boost::none;
    }
  }} getXY;

  // Find bounds
  double minX = numeric_limits<double>::infinity(), maxX = -numeric_limits<double>::infinity();
  double minY = numeric_limits<double>::infinity(), maxY = -numeric_limits<double>::infinity();
  BOOST_FOREACH(Key key, keys) {
    if(values.exists(key)) {
      boost::optional<Point2> xy = getXY(values.at(key), graphvizFormatting);
      if(xy) {
        if(xy->x() < minX)
          minX = xy->x();
        if(xy->x() > maxX)
          maxX = xy->x();
        if(xy->y() < minY)
          minY = xy->y();
        if(xy->y() > maxY)
          maxY = xy->y();
      }
    }
  }

  // Create nodes for each variable in the graph
  BOOST_FOREACH(Key key, keys) {
    // Label the node with the label from the KeyFormatter
    stm << "  var" << key << "[label=\"" << keyFormatter(key) << "\"";
    if(values.exists(key)) {
      boost::optional<Point2> xy = getXY(values.at(key), graphvizFormatting);
      if(xy)
        stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\"";
    }
    stm << "];\n";
  }
  stm << "\n";

  if(graphvizFormatting.mergeSimilarFactors) {
    // Remove duplicate factors
    FastSet<vector<Key> > structure;
    BOOST_FOREACH(const sharedFactor& factor, *this) {
      if(factor) {
        vector<Key> factorKeys = factor->keys();
        std::sort(factorKeys.begin(), factorKeys.end());
        structure.insert(factorKeys);
      }
    }

    // Create factors and variable connections
    size_t i = 0;
    BOOST_FOREACH(const vector<Key>& factorKeys, structure) {
      // Make each factor a dot
      stm << "  factor" << i << "[label=\"\", shape=point";
      {
        map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
        if(pos != graphvizFormatting.factorPositions.end())
          stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
      }
      stm << "];\n";

      // Make factor-variable connections
      BOOST_FOREACH(Key key, factorKeys) {
        stm << "  var" << key << "--" << "factor" << i << ";\n"; }

      ++ i;
    }
  } else {
    // Create factors and variable connections
    for(size_t i = 0; i < this->size(); ++i) {
      // Make each factor a dot
      stm << "  factor" << i << "[label=\"\", shape=point];\n";

      // Make factor-variable connections
      if(this->at(i)) {
        BOOST_FOREACH(Key key, *this->at(i)) {
          stm << "  var" << key << "--" << "factor" << i << ";\n"; } }
    }
  }

  stm << "}\n";
}

/* ************************************************************************* */
double NonlinearFactorGraph::error(const Values& c) const {
  gttic(NonlinearFactorGraph_error);
  double total_error = 0.;
  // iterate over all the factors_ to accumulate the log probabilities
  BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
    if(factor)
      total_error += factor->error(c);
  }
  return total_error;
}

/* ************************************************************************* */
FastSet<Key> NonlinearFactorGraph::keys() const {
  FastSet<Key> keys;
  BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
    if(factor)
      keys.insert(factor->begin(), factor->end());
  }
  return keys;
}

/* ************************************************************************* */
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
    const Values& config) const
{
  gttic(NonlinearFactorGraph_orderingCOLAMD);

  // Create symbolic graph and initial (iterator) ordering
  SymbolicFactorGraph::shared_ptr symbolic;
  Ordering::shared_ptr ordering;
  boost::tie(symbolic, ordering) = this->symbolic(config);

  // Compute the VariableIndex (column-wise index)
  VariableIndex variableIndex(*symbolic, ordering->size());
  if (config.size() != variableIndex.size()) throw std::runtime_error(
      "orderingCOLAMD: some variables in the graph are not constrained!");

  // Compute a fill-reducing ordering with COLAMD
  Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD(
      variableIndex));

  // Permute the Ordering with the COLAMD ordering
  ordering->permuteInPlace(*colamdPerm);
  return ordering;
}

/* ************************************************************************* */
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Values& config,
    const std::map<Key, int>& constraints) const
{
  gttic(NonlinearFactorGraph_orderingCOLAMDConstrained);

  // Create symbolic graph and initial (iterator) ordering
  SymbolicFactorGraph::shared_ptr symbolic;
  Ordering::shared_ptr ordering;
  boost::tie(symbolic, ordering) = this->symbolic(config);

  // Compute the VariableIndex (column-wise index)
  VariableIndex variableIndex(*symbolic, ordering->size());
  if (config.size() != variableIndex.size()) throw std::runtime_error(
      "orderingCOLAMD: some variables in the graph are not constrained!");

  // Create a constraint list with correct indices
  typedef std::map<Key, int>::value_type constraint_type;
  std::map<Index, int> index_constraints;
  BOOST_FOREACH(const constraint_type& p, constraints)
    index_constraints[ordering->at(p.first)] = p.second;

  // Compute a constrained fill-reducing ordering with COLAMD
  Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMDGrouped(
      variableIndex, index_constraints));

  // Permute the Ordering with the COLAMD ordering
  ordering->permuteInPlace(*colamdPerm);
  return ordering;
}

/* ************************************************************************* */
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
  gttic(NonlinearFactorGraph_symbolic_from_Ordering);

  // Generate the symbolic factor graph
  SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
  symbolicfg->reserve(this->size());

  BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
    if(factor)
      symbolicfg->push_back(factor->symbolic(ordering));
    else
      symbolicfg->push_back(SymbolicFactorGraph::sharedFactor());
  }

  return symbolicfg;
}

/* ************************************************************************* */
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
    const Values& config) const
{
  gttic(NonlinearFactorGraph_symbolic_from_Values);

  // Generate an initial key ordering in iterator order
  Ordering::shared_ptr ordering(config.orderingArbitrary());
  return make_pair(symbolic(*ordering), ordering);
}

/* ************************************************************************* */
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
    const Values& config, const Ordering& ordering) const
{
  gttic(NonlinearFactorGraph_linearize);

  // create an empty linear FG
  GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
  linearFG->reserve(this->size());

  // linearize all factors
  BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
    if(factor) {
      GaussianFactor::shared_ptr lf = factor->linearize(config, ordering);
      if (lf) linearFG->push_back(lf);
    } else
      linearFG->push_back(GaussianFactor::shared_ptr());
  }

  return linearFG;
}

/* ************************************************************************* */
NonlinearFactorGraph NonlinearFactorGraph::clone() const {
  NonlinearFactorGraph result;
  BOOST_FOREACH(const sharedFactor& f, *this) {
    if (f)
      result.push_back(f->clone());
    else
      result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
  }
  return result;
}

/* ************************************************************************* */
NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map<Key,Key>& rekey_mapping) const {
  NonlinearFactorGraph result;
  BOOST_FOREACH(const sharedFactor& f, *this) {
    if (f)
      result.push_back(f->rekey(rekey_mapping));
    else
      result.push_back(sharedFactor());
  }
  return result;
}

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

} // namespace gtsam


#include <boost/geometry/formulas/area_formulas.hpp>


/*!
@file
Defines `boost::hana::sum`.

@copyright Louis Dionne 2013-2017
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_HANA_SUM_HPP
#define BOOST_HANA_SUM_HPP

#include <boost/hana/fwd/sum.hpp>

#include <boost/hana/concept/foldable.hpp>
#include <boost/hana/concept/monoid.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/core/dispatch.hpp>
#include <boost/hana/fold_left.hpp>
#include <boost/hana/integral_constant.hpp> // required by fwd decl
#include <boost/hana/plus.hpp>
#include <boost/hana/zero.hpp>

namespace boost { namespace hana {
    //! @cond
    template <typename M>
    template <typename Xs>
    constexpr decltype(auto) sum_t<M>::operator()(Xs&& xs) const {
        using S = typename hana::tag_of<Xs>::type;
        using Sum = BOOST_HANA_DISPATCH_IF(sum_impl<S>,
            hana::Foldable<S>::value
        );

    #ifndef BOOST_HANA_CONFIG_DISABLE_CONCEPT_CHECKS
        static_assert(hana::Monoid<M>::value,
        "hana::sum<M> requires 'M' to be a Monoid");

        static_assert(hana::Foldable<S>::value,
        "hana::sum<M>(xs) requires 'xs' to be Foldable");
    #endif

        return Sum::template apply<M>(static_cast<Xs&&>(xs));
    }
    //! @endcond

    template <typename T, bool condition>
    struct sum_impl<T, when<condition>> : default_ {
        template <typename M, typename Xs>
        static constexpr decltype(auto) apply(Xs&& xs) {
            return hana::fold_left(static_cast<Xs&&>(xs), hana::zero<M>(), hana::plus);
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_SUM_HPP


#include <iostream>
#include <fstream>
#include <armadillo>
#include <math.h>
#include <sys/time.h>
#include "sys/stat.h"

using namespace arma;

#include "class_data.hpp"
#include "constants.hpp"

//Implement the member function of Data, load_data, which loads data from file with name DATA_FILE_NAME. The data file is assumed to begin with the number of locations (type: unsigned 64-bit integer), followed by an array of longitudes, latitudes, and observations (type: 64-bit real numbers)
void Data::load_data()
{
	gettimeofday(&timeNow, NULL);
	cout<<"===========================> Loading data starts. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";

    //Check data file exists
    struct stat fileStatus;
    if(stat(DATA_FILE_NAME.c_str(),&fileStatus) == -1 || !S_ISREG(fileStatus.st_mode))
    {
        cout<<"Program exits with an error: the data file "<<DATA_FILE_NAME<<" does not exist.\n";
        exit(EXIT_FAILURE);
    }

    //Open the data file by DATA_FILE_NAME
    std::ifstream file;
    file.open(DATA_FILE_NAME.c_str(), ios::binary);

    //Get the number of locations, nLocs
    nLocations=0;file.read((char*)&nLocations,8);

    //longitude, latitude, and values of all data points, including observations that have valid values and predicitons that have NaN values
    double *lon, *lat, *values;

    lon = new double [nLocations];
    lat = new double [nLocations];
    values = new double [nLocations];

    //Get lon, lat, values from the data file
    double minLon = 1e9, minLat = 1e9, maxLon = -1e9, maxLat = -1e9;
    for(unsigned long iLocation = 0; iLocation < nLocations; iLocation++)
    {
        file.read((char*)&lon[iLocation],8);
        if(lon[iLocation]<minLon) minLon=lon[iLocation];
        if(lon[iLocation]>maxLon) maxLon=lon[iLocation];
    }
    for(unsigned long iLocation = 0; iLocation < nLocations; iLocation++)
    {
        file.read((char*)&lat[iLocation],8);
        if(lat[iLocation]<minLat) minLat=lat[iLocation];
        if(lat[iLocation]>maxLat) maxLat=lat[iLocation];
    }
    for(unsigned long iLocation = 0; iLocation < nLocations; iLocation++)
        file.read((char*)&values[iLocation],8);

    //Close the data file
    file.close();

    //Assign the boundry, domainBoundaries
    domainBoundaries[0] = minLon;
    domainBoundaries[1] = maxLon+0.1*(maxLon-minLon);
    domainBoundaries[2] = minLat;
    domainBoundaries[3] = maxLat+0.1*(maxLat-minLat);

    //Split the data into observations and missing values
    numObservationsInRawData = 0;
    for(unsigned long iLocation = 0; iLocation < nLocations; iLocation++)
        if(!::isnan(values[iLocation])) numObservationsInRawData++;

    observationLon = new double [numObservationsInRawData];
    observationLat = new double [numObservationsInRawData];
    double *observationValues = new double [numObservationsInRawData];

    numPredictionsInRawData=nLocations;
    if(CALCULATION_MODE=="prediction" && PREDICTION_LOCATION_MODE=='N')
    {
        predictionLon = new double [numPredictionsInRawData];
        predictionLat = new double [numPredictionsInRawData];

        unsigned long observationCount=0, predictionCount=0;
        for(unsigned long iLocation = 0; iLocation < nLocations; iLocation++)
        {
            if(!::isnan(values[iLocation]))
            {
                observationLon[observationCount]=lon[iLocation];
                observationLat[observationCount]=lat[iLocation];
                observationValues[observationCount++]=values[iLocation];
            }else
            {
                predictionLon[predictionCount]=lon[iLocation];
                predictionLat[predictionCount++]=lat[iLocation];
            }
        }
        numPredictionsInRawData = predictionCount;
    }else
    {
        unsigned long observationCount=0, predictionCount=0;
        for(unsigned long iLocation = 0; iLocation < nLocations; iLocation++)
        {
            if(!::isnan(values[iLocation]))
            {
                observationLon[observationCount]=lon[iLocation];
                observationLat[observationCount]=lat[iLocation];
                observationValues[observationCount++]=values[iLocation];
            }
        }
    }

    if(CALCULATION_MODE=="prediction" && PREDICTION_LOCATION_MODE=='D')
    {
        predictionLon = lon;
        predictionLat = lat;
    }else
    {
        delete[] lon;
        delete[] lat;
    }

    if(CALCULATION_MODE=="prediction" && PREDICTION_LOCATION_MODE=='A')
    {
        if(stat(PREDICTION_LOCATION_FILE.c_str(),&fileStatus) == -1 || !S_ISREG(fileStatus.st_mode))
        {
            cout<<"Program exits with an error: the prediction location file "<<PREDICTION_LOCATION_FILE<<" does not exist.\n";
            exit(EXIT_FAILURE);
        }

        //Open the prediciton location file by PREDICTION_LOCATION_FILE
        file.open(PREDICTION_LOCATION_FILE.c_str(), ios::binary);

        //Read data
        file.read((char*)&numPredictionsInRawData,8);

        predictionLon = new double [numPredictionsInRawData];
        predictionLat = new double [numPredictionsInRawData];

        //Get prediction lon
        for(unsigned long iLocation = 0; iLocation < numPredictionsInRawData; iLocation++)
            file.read((char*)&predictionLon[iLocation],8);

        //Get prediction lat
        for(unsigned long iLocation = 0; iLocation < numPredictionsInRawData; iLocation++)
            file.read((char*)&predictionLat[iLocation],8);

        //Close the prediction location file
        file.close();
    }

    delete[] values;

    //Eliminate duplicates in the raw data
    if(ELIMINATION_DUPLICATES_FLAG)
        eliminate_duplicate(observationValues);
    else
    {
        NUM_OBSERVATIONS = numObservationsInRawData;
        NUM_PREDICTIONS = numPredictionsInRawData;
    }

    //Detrend data by a linear regression to lon, lat and a constant
    mat *desigenMatrix = new mat(NUM_OBSERVATIONS,3,fill::ones);

    vec *lonVector = new vec(observationLon,NUM_OBSERVATIONS,false,true);
    vec *latVector = new vec(observationLat,NUM_OBSERVATIONS,false,true);
    vec *valueVector = new vec(observationValues,NUM_OBSERVATIONS,false,true);

    (*desigenMatrix).col(1)=(*lonVector);
    (*desigenMatrix).col(2)=(*latVector);

    coefficients = solve(*desigenMatrix,*valueVector);

    vec *residualVector = new vec;
    *residualVector = (*valueVector) - (*desigenMatrix) * coefficients;

    delete[] observationValues;

    observationResiduals = new double [NUM_OBSERVATIONS];
    for(unsigned long iObservation = 0; iObservation < NUM_OBSERVATIONS; iObservation++) observationResiduals[iObservation] = (*residualVector)(iObservation);

    delete lonVector;
    delete latVector;
    delete valueVector;
    delete desigenMatrix;
    delete residualVector;

    gettimeofday(&timeNow, NULL);
    cout<<"===========================> Loading data is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n\n";
    if(PRINT_DETAIL_FLAG) print_data_summary();
}

//Implement the member function of Data, print_data_summary, which prints a brief summary of the data
void Data::print_data_summary()
{
    cout<<">>The number of locations: "<<nLocations<<endl<<endl;
    cout<<">>The number of observations in the raw data: "<<numObservationsInRawData<<endl<<endl;
    if(ELIMINATION_DUPLICATES_FLAG) cout<<">>The number of observations after eliminating duplicates: "<<NUM_OBSERVATIONS<<endl<<endl;

    cout<<">>The boundry of the spatial domain"<<endl;
    cout<<"  longitude: minimal "<<domainBoundaries[0]<<", maximal "<<domainBoundaries[1]<<endl;
    cout<<"  latitude : minimal "<<domainBoundaries[2]<<", maximal "<<domainBoundaries[3]<<endl<<endl;

    cout<<">>Examples of observations"<<endl;
    cout<<"     longitude: ";
    for(unsigned long iObs = 0; iObs < std::min(NUM_OBSERVATIONS,(unsigned long)5); iObs++)
        printf("%8.2lf ",observationLon[iObs]);
    cout<<"\n     latitude:  ";
    for(unsigned long iObs = 0; iObs < std::min(NUM_OBSERVATIONS,(unsigned long)5); iObs++)
        printf("%8.2lf ",observationLat[iObs]);
    cout<<"\n     residuals: ";
    for(unsigned long iObs = 0; iObs < std::min(NUM_OBSERVATIONS,(unsigned long)5); iObs++)
        printf("%8.2lf ",observationResiduals[iObs]);
    cout<<"\n\n";

    if(CALCULATION_MODE=="prediction")
    {
        cout<<">>The number of prediction locations in the raw data: "<<numPredictionsInRawData<<endl<<endl;
        if(ELIMINATION_DUPLICATES_FLAG) cout<<">>The number of prediction locations after eliminating duplicates: "<<NUM_PREDICTIONS<<endl<<endl;

        cout<<">>Examples of prediction locations"<<endl;
        cout<<"     longitude: ";
        for(unsigned long iPred = 0; iPred < std::min(NUM_PREDICTIONS,(unsigned long)5); iPred++)
            printf("%8.2lf ",predictionLon[iPred]);
        cout<<"\n     latitude:  ";
        for(unsigned long iPred = 0; iPred < std::min(NUM_PREDICTIONS,(unsigned long)5); iPred++)
            printf("%8.2lf ",predictionLat[iPred]);
        cout<<"\n\n";
    }
}

//Implement the function that eliminate duplicate observations and predictions
void Data::eliminate_duplicate(double *& observationValues)
{
    struct compare
    {
        Data* data;
        compare(Data *data): data(data) {}
        bool operator() (unsigned long i, unsigned long j) const
        {
            if(data->observationLon[i]<data->observationLon[j]) return true;
            if(data->observationLon[i]==data->observationLon[j]) return data->observationLat[i]<data->observationLat[j];
            return false;
        }
    };

    struct prediction_compare
    {
        Data* data;
        prediction_compare(Data *data): data(data) {}
        bool operator() (unsigned long i, unsigned long j) const
        {
            if(data->predictionLon[i]<data->predictionLon[j]) return true;
            if(data->predictionLon[i]==data->predictionLon[j]) return data->predictionLat[i]<data->predictionLat[j];
            return false;
        }
    };

    unsigned long *tmpIndex = new unsigned long [numObservationsInRawData];
    for(unsigned long index = 0; index < numObservationsInRawData; index++) tmpIndex[index]=index;

    std::sort(tmpIndex,tmpIndex+numObservationsInRawData, compare(this));

    bool *duplicateFlag = new bool [numObservationsInRawData]();

    NUM_OBSERVATIONS = numObservationsInRawData;

    for(unsigned long index = 1; index < numObservationsInRawData; index++)
        if((observationLon[tmpIndex[index]]==observationLon[tmpIndex[index-1]]) && (observationLat[tmpIndex[index]]==observationLat[tmpIndex[index-1]]))
        {
            duplicateFlag[index] = true;
            NUM_OBSERVATIONS--;
        }

    double *tmpObservationLon, *tmpObservationLat, *tmpObservationValues;
    tmpObservationLon = new double [NUM_OBSERVATIONS];
    tmpObservationLat = new double [NUM_OBSERVATIONS];
    tmpObservationValues = new double [NUM_OBSERVATIONS];

    unsigned long count=0;
    for(unsigned long index = 0; index < numObservationsInRawData; index++)
    {
        if(duplicateFlag[index]) continue;
        tmpObservationLon[count]=observationLon[tmpIndex[index]];
        tmpObservationLat[count]=observationLat[tmpIndex[index]];
        tmpObservationValues[count++]=observationValues[tmpIndex[index]];
    }

    delete[] observationLon;
    delete[] observationLat;
    delete[] observationValues;

    observationLon=tmpObservationLon;
    observationLat=tmpObservationLat;
    observationValues=tmpObservationValues;

    delete[] tmpIndex;
    delete[] duplicateFlag;

    if(CALCULATION_MODE=="prediction")
    {
        unsigned long *tmpIndex = new unsigned long [numPredictionsInRawData];
        for(unsigned long index = 0; index < numPredictionsInRawData; index++) tmpIndex[index]=index;

        std::sort(tmpIndex,tmpIndex+numPredictionsInRawData, prediction_compare(this));

        bool *duplicateFlag = new bool [numPredictionsInRawData]();

        NUM_PREDICTIONS = numPredictionsInRawData;

        for(unsigned long index = 1; index < numPredictionsInRawData; index++)
            if((predictionLon[tmpIndex[index]]==predictionLon[tmpIndex[index-1]]) && (predictionLat[tmpIndex[index]]==predictionLat[tmpIndex[index-1]]))
            {
                duplicateFlag[index] = true;
                NUM_PREDICTIONS--;
            }

        double *tmpPredictionLon, *tmpPredictionLat;
        tmpPredictionLon = new double [NUM_PREDICTIONS];
        tmpPredictionLat = new double [NUM_PREDICTIONS];

        unsigned long count=0;
        for(unsigned long index = 0; index < numPredictionsInRawData; index++)
        {
            if(duplicateFlag[index]) continue;
            tmpPredictionLon[count]=predictionLon[tmpIndex[index]];
            tmpPredictionLat[count++]=predictionLat[tmpIndex[index]];
        }

        delete[] predictionLon;
        delete[] predictionLat;

        predictionLon=tmpPredictionLon;
        predictionLat=tmpPredictionLat;

        delete[] tmpIndex;
        delete[] duplicateFlag;
    }
}

//Implement the destructor of class Data
Data::~Data()
{
    //delete[] predictionLon;
    //delete[] predictionLat;

    //cout<<"Data is deleted.\n";
}


// Copyright (c) 2000, 2001, 2002, 2003 by David Scherer and others.
// See the file license.txt for complete license terms.
// See the file authors.txt for a complete list of contributors.

#include "python/faces.hpp"
#include <boost/python/tuple.hpp>

#include <map>
#include <set>
#include "wrap_gl.hpp"

#include "python/slice.hpp"
#include "util/gl_enable.hpp"

using boost::python::numeric::array;

namespace cvisual { namespace python {

bool
faces::degenerate() const
{
	return count < 3;
}

faces::faces()
{
	double* k = normal.data();
	k[0] = k[1] = k[2] = 0.0;
}

void faces::set_length(size_t new_len) {
	normal.set_length(new_len);
	arrayprim_color::set_length(new_len);
}

void
faces::append_rgb( const vector& nv_pos, const vector& nv_normal, float red, float green, float blue)
{
	arrayprim_color::append_rgb( nv_pos, red, green, blue );
	double* n = normal.data(count-1);
	n[0] = nv_normal.x;
	n[1] = nv_normal.y;
	n[2] = nv_normal.z;
}

void
faces::append( const vector& nv_pos, const vector& nv_normal, const rgb& nv_color)
{
	arrayprim_color::append( nv_pos, nv_color );
	double* n = normal.data(count-1);
	n[0] = nv_normal.x;
	n[1] = nv_normal.y;
	n[2] = nv_normal.z;
}

void
faces::append( const vector& nv_pos, const vector& nv_normal)
{
	arrayprim_color::append( nv_pos );
	double* n = normal.data(count-1);
	n[0] = nv_normal.x;
	n[1] = nv_normal.y;
	n[2] = nv_normal.z;
}

void
faces::append( const vector& nv_pos)
{
	arrayprim_color::append( nv_pos );
	double* n = normal.data(count-1);
	n[0] = 0.;
	n[1] = 0.;
	n[2] = 0.;
}

// Define an ordering for the stl-sorting criteria.
struct stl_cmp_vector
{
	//AS added "const" to allow template match for VC++ build
 	bool operator()( const vector& lhs, const vector& rhs) const	{
		if (lhs.x < rhs.x)
			return true;
		else if (lhs.x > rhs.x)
			return false;
		else
			if (lhs.y < rhs.y)
				return true;
			else if (lhs.y > rhs.y)
				return false;
			else
				if (lhs.z < rhs.z)
					return true;
				else
					return false;
	}
};

void
faces::make_normals()
{
	if (shape(pos) != shape(normal))
		throw std::invalid_argument( "Dimension mismatch between pos and normal.");

	// Create normals that are perpendicular to all faces
	if (count == 0) return;
	using boost::python::make_tuple;
	normal[slice(0, count)] = make_tuple( 0, 0, 0);
	double* norm_i = normal.data();
	const double* pos_i = pos.data();
	const double* pos_end = pos.end();
	int i = 0;
	for ( ; pos_i < pos_end; pos_i+=9, norm_i+=9, i+=9) {
		if ((pos_i+9) > pos_end) break;
		vector v1 = vector(pos_i+3)-vector(pos_i);
		vector v2 = vector(pos_i+6)-vector(pos_i+3);
		vector n = v1.cross(v2).norm();
		double nx = n.get_x();
		double ny = n.get_y();
		double nz = n.get_z();
		norm_i[0] = norm_i[3] = norm_i[6] = nx;
		norm_i[1] = norm_i[4] = norm_i[7] = ny;
		norm_i[2] = norm_i[5] = norm_i[8] = nz;
	}
}

void
faces::make_twosided()
{
	if (shape(pos) != shape(normal))
		throw std::invalid_argument( "Dimension mismatch between pos and normal.");
	if (shape(pos) != shape(color))
		throw std::invalid_argument( "Dimension mismatch between pos and color.");

	// Duplicate existing faces with opposite windings and normals
	if (count < 3) return;
	double* pos_i = pos.data();
	double* norm_i = normal.data();
	double* color_i = color.data();
	// Make sure that there are 3 vertices per triangle
	if ((count % 3) == 1) {
		append(vector(pos_i+3*(count-1)), vector(norm_i+3*(count-1)), rgb(color_i+3*(count-1)));
		// Array may have moved: reestablish the pointers
		pos_i = pos.data();
		norm_i = normal.data();
		color_i = color.data();
	}
	if ((count % 3) == 2) {
		append(vector(pos_i+3*(count-1)), vector(norm_i+3*(count-1)), rgb(color_i+3*(count-1)));
		// Array may have moved: reestablish the pointers
		pos_i = pos.data();
		norm_i = normal.data();
		color_i = color.data();
	}
	int icount = 3*count;
	for (int i=0; i<icount; i+=3) {
		append(vector(pos_i+i), vector(norm_i+i), rgb(color_i+i));
		// Array may have moved: reestablish the pointers
		pos_i = pos.data();
		norm_i = normal.data();
		color_i = color.data();
	}
	for (int i=0; i<icount; i+=9) {
		for (int n=0; n<3; n++) {
			pos_i[icount+i+3+n] = pos_i[i+6+n];
			pos_i[icount+i+6+n] = pos_i[i+3+n];
			norm_i[icount+i+n] = -norm_i[i+n];
			norm_i[icount+i+3+n] = -norm_i[i+6+n];
			norm_i[icount+i+6+n] = -norm_i[i+3+n];
			color_i[icount+i+3+n] = color_i[i+6+n];
			color_i[icount+i+6+n] = color_i[i+3+n];
		}
	}
}

void
faces::smooth()
{
	smooth_d(0.95f);
}

void
faces::smooth_d(const float cosangle)
{
	if (shape(pos) != shape(normal))
		throw std::invalid_argument( "Dimension mismatch between pos and normal.");

	// positions -> normals
	typedef std::map< const vector, std::set<int>, stl_cmp_vector> vmap;
	vmap vertices;

	// First, map into sets all indices for the same vertex
	const double* pos_i = pos.data();
	const double* pos_end = pos.end();
	int i = 0;
	for ( ; pos_i < pos_end; pos_i+=3, i+=3) {
		vertices[vector(pos_i)].insert(i);
	}

	// Next, in a set of vertices, find those with similar normals
	// and average those normals, then find another group of similar normals
	// in that set and average those; continue until set is exhausted.
	double* norm_i = normal.data();
	vmap::iterator iter = vertices.begin();
	const vmap::iterator iterend = vertices.end();
	for ( ; iter != iterend; iter++) {
		while (! (iter->second).empty()) {
			std::list<int> similar;
			std::set<int>::iterator setiter = (iter->second).begin();
			const std::set<int>::iterator setiterend = (iter->second).end();
			int pt = *setiter;
			vector thisnorm = vector(norm_i+pt).norm();
			if (thisnorm == vector(0,0,0) ) {
				// Choose a different seed
				(iter->second).erase(*setiter);
				continue;
			}
			for ( ; setiter != setiterend; setiter++) {
				if (vector(norm_i+*setiter).norm().dot(thisnorm) >= cosangle) {
					similar.push_back(*setiter);
				}
			}
			vector average = vector(0,0,0);
			std::list<int>::iterator viter = similar.begin();
			const std::list<int>::iterator viterend = similar.end();
			for ( ; viter != viterend; viter++) {
				average += vector(norm_i+*viter).norm();
			}
			average = average.norm();
			double averagex = average.get_x();
			double averagey = average.get_y();
			double averagez = average.get_z();
			for ( viter=similar.begin(); viter != viterend; viter++) {
				norm_i[*viter] = averagex;
				norm_i[*viter+1] = averagey;
				norm_i[*viter+2] = averagez;
				(iter->second).erase(*viter);
			}
			similar.clear();
		}
	}
}

boost::python::object faces::get_normal() {
	return normal[all()];
}

void faces::set_normal( const double_array& n_normal)
{
	std::vector<npy_intp> dims = shape(n_normal);
	if (dims.size() == 2 && dims[1] == 3) {
		if (count == 0) { // This happens if set_normal called before set_pos in constructor
			set_length( dims[0] );
		}
	} else if (dims.size() == 1 && dims[0] == 3) {
		if (count == 0) { // This happens if set_normal called before set_pos in constructor
			set_length( 1 );
		}
	}

	normal[slice(0, count)] = n_normal;
	double* norm_i = normal.data();
}

void faces::set_normal_v( vector v)
{
	// We seem never to get here, which I don't understand
	using boost::python::make_tuple;
	// Broadcast the new normal across the array.
	int npoints = count ? count : 1;
	normal[slice(0, npoints)] = make_tuple( v.x, v.y, v.z);
}

void
faces::gl_render(view& scene)
{
	if (degenerate())
		return;

	std::vector<vector> spos;
	std::vector<rgb> tcolor;

	gl_enable_client vertexes( GL_VERTEX_ARRAY);
	gl_enable_client normals( GL_NORMAL_ARRAY);
	gl_enable_client colors( GL_COLOR_ARRAY);

	glNormalPointer( GL_DOUBLE, 0, normal.data() );

	/*
	// This attempt to minimize loop overhead made no difference in faces rendering speed
	if (scene.gcf != 1.0 || (scene.gcfvec[0] != scene.gcfvec[1])) {
		double gx = scene.gcfvec[0];
		double gy = scene.gcfvec[1];
		double gz = scene.gcfvec[2];
		std::vector<vector> tmp( count);
		spos.swap( tmp);
		const double* p = pos.data();
		double* s = &spos[0][0];
		size_t i;
		for (i=0; i<(3*(count-10)); ) { // reduce loop overhead to a minimum
			s[i   ] = gx*p[i   ]; s[i+1 ] = gy*p[i+1 ]; s[i+2 ] = gz*p[i+2];
			s[i+3 ] = gx*p[i+3 ]; s[i+4 ] = gy*p[i+4 ]; s[i+5 ] = gz*p[i+5];
			s[i+6 ] = gx*p[i+6 ]; s[i+7 ] = gy*p[i+7 ]; s[i+8 ] = gz*p[i+8];
			s[i+9 ] = gx*p[i+9 ]; s[i+10] = gy*p[i+10]; s[i+11] = gz*p[i+11];
			s[i+12] = gx*p[i+12]; s[i+13] = gy*p[i+13]; s[i+14] = gz*p[i+14];
			s[i+15] = gx*p[i+15]; s[i+16] = gy*p[i+16]; s[i+17] = gz*p[i+17];
			s[i+18] = gx*p[i+18]; s[i+19] = gy*p[i+19]; s[i+20] = gz*p[i+20];
			s[i+21] = gx*p[i+21]; s[i+22] = gy*p[i+22]; s[i+23] = gz*p[i+23];
			s[i+24] = gx*p[i+24]; s[i+25] = gy*p[i+25]; s[i+26] = gz*p[i+26];
			s[i+27] = gx*p[i+27]; s[i+28] = gy*p[i+28]; s[i+29] = gz*p[i+29];
			i += 30;
		}
		for (; i<3*count;) {
			s[i] = gx*p[i]; s[i+1] = gy*p[i+1]; s[i+2] = gz*p[i+2];
			i += 3;
		}

		glVertexPointer( 3, GL_DOUBLE, 0, s);
	}
	else
		glVertexPointer( 3, GL_DOUBLE, 0, pos.data() );
	*/

	if (scene.gcf != 1.0 || (scene.gcfvec[0] != scene.gcfvec[1])) {
		std::vector<vector> tmp( count);
		spos.swap( tmp);
		const double* pos_i = pos.data();
		for (std::vector<vector>::iterator i = spos.begin(); i != spos.end(); ++i) {
			*i = vector(pos_i).scale(scene.gcfvec);
			pos_i += 3;
		}
		glVertexPointer( 3, GL_DOUBLE, 0, &*spos.begin());
	}
	else
		glVertexPointer( 3, GL_DOUBLE, 0, pos.data() );

	if (scene.anaglyph) {
		std::vector<rgb> tmp( count);
		tcolor.swap( tmp);
		const double* color_i = color.data();
		for (std::vector<rgb>::iterator i = tcolor.begin(); i != tcolor.end(); ++i) {
			if (scene.coloranaglyph)
				*i = rgb(color_i).desaturate();
			else
				*i =  rgb(color_i).grayscale();
			color_i += 3;
		}
		glColorPointer( 3, GL_FLOAT, 0, &*tcolor.begin());
	}
	else
		glColorPointer( 3, GL_DOUBLE, 0, color.data() );

	gl_enable cull_face( GL_CULL_FACE);
	for (size_t drawn = 0; drawn < count - count%3; drawn += 540) {
		glDrawArrays( GL_TRIANGLES, drawn,
			std::min( count - count%3 - drawn, (size_t)540));
	}
}

void
faces::gl_pick_render(view& scene)
{
	gl_render( scene);
}

vector
faces::get_center() const
{
	vector ret;
	const double* pos_i = pos.data();
	const double* pos_end = pos.data( count - count%3 );
	while (pos_i < pos_end) {
		ret += vector(pos_i);
		pos_i += 3; // 3 doubles per vector point
	}
	if (count)
		ret /= count;
	return ret;
}

void
faces::grow_extent( extent& world)
{
	const double* pos_i = pos.data();
	const double* pos_end = pos.data( count - count%3 );
	while (pos_i < pos_end) {
		world.add_point( vector(pos_i));
		pos_i += 3; // 3 doubles per vector point
	}
	world.add_body();
}

void
faces::get_material_matrix( const view& v, tmatrix& out ) {
	if (degenerate()) return;

	// TODO: Add some caching for extent with grow_extent etc
	vector min_extent, max_extent;
	const double* pos_i = pos.data();
	const double* pos_end = pos.data( count - count%3 );
	min_extent = max_extent = vector( pos_i ); pos_i += 3;
	while (pos_i < pos_end)
		for(int j=0; j<3; j++) {
			if (*pos_i < min_extent[j]) min_extent[j] = *pos_i;
			else if (*pos_i > max_extent[j]) max_extent[j] = *pos_i;
			pos_i++;
		}

	out.translate( vector(.5,.5,.5) );
	out.scale( vector(1,1,1) * (.999 / (v.gcf * std::max(max_extent.x-min_extent.x, std::max(max_extent.y-min_extent.y, max_extent.z-min_extent.z)))) );
	out.translate( -.5 * v.gcf * (min_extent + max_extent) );
}

} } // !namespace cvisual::python


#include "cloudproc.hpp"
#include "convexdecomp.hpp"
using namespace pcl;
using namespace std;
using namespace Eigen;
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>
#include "utils/clock.hpp"
#include <boost/format.hpp>
#include "sphere_sampling.hpp"
using namespace util;
using namespace cloudproc;

int main (int argc, char** argv)
{
  if (argc != 2) {
    printf("usage: %s inputfile.pcd\n", argv[0]);
    abort();
  }

#if 1
  PointCloud<PointXYZ>::Ptr cloud = readPCDXYZ(argv[1]);
    PointCloud<PointXYZ>::Ptr cloud_ds = cloud;//downsampleCloud(cloud, .02f);
    pcl::io::savePCDFileBinary("/home/joschu/Data/scp/three_objs_ds.pcd", *cloud_ds);
    MatrixXf dirs = getSpherePoints(1);

#else
  PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
  cloud->push_back(PointXYZ(0,0,0));
  cloud->push_back(PointXYZ(0,1,0));
  cloud->push_back(PointXYZ(1,0,0));
  cloud->push_back(PointXYZ(1,1,0));
  cloud->push_back(PointXYZ(.5,.5,0));

  RowMat dirs(4,3);
  dirs << 1,0,0,
      -1,0,0,
      0,1,0,
      0,-1,0;
#endif

  double tStart = GetClock();
  vector<vector<int> > label2inds;
  ConvexDecomp(cloud_ds, dirs, .03, &label2inds, NULL);
  cout << "time: " << GetClock() - tStart << endl;

  typedef pair<int,int> IntPair;
  vector< IntPair > size_label;
  for (int i=0; i < label2inds.size(); ++i) {
    size_label.push_back(IntPair(-label2inds[i].size(), i));
  }

  std::sort(size_label.begin(), size_label.end());

  visualization::PCLVisualizer  viewer ("3d Viewer");
  viewer.setBackgroundColor (0,0,0);

  for (int i=0; i < size_label.size(); ++i) {
    int label = size_label[i].second;
    cout << label << " " << label2inds[label].size() << endl;
    pcl::PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);

    float r=rand()%255, g = rand()%255, b = rand()%255;

    for (int j=0; j < label2inds[label].size(); ++j) {
      PointXYZ  oldpt = cloud_ds->points[label2inds[label][j]];
      PointXYZRGB newpt;
      newpt.x = oldpt.x;
      newpt.y = oldpt.y;
      newpt.z = oldpt.z;
      newpt.r = r;
      newpt.g = g;
      newpt.b = b;
      cloud->push_back(newpt);
    }
    viewer.addPointCloud(cloud, (boost::format("cloud_%i")%i).str());
    viewer.spin();
  }

  return (0);
}


#include "IKSoftCSolver.hpp"

#include <vector>

#include <rw/rw.hpp>

#include <boost/tuple/tuple.hpp>

/**
 * @brief an iterative and heuristics based inverse kinematics solver specifically for the
 * Schunk Dexterous Hand.
 *
 * It is assumed that the pose of the hand-base is part of the configuration to solve. So 7
 * dof of the fingers plus the 6 dof of cartesean space is used to find a solution for the targets.
 *
 *
 */
class SDHInvKinSolver{
public:

    SDHInvKinSolver(std::string path);

    virtual ~SDHInvKinSolver(){};

    /**
     * @brief solve inverse kinematics for hand but make the hand approach from a specific
     * direction. The approach
     * @param baseTend [in] 3 unordered targets. All combinations are investigated.
     * @param approach [in] the direction of the z-axis of the base of the hand.
     * @param state [in] current state.
     * @return
     */
    std::vector<boost::tuple<rw::math::Transform3D<>, rw::math::Q, bool> > solve(const std::vector<rw::math::Transform3D<> >& baseTend,
                                   const rw::math::Vector3D<>& approach) const;

private:
    rw::models::WorkCell::Ptr _wc;
    rw::models::TreeDevice::Ptr _sdh;
    mutable rw::kinematics::State _defState;
    IKSoftCSolver *_iksolver;
    rw::invkin::JacobianIKSolverM *_jiksolver;
    rw::invkin::JacobianIKSolverM *_jiksolver2f;
    rw::kinematics::MovableFrame* _sdhbase;
    rw::models::Device::Ptr _se3dev;
    mutable rw::trajectory::TimedStatePath _path;
    mutable int _nrReducedTargets;
    rw::math::Transform3D<> _baseTtcp;
};


// File: vector.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

template <typename Vector>
void fill_and_print(Vector& v, const char* name)
{
    // Set values in traditional way and print them
    v= 1.2, 3.4, 5.6;
    std::cout << name << " is " << v << "\n";
}

int main(int, char**)
{
#if defined(MTL_WITH_VARIADIC_TEMPLATE) && defined(MTL_WITH_TEMPLATE_ALIAS)
    using namespace mtl;

    // Regular vector
    vector<double>                                  v1(3);
    fill_and_print(v1, "v1");

    // Row vector
    vector<double, row_major>                       v2(3);
    fill_and_print(v2, "v2");

    // Fixed-size vector
    vector<double, dim<3> >                         v3;
    fill_and_print(v3, "v3");
#endif
    return 0;
}