/*
 * LineSegmentSet.cpp
 *
 *  Created on: Jul 22, 2010
 *      Author: crueger
 */

// include config.h
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "CodePatterns/MemDebug.hpp"

#include "LinearAlgebra/LineSegmentSet.hpp"

#include "CodePatterns/Assert.hpp"
#include "LinearAlgebra/Line.hpp"
#include "LinearAlgebra/LineSegment.hpp"

#include "CodePatterns/Range.hpp"

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

using namespace std;

LineSegmentSet::LineSegmentSet(const Line &_line) :
  line(new Line(_line))
{}

LineSegmentSet::LineSegmentSet(const LineSegmentSet &src)
{
  line.reset(new Line(*src.line));
  segments=src.segments;
}

LineSegmentSet& LineSegmentSet::operator=(const LineSegmentSet &src){
  if(this!=&src){
    lineptr _line(new Line(*src.line));
    segments=src.segments;
    line = _line;
  }
  return *this;
}

void LineSegmentSet::insert(const LineSegment &lineSeg){
  ASSERT(*line==lineSeg.getLine(),"Inserted LineSegment was not from the line of the set");
  // we might get messed up sets
  if(lineSeg.hasZeroLength()){
    // nothing to do here... there is practically nothing that is being inserted
    return;
  }
  if(lineSeg.getBegin()>lineSeg.getEnd()){
    // just insert the reversed set
    insert(LineSegment(lineSeg.getEnd(),lineSeg.getBegin()));
    return;
  }
  // get the first overlapping segment
  set_t::iterator begin = find_if(segments.begin(),
                                  segments.end(),
                                  boost::bind(&LineSegment::overlaps,lineSeg,_1));
  // see if we have overlapping segments
  if(begin==segments.end()){
    // no overlapping segments... we can just insert and get going
    segments.insert(lineSeg);
    return;
  }
  // get the last overlapping segment
  set_t::iterator last = begin;
  while(last!=segments.end() && lineSeg.overlaps(*last))++last;
  last--;
  // all segments between last and end now overlap and both are valid iterators
  ASSERT(begin!=segments.end() && last!=segments.end(),"ERROR: last or begin are behind end of set");
  // get the endpoints of the section
  LinePoint beginPoint = min(begin->getBegin(),lineSeg.getBegin());
  LinePoint endPoint = max(last->getEnd(),lineSeg.getEnd());
  LineSegment insertSeg(beginPoint,endPoint);
  // delete all overlapping segments
  last++;
  segments.erase(begin,last);
  // insert the new combined segments
  segments.insert(insertSeg);
}

void LineSegmentSet::erase(const LineSegment &lineSeg){
  ASSERT(*line==lineSeg.getLine(),"Erased LineSegment was not from the line of the set");
  // we might get messed up sets
  if(lineSeg.hasZeroLength()){
    // nothing to do here... there is practically nothing that is being erase
    return;
  }
  if(lineSeg.getBegin()>lineSeg.getEnd()){
    // just erase the reversed set
    erase(LineSegment(lineSeg.getEnd(),lineSeg.getBegin()));
    return;
  }
  // get the first overlapping segment
  set_t::iterator begin = find_if(segments.begin(),
                                  segments.end(),
                                  boost::bind(&LineSegment::overlaps,lineSeg,_1));
  if(begin==segments.end()){
    // nothing to erase
    return;
  }
  // get the last overlapping segment
  set_t::iterator last = begin;
  while(last!=segments.end() && lineSeg.overlaps(*last))++last;
  last--;
  // at the ends there might be two overlapping lineSegments of which parts remain
  LineSegment beginSegment(begin->getBegin(),lineSeg.getBegin());
  LineSegment endSegment(lineSeg.getEnd(),last->getEnd());
  // erase all elements that have been overlapped
  last++;
  segments.erase(begin,last);
  // insert the two segments if they still have space left
  if(beginSegment.getBegin()<beginSegment.getEnd())
    segments.insert(beginSegment);
  if(endSegment.getBegin()<endSegment.getEnd())
      segments.insert(endSegment);
}

