/****************************************************************************
* VCGLib                                                            o o     *
* Visual and Computer Graphics Library                            o     o   *
*                                                                _   O  _   *
* Copyright(C) 2004-2016                                           \/)\/    *
* Visual Computing Lab                                            /\/|      *
* ISTI - Italian National Research Council                           |      *
*                                                                    \      *
* All rights reserved.                                                      *
*                                                                           *
* This program is free software; you can redistribute it and/or modify      *
* it under the terms of the GNU General Public License as published by      *
* the Free Software Foundation; either version 2 of the License, or         *
* (at your option) any later version.                                       *
*                                                                           *
* This program is distributed in the hope that it will be useful,           *
* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
* for more details.                                                         *
*                                                                           *
****************************************************************************/
#ifndef __VCGLIB_SPATIAL_ITERATORS
#define __VCGLIB_SPATIAL_ITERATORS

#include <vector>
#include <vcg/space/intersection3.h>
#include <vcg/space/point3.h>
#include <vcg/space/box3.h>
#include <vcg/space/ray3.h>
#include <vcg/math/base.h>
#include <algorithm>
#include <float.h>
#include <limits>

namespace vcg{
  template <class Spatial_Idexing,class INTFUNCTOR,class TMARKER>
  class RayIterator
  {
  public:
    typedef typename Spatial_Idexing::ScalarType ScalarType;
    typedef typename vcg::Ray3<ScalarType> RayType;
    typedef typename Spatial_Idexing::Box3x IndexingBoxType;
  protected:
    typedef typename Spatial_Idexing::ObjType ObjType;
    typedef typename vcg::Point3<ScalarType>  CoordType;
    typedef typename Spatial_Idexing::CellIterator CellIterator;
    ScalarType max_dist;

    ///control right bonding current cell index (only on initialization)
    void _ControlLimits()
    {
      for (int i=0;i<3;i++)
      {
        vcg::Point3i dim=Si.siz;
        if (CurrentCell.V(i)<0)
          CurrentCell.V(i) = 0;
        else
          if (CurrentCell.V(i)>=dim.V(i))
            CurrentCell.V(i)=dim.V(i)-1;
      }
    }

    ///find initial line parameters
    void _FindLinePar()
    {
      /* Punti goal */

      ///da verificare se vanno oltre ai limiti
      vcg::Point3i ip;
      Si.PToIP(start,ip);
      Si.IPiToPf(ip,goal);
      for (int i=0;i<3;i++)
        if(r.Direction().V(i)>0.0)
          goal.V(i)+=Si.voxel.V(i);

      ScalarType gx=goal.X();
      ScalarType gy=goal.Y();
      ScalarType gz=goal.Z();

      dist=(r.Origin()-goal).Norm();

      const float LocalMaxScalar = (std::numeric_limits<float>::max)();
                        const float  EPS = std::numeric_limits<float>::min();

      /* Parametri della linea */
      ScalarType tx,ty,tz;

      if( fabs(r.Direction().X())>EPS ) {

        tx = (gx-r.Origin().X())/r.Direction().X();
      } else {
        tx  = LocalMaxScalar;
      }

      if(  fabs(r.Direction().Y())>EPS) {
        ty = (gy-r.Origin().Y())/r.Direction().Y();
      } else {
        ty = LocalMaxScalar;
      }

      if(  fabs(r.Direction().Z())>EPS  ) {
        tz = (gz-r.Origin().Z())/r.Direction().Z();
      } else {
        tz = LocalMaxScalar;
      }

      t=CoordType(tx,ty,tz);
    }

    bool _controlEnd()
    {
      return  (((CurrentCell.X()<0)||(CurrentCell.Y()<0)||(CurrentCell.Z()<0))||
        ((CurrentCell.X()>=Si.siz.X())||(CurrentCell.Y()>=Si.siz.Y())||(CurrentCell.Z()>=Si.siz.Z())));
    }

