/**********************************************************************
 *
 * GEOS - Geometry Engine Open Source
 * http://geos.osgeo.org
 *
 * Copyright (C) 2009-2010  Sandro Santilli <strk@keybit.net>
 * Copyright (C) 2006 Refractions Research Inc.
 *
 * This is free software; you can redistribute and/or modify it under
 * the terms of the GNU Lesser General Public Licence as published
 * by the Free Software Foundation. 
 * See the COPYING file for more information.
 *
 ***********************************************************************
 *
 * Last port: operation/overlay/snap/LineStringSnapper.java r320 (JTS-1.12)
 *
 * NOTE: algorithm changed to improve output quality by reducing 
 *       probability of self-intersections
 *
 **********************************************************************/

#include <geos/operation/overlay/snap/LineStringSnapper.h>
#include <geos/geom/CoordinateSequence.h>
#include <geos/geom/Coordinate.h>
#include <geos/geom/CoordinateList.h>
#include <geos/util/UniqueCoordinateArrayFilter.h>
#include <geos/geom/LineSegment.h>

#include <vector>
#include <memory>

#ifndef GEOS_DEBUG
#define GEOS_DEBUG 0
#endif

#if GEOS_DEBUG
#include <iostream>
#include <iomanip>
using std::cerr;
using std::endl;
#endif

//using namespace std;
using namespace geos::geom;