LineSegmentSet::iterator LineSegmentSet::begin(){
  return segments.begin();
}
LineSegmentSet::const_iterator LineSegmentSet::begin() const{
  return segments.begin();
}
LineSegmentSet::iterator LineSegmentSet::end(){
  return segments.end();
}
LineSegmentSet::const_iterator LineSegmentSet::end() const{
  return segments.end();
}

bool LineSegmentSet::isContained(const Vector &point) const{
  if(!line->isContained(point)){
    return false;
  }
  LinePoint lp = line->getLinePoint(point);
  for(set_t::iterator iter=segments.begin();iter!=segments.end();++iter){
    if(iter->isContained(point)){
      return true;
    }
    if(iter->getBegin()>lp){
      break;
    }
  }
  return false;
}

Line LineSegmentSet::getLine(){
  return *line;
}

LineSegmentSet merge(const LineSegmentSet &x,const LineSegmentSet &y){
  typedef range<LineSegmentSet::set_t::iterator> itRange;
  ASSERT(*x.line==*y.line,"Lines of merges LineSegmentSets don't match");
  LineSegmentSet res(*x.line);
  // standard merge algorithm with a few modifications to deal with overlapping lines
  itRange small(x.segments.begin(),x.segments.end());
  itRange large(y.segments.begin(),y.segments.end());
  while(small.first!=small.last && large.first != large.last){
    if(large.first->getBegin()<small.first->getBegin()){
      itRange tmp = small;
      small = large;
      large = tmp;
    }

    if(small.first->overlaps(*large.first)){
      // get all overlapping segments from larger part
      LineSegmentSet::set_t::iterator last = large.first;
      while(last!=large.last && small.first->overlaps(*last))++last;
      last--;
      // get the endpoint of the new segment to be inserted
      LinePoint lpEnd = max(small.first->getEnd(),last->getEnd());
      res.segments.insert(LineSegment(small.first->getBegin(),lpEnd));
      large.first=last; ++large.first; // all overlapping segments of the larger part have been dealt with
    }
    else{
      // no overlap, we can just insert
      res.segments.insert(*small.first);
    }
    ++small.first;
  }
  // deal with the rest of the iterator
  // we don't know which one is done, so we just do both
  res.segments.insert(small.first,small.last);
  res.segments.insert(large.first,large.last);
  return res;
}

LineSegmentSet intersect(const LineSegmentSet &x, const LineSegmentSet &y){
  typedef range<LineSegmentSet::set_t::iterator> itRange;
  ASSERT(*x.line==*y.line,"Lines of merges LineSegmentSets don't match");
  LineSegmentSet res(*x.line);
  itRange small(x.segments.begin(),x.segments.end());
  itRange large(y.segments.begin(),y.segments.end());

  while(small.first!=small.last && large.first != large.last){
    if(large.first->getBegin()<small.first->getBegin()){
      itRange tmp = small;
      small = large;
      large = tmp;
    }
    // add overlapping parts of segments from large
    while(large.first!=large.last && small.first->overlaps(*large.first)){
      LinePoint lpEnd = min(small.first->getEnd(),large.first->getEnd());
      res.segments.insert(LineSegment(large.first->getBegin(),lpEnd));
      ++large.first;
    }
    ++small.first;
  }
  return res;
}

LineSegmentSet invert(const LineSegmentSet &x){
  LineSegmentSet res(*x.line);
  LinePoint lpBegin = x.line->negEndpoint();
  for(LineSegmentSet::set_t::iterator iter = x.segments.begin();iter!=x.segments.end();++iter){
    if(lpBegin!=iter->getBegin())
      res.segments.insert(LineSegment(lpBegin,iter->getBegin()));
    // the end of the current scanned segment is the beginning of the next one to insert
    lpBegin = iter->getEnd();
  }
  // we might need to extend to infinity
  if(!lpBegin.isPosInfinity())
    res.segments.insert(LineSegment(lpBegin,x.line->posEndpoint()));
  return res;
}