    void _NextCell()
    {
      assert(!end);
      vcg::Box3<ScalarType> bb_current;

      Si.IPiToPf(CurrentCell,bb_current.min);
      Si.IPiToPf(CurrentCell+vcg::Point3i(1,1,1),bb_current.max);

      CoordType inters;
      if (!IntersectionRayBox(bb_current, r, inters)) {
        end = true;
        return;
      }
      ScalarType testmax_dist = (inters - r.Origin()).Norm();

      if (testmax_dist>max_dist) {
        end=true;
      } else {
        if( t.X()<t.Y() && t.X()<t.Z() ) {
          if(r.Direction().X()<0.0)
          {goal.X() -= Si.voxel.X(); --CurrentCell.X();}
          else
          {goal.X() += Si.voxel.X(); ++CurrentCell.X();}
          t.X() = (goal.X()-r.Origin().X())/r.Direction().X();
        } else if( t.Y()<t.Z() ) {
          if(r.Direction().Y()<0.0)
          {goal.Y() -= Si.voxel.Y(); --CurrentCell.Y();}
          else
          {goal.Y() += Si.voxel.Y(); ++CurrentCell.Y();}
          t.Y() = (goal.Y()-r.Origin().Y())/r.Direction().Y();
        } else {
          if(r.Direction().Z()<0.0)
          { goal.Z() -= Si.voxel.Z(); --CurrentCell.Z();}
          else
          { goal.Z() += Si.voxel.Z(); ++CurrentCell.Z();}
          t.Z() = (goal.Z()-r.Origin().Z())/r.Direction().Z();
        }

        dist = (r.Origin() - goal).Norm();
        end = _controlEnd();
      }
    }

  public:


    ///contructor
    RayIterator(Spatial_Idexing &_Si,
          INTFUNCTOR & _int_funct
          ,const ScalarType &_max_dist)
          :Si(_Si),int_funct(_int_funct)
    {
      max_dist=_max_dist;
    };

    void SetMarker(TMARKER  _tm)
    {
      tm=_tm;
    }

    void Init(const RayType _r)
    {
      r=_r;
      end=false;
      tm.UnMarkAll();
      Elems.clear();
      //CoordType ip;
      //control if intersect the bounding box of the mesh
      if (Si.bbox.IsIn(r.Origin())) {
        start=r.Origin();
      } else if (!(vcg::IntersectionRayBox<ScalarType>(Si.bbox,r,start))){
        end=true;
        return;
      }
      Si.PToIP(start,CurrentCell);
      _ControlLimits();
      _FindLinePar();
      //go to first intersection
      while ((!End())&& Refresh()) {
        _NextCell();
      }

    }

    bool End() {return end;}


    ///refresh current cell intersection , return false if there are
    ///at lest 1 intersection
    bool Refresh()
    {
      //Elems.clear();

      typename Spatial_Idexing::CellIterator first,last,l;

      ///take first, last iterators to elements in the cell
      Si.Grid(CurrentCell.X(),CurrentCell.Y(),CurrentCell.Z(),first,last);
      for(l=first;l!=last;++l)
      {
        ObjType* elem=&(*(*l));
        ScalarType t;
        CoordType Int;
        if((!elem->IsD())&&(!tm.IsMarked(elem))&&(int_funct((**l),r,t))&&(t<=max_dist))
        {
          Int=r.Origin()+r.Direction()*t;
          Elems.push_back(Entry_Type(elem,t,Int));
          tm.Mark(elem);
        }
      }
      ////then control if there are more than 1 element
      std::sort(Elems.begin(),Elems.end());
      CurrentElem=Elems.rbegin();

      return((Elems.size()==0)||(Dist()>dist));
    }

    void operator ++()
    {
      if (!Elems.empty()) Elems.pop_back();

      CurrentElem = Elems.rbegin();

      if (Dist()>dist)
      {
        if (!End())
        {
          _NextCell();
          while ((!End())&&Refresh())
            _NextCell();
        }
      }
    }