namespace geos {
namespace operation { // geos.operation
namespace overlay { // geos.operation.overlay
namespace snap { // geos.operation.overlay.snap

/*public*/
std::auto_ptr<Coordinate::Vect>
LineStringSnapper::snapTo(const geom::Coordinate::ConstVect& snapPts)
{
	geom::CoordinateList coordList(srcPts);

	snapVertices(coordList, snapPts);
	snapSegments(coordList, snapPts);

	return coordList.toCoordinateArray();
}

/*private*/
CoordinateList::iterator
LineStringSnapper::findVertexToSnap(
			const Coordinate& snapPt,
			CoordinateList::iterator from,
			CoordinateList::iterator too_far)
{
	double minDist = snapTolerance; // make sure the first closer then
	                                // snapTolerance is accepted
	CoordinateList::iterator match=too_far;

	for ( ; from != too_far; ++from)
	{
    Coordinate& c0 = *from;

#if GEOS_DEBUG
cerr << " Checking vertex " << c0 << endl;
#endif

		double dist = c0.distance(snapPt);
		if ( dist >= minDist) {
#if GEOS_DEBUG
cerr << "   snap point distance " << dist
     << " not smaller than tolerance "
     << snapTolerance << " or previous closest "
     << minDist << endl;
#endif
      continue;
    }

#if GEOS_DEBUG
    cerr << "   snap point distance " << dist << " within tolerance "
         << snapTolerance << " and closer than previous candidate "
         << minDist << endl;
#endif

    if ( dist == 0.0 ) return from; // can't find any closer

    match = from;
    minDist = dist;

	}

	return match;
}


/*private*/
void
LineStringSnapper::snapVertices(geom::CoordinateList& srcCoords,
			const geom::Coordinate::ConstVect& snapPts)
{
  // nothing to do if there are no source coords..
  if ( srcCoords.empty() ) return;

#if GEOS_DEBUG
cerr << "Snapping vertices of: " << srcCoords << endl;
#endif

	for ( Coordinate::ConstVect::const_iterator
			it=snapPts.begin(), end=snapPts.end();
			it != end;
			++it)
	{
		assert(*it);
		const Coordinate& snapPt = *(*it);

#if GEOS_DEBUG
cerr << "Checking for a vertex to snap to snapPt " << snapPt << endl;
#endif

		CoordinateList::iterator too_far = srcCoords.end();
    if ( isClosed ) --too_far;
		CoordinateList::iterator vertpos =
			findVertexToSnap(snapPt, srcCoords.begin(), too_far);
		if ( vertpos == too_far)
		{
#if GEOS_DEBUG
cerr << " No vertex to snap" << endl;
#endif
			continue;
		}

#if GEOS_DEBUG
cerr << " Vertex to be snapped found, snapping" << endl;
#endif
    *vertpos = snapPt;

    // keep final closing point in synch (rings only)
    if (vertpos == srcCoords.begin() && isClosed)
    {
      vertpos = srcCoords.end(); --vertpos;
      *vertpos = snapPt;
    }

	}

#if GEOS_DEBUG
cerr << " After vertex snapping, srcCoors are: " << srcCoords << endl;
#endif

}

/*private*/
Coordinate::ConstVect::const_iterator
LineStringSnapper::findSnapForVertex(const Coordinate& pt,
			const Coordinate::ConstVect& snapPts)
{
	Coordinate::ConstVect::const_iterator end = snapPts.end();
	Coordinate::ConstVect::const_iterator candidate = end;
	double minDist = snapTolerance;

	// TODO: use std::find_if
	for ( Coordinate::ConstVect::const_iterator
			it=snapPts.begin();
			it != end;
			++it)
	{
		assert(*it);
		const Coordinate& snapPt = *(*it);

		if ( snapPt.equals2D(pt) )
		{
#if GEOS_DEBUG
cerr << " points are equal, returning not-found " << endl;
#endif
			return end;
		}

		double dist = snapPt.distance(pt);
#if GEOS_DEBUG
cerr << " distance from snap point " << snapPt << ": " << dist << endl;
#endif

		if ( dist < minDist )
		{
      minDist = dist;
      candidate = it;
		}
	}

#if GEOS_DEBUG
  if ( candidate == end ) {
cerr << " no snap point within distance, returning not-found" << endl;
  }
#endif

	return candidate;
}


/*private*/
void
LineStringSnapper::snapSegments(geom::CoordinateList& srcCoords,
			const geom::Coordinate::ConstVect& snapPts)
{

  // nothing to do if there are no source coords..
  if ( srcCoords.empty() ) return;

#if GEOS_DEBUG
cerr << "Snapping segments of: " << srcCoords << endl;
#endif

	for ( Coordinate::ConstVect::const_iterator
			it=snapPts.begin(), end=snapPts.end();
			it != end;
			++it)
	{
		assert(*it);
		const Coordinate& snapPt = *(*it);

#if GEOS_DEBUG
cerr << "Checking for a segment to snap to snapPt " << snapPt << endl;
#endif

		CoordinateList::iterator too_far = srcCoords.end(); --too_far;
		CoordinateList::iterator segpos =
			findSegmentToSnap(snapPt, srcCoords.begin(), too_far);
		if ( segpos == too_far)
		{
#if GEOS_DEBUG
cerr << " No segment to snap" << endl;
#endif
			continue;
		}

    /* Check if the snap point falls outside of the segment */
    // If the snap point is outside, this means that an endpoint
    // was not snap where it should have been
    // so what we should do is re-snap the endpoint to this
    // snapPt and then snap the closest between this and
    // previous (for pf < 0.0) or next (for pf > 1.0) segment
    // to the old endpoint.
    //     --strk May 2013
    //
    // TODO: simplify this code, make more readable
    //
    CoordinateList::iterator to = segpos; ++to;
    LineSegment seg(*segpos, *to);
    double pf = seg.projectionFactor(snapPt);
    if ( pf >= 1.0 ) {
#if GEOS_DEBUG
      cerr << " Segment to be snapped is closer on his end point" << endl;
#endif
      Coordinate newSnapPt = seg.p1;
      *to = seg.p1 = snapPt;
      // now snap from-to (segpos) or to-next (segpos++) to newSnapPt
      if ( to == too_far ) {
        if ( isClosed ) { 
#if GEOS_DEBUG
          cerr << " His end point is the last one, but is closed " << endl;
#endif
          *(srcCoords.begin()) = snapPt; // sync to start point
          to = srcCoords.begin();
        } else {
#if GEOS_DEBUG
          cerr << " His end point is the last one, inserting " << newSnapPt << " before it" << endl;
#endif
          srcCoords.insert(to, newSnapPt);
          continue;
        }
      }
      ++to;
      LineSegment nextSeg(seg.p1, *to);
      if ( nextSeg.distance(newSnapPt) < seg.distance(newSnapPt) ) {
#if GEOS_DEBUG
        cerr << " Next segment closer, inserting " << newSnapPt << " into " << nextSeg << endl;
#endif
        // insert into next segment
        srcCoords.insert(to, newSnapPt);
      } else {
#if GEOS_DEBUG
        cerr << " This segment closer, inserting " << newSnapPt << " into " << seg << endl;
#endif
        // insert must happen one-past first point (before next point)
        ++segpos;
        srcCoords.insert(segpos, newSnapPt);
      }
    }
    else if ( pf <= 0.0 ) {
#if GEOS_DEBUG
      cerr << " Segment to be snapped is closer on his start point" << endl;
#endif
      Coordinate newSnapPt = seg.p0;
      *segpos = seg.p0 = snapPt;
      // now snap prev-from (--segpos) or from-to (segpos) to newSnapPt
      if ( segpos == srcCoords.begin() ) {
        if ( isClosed ) { 
#if GEOS_DEBUG
          cerr << " His start point is the first one, but is closed " << endl;
#endif
          segpos = srcCoords.end(); --segpos;
          *segpos = snapPt; // sync to end point
        } else {
#if GEOS_DEBUG
          cerr << " His start point is the first one, inserting " << newSnapPt << " before it" << endl;
#endif
          ++segpos;
          srcCoords.insert(segpos, newSnapPt);
          continue;
        }
      }

#if GEOS_DEBUG
cerr << " Before seg-snapping, srcCoors are: " << srcCoords << endl;
#endif

      --segpos;
      LineSegment prevSeg(*segpos, seg.p0);
      if ( prevSeg.distance(newSnapPt) < seg.distance(newSnapPt) ) {
#if GEOS_DEBUG
        cerr << " Prev segment closer, inserting " << newSnapPt << " into " << prevSeg << endl;
#endif
        // insert into prev segment
        srcCoords.insert(segpos, newSnapPt);
      } else {
#if GEOS_DEBUG
        cerr << " This segment closer, inserting " << newSnapPt << " into " << seg << endl;
#endif
        // insert must happen one-past first point (before next point)
        srcCoords.insert(to, newSnapPt);
      }
    }
    else {
      //assert(pf != 0.0);
#if GEOS_DEBUG
cerr << " Segment to be snapped found, projection factor is " << pf << ", inserting point" << endl;
#endif
      // insert must happen one-past first point (before next point)
      ++segpos;
      srcCoords.insert(segpos, snapPt);
    }
	}

#if GEOS_DEBUG
cerr << " After segment snapping, srcCoors are: " << srcCoords << endl;
#endif

}

/*private*/
/* NOTE: this is called findSegmentIndexToSnap in JTS */
CoordinateList::iterator
LineStringSnapper::findSegmentToSnap(
			const Coordinate& snapPt,
			CoordinateList::iterator from,
			CoordinateList::iterator too_far)
{
	LineSegment seg;
	double minDist = snapTolerance; // make sure the first closer then
	                                // snapTolerance is accepted
	CoordinateList::iterator match=too_far;

	// TODO: use std::find_if
	for ( ; from != too_far; ++from)
	{
		seg.p0 = *from; 
		CoordinateList::iterator to = from;
		++to;
		seg.p1 = *to;

#if GEOS_DEBUG
cerr << " Checking segment " << seg << endl;
#endif

		/**
		 * Check if the snap pt is equal to one of
		 * the segment endpoints.
		 *
		 * If the snap pt is already in the src list,
		 * don't snap at all (unless allowSnappingToSourceVertices
		 * is set to true)
		 */
		if ( seg.p0.equals2D(snapPt) || seg.p1.equals2D(snapPt) )
		{

			if (allowSnappingToSourceVertices) {
#if GEOS_DEBUG
cerr << "   snap point matches a segment endpoint, checking next segment"
     << endl;
#endif
				continue;
			} else {
#if GEOS_DEBUG
cerr << "   snap point matches a segment endpoint, giving up seek" << endl;
#endif
				return too_far;
			}
		}

		double dist = seg.distance(snapPt);
		if ( dist >= minDist) {
#if GEOS_DEBUG
cerr << "   snap point distance " << dist
     << " not smaller than tolerance "
     << snapTolerance << " or previous closest "
     << minDist << endl;
#endif
      continue;
    }

#if GEOS_DEBUG
    cerr << "   snap point distance " << dist << " within tolerance "
         << snapTolerance << " and closer than previous candidate "
         << minDist << endl;
#endif

    if ( dist == 0.0 ) return from; // can't find any closer

    match = from;
    minDist = dist;

	}

	return match;
}

} // namespace geos.operation.snap
} // namespace geos.operation.overlay
} // namespace geos.operation
} // namespace geos