    ObjType &operator *(){return *((*CurrentElem).elem);}

    CoordType IntPoint()
    {return ((*CurrentElem).intersection);}

    ScalarType Dist()
    {
      if (Elems.size()>0)
        return ((*CurrentElem).dist);
      else
        return ((ScalarType)FLT_MAX);
    }

    ///set the current spatial indexing structure used
    void SetIndexStructure(Spatial_Idexing &_Si)
    {Si=_Si;}



  protected:

    ///structure that mantain for the current cell pre-calculated data
    struct Entry_Type
    {
    public:

      Entry_Type(ObjType* _elem,ScalarType _dist,CoordType _intersection)
      {
        elem=_elem;
        dist=_dist;
        intersection=_intersection;
      }
      inline bool operator <  ( const Entry_Type & l ) const{return (dist > l.dist); }
      ObjType* elem;
      ScalarType dist;
      CoordType intersection;
    };

    RayType r;              //ray to find intersections
    Spatial_Idexing &Si;    //reference to spatial index algorithm
    bool end;                //true if the scan is terminated
    INTFUNCTOR &int_funct;
    TMARKER tm;

    std::vector<Entry_Type> Elems;          //element loaded from curren cell
    typedef typename std::vector<Entry_Type>::reverse_iterator ElemIterator;
    ElemIterator CurrentElem;  //iterator to current element

    vcg::Point3i CurrentCell;            //current cell

    //used for raterization
    CoordType start;
    CoordType goal;
    ScalarType dist;
    CoordType t;

  };


  template <class Spatial_Idexing,class DISTFUNCTOR,class TMARKER>
  class ClosestIterator
  {
    typedef typename Spatial_Idexing::ObjType ObjType;
    typedef typename Spatial_Idexing::ScalarType ScalarType;
    typedef typename vcg::Point3<ScalarType>  CoordType;
    typedef typename Spatial_Idexing::CellIterator CellIterator;



    ///control the end of scanning
    bool  _EndGrid()
    {
      if ((explored.min==vcg::Point3i(0,0,0))&&(explored.max==Si.siz))
        end =true;
      return end;
    }

    void _UpdateRadius()
    {
      if (radius>=max_dist)
        end=true;

      radius+=step_size;
      //control bounds
      if (radius>max_dist)
        radius=max_dist;
    }

    ///add cell to the curren set of explored cells
    bool _NextShell()
    {

      //then expand the box
      explored=to_explore;
      _UpdateRadius();
      Box3<ScalarType> b3d(p,radius);
      Si.BoxToIBox(b3d,to_explore);
      Box3i ibox(Point3i(0,0,0),Si.siz-Point3i(1,1,1));
      to_explore.Intersect(ibox);
      if (!to_explore.IsNull())
      {
        assert(!( to_explore.min.X()<0 || to_explore.max.X()>=Si.siz[0] ||
          to_explore.min.Y()<0 || to_explore.max.Y()>=Si.siz[1] ||  to_explore.min.Z()<0
          || to_explore.max.Z()>=Si.siz[2] ));
        return true;
      }
      return false;
    }



  public:

    ///contructor
    ClosestIterator(Spatial_Idexing &_Si,DISTFUNCTOR _dist_funct):Si(_Si),dist_funct(_dist_funct){}

    ///set the current spatial indexing structure used
    void SetIndexStructure(Spatial_Idexing &_Si)
    {Si=_Si;}

    void SetMarker(TMARKER _tm)
    {
      tm=_tm;
    }

    ///initialize the Iterator
    void Init(CoordType _p,const ScalarType &_max_dist)
    {
      explored.SetNull();
      to_explore.SetNull();
      p=_p;
      max_dist=_max_dist;
      Elems.clear();
      end=false;
      tm.UnMarkAll();
      //step_size=Si.voxel.X();
      step_size=Si.voxel.Norm();
      radius=0;

      ///inflate the bbox until find a valid bbox
      while ((!_NextShell())&&(!End())) {}

      while ((!End())&& Refresh()&&(!_EndGrid()))
          _NextShell();
    }

    //return true if the scan is complete
    bool End()
    {return end;}

    ///refresh Object found  also considering current shere radius,
    //and object comes from  previos  that are already in  the  stack
    //return false if no elements find
    bool Refresh()
    {
      int  ix,iy,iz;
      for( iz = to_explore.min.Z();iz <=  to_explore.max.Z(); ++iz)
      {
        for(iy =to_explore.min.Y(); iy  <=to_explore.max.Y(); ++iy)
        {
          for(ix =to_explore.min.X(); ix  <= to_explore.max.X();++ix)
          {
            // this test is to avoid to re-process already analyzed cells.
            if((explored.IsNull())||
              (ix<explored.min[0] || ix>explored.max[0] ||
              iy<explored.min[1] || iy>explored.max[1] ||
              iz<explored.min[2] || iz>explored.max[2] ))
            {
              typename Spatial_Idexing::CellIterator first,last,l;

              Si.Grid(ix,iy,iz,first,last);
              for(l=first;l!=last;++l)
              {
                ObjType  *elem=&(**l);
                if (!tm.IsMarked(elem))
                {

                  CoordType nearest;
                  ScalarType dist=max_dist;
                  if (dist_funct((**l),p,dist,nearest))
                    Elems.push_back(Entry_Type(elem,fabs(dist),nearest));
                  tm.Mark(elem);
                }
              }
            }
          }
        }
      }

      ////sort the elements in Elems and take a iterator to the last one
      std::sort(Elems.begin(),Elems.end());
      CurrentElem=Elems.rbegin();

      return((Elems.size()==0)||(Dist()>radius));
    }

    bool ToUpdate()
    {return ((Elems.size()==0)||(Dist()>radius));}

    void operator ++()
    {
      if (!Elems.empty()) Elems.pop_back();

      CurrentElem = Elems.rbegin();

      if ((!End())&& ToUpdate())
        do{_NextShell();}
          while (Refresh()&&(!_EndGrid()));
    }

    ObjType &operator *(){return *((*CurrentElem).elem);}

    //return distance of the element form the point if no element
    //are in the vector then return max dinstance
    ScalarType Dist()
    {
      if (Elems.size()>0)
        return ((*CurrentElem).dist);
      else
        return ((ScalarType)FLT_MAX);
    }

    CoordType NearestPoint()
    {return ((*CurrentElem).intersection);}

  protected:

    ///structure that mantain for the current cell pre-calculated data
    struct Entry_Type
    {
    public:

      Entry_Type(ObjType* _elem,ScalarType _dist,CoordType _intersection)
      {
        elem=_elem;
        dist=_dist;
        intersection=_intersection;
      }

      inline bool operator <  ( const Entry_Type & l ) const{return (dist > l.dist); }

      inline bool operator ==  ( const Entry_Type & l ) const{return (elem == l.elem); }

      ObjType* elem;
      ScalarType dist;
      CoordType intersection;
    };

    CoordType p;              //initial point
    Spatial_Idexing &Si;      //reference to spatial index algorithm
    bool end;                  //true if the scan is terminated
    ScalarType max_dist;      //max distance when the scan terminate
    vcg::Box3i explored;      //current bounding box explored
    vcg::Box3i to_explore;    //current bounding box explored
    ScalarType radius;        //curret radius for sphere expansion
    ScalarType step_size;      //radius step
    std::vector<Entry_Type> Elems; //element loaded from the current sphere

    DISTFUNCTOR dist_funct;
    TMARKER tm;

    typedef typename std::vector<Entry_Type>::reverse_iterator ElemIterator;
    ElemIterator CurrentElem;  //iterator to current element

};
}

#endif
