/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield 
 *
 * This library is open source and may be redistributed and/or modified under  
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or 
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 * 
 * This library 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 
 * OpenSceneGraph Public License for more details.
*/

#include <osg/CoordinateSystemNode>
#include <osg/io_utils>
#include <osg/Geode>
#include <osg/Geometry>

#include <osgSim/ElevationSlice>

#include <osg/Notify>
#include <osgUtil/PlaneIntersector>

#include <osgDB/WriteFile>

using namespace osgSim;

namespace ElevationSliceUtils
{

struct DistanceHeightCalculator
{
    DistanceHeightCalculator(osg::EllipsoidModel* em, const osg::Vec3d& startPoint, osg::Vec3d& endPoint):
        _em(em),
        _startPoint(startPoint),
        _startNormal(startPoint),
        _endPoint(endPoint),
        _endNormal(endPoint)
    {
        double latitude, longitude, height;

        // set up start point variables
        _em->convertXYZToLatLongHeight(_startPoint.x(), _startPoint.y(), _startPoint.z(), latitude, longitude, height);
        _startRadius = _startPoint.length() - height;
        _startNormal.normalize();

        // set up end point variables
        _em->convertXYZToLatLongHeight(_endPoint.x(), _endPoint.y(), _endPoint.z(), latitude, longitude, height);
        _endRadius = _endPoint.length() - height;
        _endNormal.normalize();

        osg::Vec3d normal = _startNormal ^ _endNormal;
        normal.normalize();

        _angleIncrement = 0.005;
        
        _radiusList.push_back(_startRadius);        
        _distanceList.push_back(0.0);
        
        osg::Matrixd rotationMatrix;
        double angleBetweenStartEnd = acos( _startNormal * _endNormal );
        double prevRadius = _startRadius;
        double distance = 0.0;
        for(double angle = _angleIncrement;
            angle < angleBetweenStartEnd;
            angle += _angleIncrement)
        {
            rotationMatrix.makeRotate(angle, normal);
            osg::Vec3d newVector = osg::Matrixd::transform3x3(_startPoint, rotationMatrix);
            
            _em->convertXYZToLatLongHeight(newVector.x(), newVector.y(), newVector.z(), latitude, longitude, height);
            double newRadius = newVector.length() - height;
            
            double distanceIncrement = _angleIncrement * (newRadius + prevRadius) *0.5;
            distance  += distanceIncrement;
            
            _radiusList.push_back(newRadius);
            _distanceList.push_back(distance);
            
            // osg::notify(osg::INFO)<<"  newVector = "<<newVector<<" newRadius = "<<newRadius<<" distanceIncrement="<<distanceIncrement<<std::endl;
            
            prevRadius = newRadius;
        }

    }
    
        
    void computeDistanceHeight(const osg::Vec3d& v, double& distance, double& height) const
    {
        osg::Vec3d vNormal = v;
        vNormal.normalize();

        // compute the height at position
        double latitude, longitude;
        _em->convertXYZToLatLongHeight(v.x(), v.y(), v.z(),
                                       latitude, longitude, height);
                                       
        // compute the radius at the point
        double Rv = v.length() - height;

        // compute the angle from the _startPoint
        double alpha = acos( vNormal * _startNormal);
        
        unsigned int int_alpha = static_cast<unsigned int>(floor(alpha / _angleIncrement));
        if (int_alpha >= _distanceList.size())
        {
            int_alpha = _distanceList.size() - 1;
        }
        
        double prevAlpha = ((double)int_alpha) * _angleIncrement;
        double deltaAlpha = alpha - prevAlpha;
        double prevDistance = _distanceList[int_alpha];
        double prevRadius = _radiusList[int_alpha];
        
        double averageRadius = (prevRadius + Rv)*0.5;
        double distanceIncrement = deltaAlpha * averageRadius;
        
        distance = prevDistance + distanceIncrement;

#if 0
        double oldDistance = alpha * (_startRadius + Rv) *0.5;
        
        distance = oldDistance;

        osg::notify(osg::INFO)<<" new distance = "<<distance<<" old = "<<oldDistance<<" delta = "<<oldDistance-distance<<std::endl;
#endif

    }

    typedef std::vector<double> DoubleList;

    osg::ref_ptr<osg::EllipsoidModel>   _em;

    osg::Vec3d                          _startPoint;
    osg::Vec3d                          _startNormal;
    double                              _startRadius;

    osg::Vec3d                          _endPoint;
    osg::Vec3d                          _endNormal;
    double                              _endRadius;

    double                              _angleIncrement;
    
    DoubleList                          _radiusList;    
    DoubleList                          _distanceList;    

};

struct DistanceHeightXYZ
{

    DistanceHeightXYZ():
        distance(0.0),
        height(0.0) {}

    DistanceHeightXYZ(const DistanceHeightXYZ& dh):
        distance(dh.distance),
        height(dh.height),
        position(dh.position) {}

    DistanceHeightXYZ(double d, double h, const osg::Vec3d& pos):
        distance(d),
        height(h),
        position(pos) {}

    bool operator < (const DistanceHeightXYZ& rhs) const
    {
        // small distance values first
        if (distance < rhs.distance) return true;
        if (distance > rhs.distance) return false;
        
        // smallest heights first
        return (height < rhs.height);
    }
    
    bool operator == (const DistanceHeightXYZ& rhs) const
    {
        return distance==rhs.distance && height==rhs.height;
    }
    
    bool operator != (const DistanceHeightXYZ& rhs) const
    {
        return distance!=rhs.distance || height!=rhs.height;
    }
    
    bool equal_distance(const DistanceHeightXYZ& rhs, double epsilon=1e-6) const
    {
        return osg::absolute(rhs.distance - distance) <= epsilon;
    }

    double      distance;
    double      height;
    osg::Vec3d  position;
};

struct Point : public osg::Referenced, public DistanceHeightXYZ
{
    Point() {}
    Point(double d, double h, const osg::Vec3d& pos):
         DistanceHeightXYZ(d,h,pos) {}
    
    Point(const Point& point):
        osg::Referenced(),
        DistanceHeightXYZ(point) {}


};

struct Segment
{
    Segment(Point* p1, Point* p2)
    {
        if (*p1 < *p2)
        {
            _p1 = p1;
            _p2 = p2;
        }
        else
        {
            _p1 = p2;
            _p2 = p1;
        }
    }


    bool operator < ( const Segment& rhs) const
    {
        if (*_p1 < *rhs._p1) return true;
        if (*rhs._p1 < *_p1) return false;

        return (*_p2 < *rhs._p2);
    }
    
    enum Classification
    {
        UNCLASSIFIED,
        IDENTICAL,
        SEPERATE,
        JOINED,
        OVERLAPPING,
        ENCLOSING,
        ENCLOSED
    };
    
    Classification compare(const Segment& rhs) const
    {
        if (*_p1 == *rhs._p1 && *_p2==*rhs._p2) return IDENTICAL;

        const double epsilon = 1e-3; // 1mm

        if (fabs(_p2->distance - rhs._p1->distance) < epsilon)
        {
            if (fabs(_p2->height - rhs._p1->height) < epsilon) return JOINED;
            else return SEPERATE;
        }

        if (rhs._p2->distance < _p1->distance || _p2->distance < rhs._p1->distance) return SEPERATE;
        
        bool rhs_p1_inside = (_p1->distance <= rhs._p1->distance) && (rhs._p1->distance <= _p2->distance);
        bool rhs_p2_inside = (_p1->distance <= rhs._p2->distance) && (rhs._p2->distance <= _p2->distance);
        
        if (rhs_p1_inside && rhs_p2_inside) return ENCLOSING;
        
        bool p1_inside = (rhs._p1->distance <= _p1->distance) && (_p1->distance <= rhs._p2->distance);
        bool p2_inside = (rhs._p1->distance <= _p2->distance) && (_p2->distance <= rhs._p2->distance);

        if (p1_inside && p2_inside) return ENCLOSED;

        if (rhs_p1_inside || rhs_p2_inside || p1_inside || p2_inside) return OVERLAPPING;
        
        return UNCLASSIFIED;
    }
    
    double height(double d) const
    {
        double delta = (_p2->distance - _p1->distance);
        return _p1->height + ((_p2->height - _p1->height) * (d - _p1->distance) / delta);
    }
    
    double deltaHeight(Point& point) const
    {
        return point.height - height(point.distance);
    }
    
    Point* createPoint(double d) const
    {
        if (d == _p1->distance) return _p1.get();
        if (d == _p2->distance) return _p2.get();
        
        double delta = (_p2->distance - _p1->distance);
        double r = (d - _p1->distance)/delta;
        double one_minus_r = 1.0 - r;
        return new Point(d, 
                         _p1->height * one_minus_r + _p2->height * r,
                         _p1->position * one_minus_r + _p2->position * r);
    }

    osg::ref_ptr<Point> _p1;
    osg::ref_ptr<Point> _p2;

};


struct LineConstructor
{

    typedef std::set<Segment> SegmentSet;

    LineConstructor() {}
    
    
    
    void add(double d, double h, const osg::Vec3d& pos)
    {
        osg::ref_ptr<Point> newPoint = new Point(d,h,pos);
        
        
        
        if (_previousPoint.valid() && newPoint->distance != _previousPoint->distance)
        {
            const double maxGradient = 100.0;
            double gradient = fabs( (newPoint->height - _previousPoint->height) / (newPoint->distance - _previousPoint->distance) );
            
            if (gradient < maxGradient)
            {
                _segments.insert( Segment(_previousPoint.get(), newPoint.get()) );
            }
        }
        
        _previousPoint = newPoint;
    }
    
    void endline()
    {
        _previousPoint = 0;
    }
    
    void report()
    {
        osg::notify(osg::INFO)<<"Number of segments = "<<_segments.size()<<std::endl;
        
        SegmentSet::iterator prevItr = _segments.begin();
        SegmentSet::iterator nextItr = prevItr;
        ++nextItr;
        
        for(; 
            nextItr != _segments.end();
            ++nextItr,++prevItr)
        {
            Segment::Classification classification = prevItr->compare(*nextItr);
            switch(classification)
            {
                case(Segment::IDENTICAL): osg::notify(osg::INFO)<<"i"; break;
                case(Segment::SEPERATE): osg::notify(osg::INFO)<<"s"<<std::endl; break;
                case(Segment::JOINED): osg::notify(osg::INFO)<<"j"; break;
                case(Segment::OVERLAPPING): osg::notify(osg::INFO)<<"o"; break;
                case(Segment::ENCLOSING): osg::notify(osg::INFO)<<"E"; break;
                case(Segment::ENCLOSED): osg::notify(osg::INFO)<<"e"; break;
                case(Segment::UNCLASSIFIED): osg::notify(osg::INFO)<<"U"; break;
            }
            
        }

        osg::notify(osg::INFO)<<std::endl;
        
        for(SegmentSet::iterator itr = _segments.begin();
            itr != _segments.end();
            ++itr)
        {
            osg::notify(osg::INFO)<<numOverlapping(itr);
        }

        osg::notify(osg::INFO)<<std::endl;

#if 0
        for(SegmentSet::iterator itr = _segments.begin();
            itr != _segments.end();
            ++itr)
        {
            const Segment& s = *itr;
            osg::Vec3d p;
            

            double latitude, longitude, height;

            p = s._p1->position;
            _em->convertXYZToLatLongHeight(p.x(), p.y(), p.z(), latitude, longitude, height);
            double delta1 = height - s._p1->height;

            p = s._p1->position;
            _em->convertXYZToLatLongHeight(p.x(), p.y(), p.z(), latitude, longitude, height);
            double delta2 = height - s._p2->height;

            if (delta1>0.0 || delta2>0.0)
            {
                osg::notify(osg::INFO)<<"   "<<&s<<" computed height delta  ="<<delta1<<"  delta2= "<<delta2<<std::endl;
            }
        }
#endif
        
    }
    
    void pruneOverlappingSegments()
    {
        SegmentSet::iterator prevItr = _segments.begin();
        SegmentSet::iterator nextItr = prevItr;
        ++nextItr;
        
        double epsilon = 0.001;
        
        for(SegmentSet::iterator itr = _segments.begin();
            itr != _segments.end();
            ++itr)
        {
            SegmentSet::iterator nextItr = itr;
            ++nextItr;
            Segment::Classification classification = nextItr != _segments.end() ?  itr->compare(*nextItr) : Segment::UNCLASSIFIED;
            if (classification>=Segment::OVERLAPPING) osg::notify(osg::INFO)<<std::endl;
            else osg::notify(osg::INFO)<<".";
            osg::notify(osg::INFO).precision(12);
            while (classification>=Segment::OVERLAPPING)
            {
                switch(classification)
                {
                    case(Segment::OVERLAPPING):
                    {
                        // cases....
                        // compute new end points for both segments
                        // need to work out which points are overlapping - lhs_p2 && rhs_p1  or  lhs_p1 and rhs_p2
                        // also need to check for cross cases.

                        const Segment& lhs = *itr;
                        const Segment& rhs = *nextItr;

                        bool rhs_p1_inside = (lhs._p1->distance <= rhs._p1->distance) && (rhs._p1->distance <= lhs._p2->distance);
                        bool lhs_p2_inside = (rhs._p1->distance <= lhs._p2->distance) && (lhs._p2->distance <= rhs._p2->distance);

                        if (rhs_p1_inside && lhs_p2_inside)
                        {                    
                            double distance_between = osg::Vec2d(lhs._p2->distance - rhs._p1->distance, 
                                                                 lhs._p2->height - rhs._p1->height).length2();

                            if (distance_between < epsilon)
                            {
                                // osg::notify(osg::INFO)<<"OVERLAPPING : distance_between acceptable "<<distance_between<<std::endl;

                                Segment newSeg(lhs._p2.get(), rhs._p2.get());
                                _segments.insert(newSeg);
                                
                                _segments.erase(nextItr);
                                
                                nextItr = _segments.find(newSeg);
                            }
                            else
                            {
                                osg::notify(osg::INFO)<<"OVERLAPPING : distance_between unacceptable "<<distance_between<<std::endl;

                                double dh1 = lhs.deltaHeight(*rhs._p1);
                                double dh2 = -rhs.deltaHeight(*lhs._p2);

                                if (dh1 * dh2 < 0.0)
                                {
                                    osg::notify(osg::INFO)<<"OVERLAPPING : crossing "<<dh1<<" "<<dh2<<std::endl;
                                    osg::notify(osg::INFO)<<"    lhs_p1 "<<lhs._p1->distance<<" "<<lhs._p1->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    lhs_p2 "<<lhs._p2->distance<<" "<<lhs._p2->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    rhs_p1 "<<rhs._p1->distance<<" "<<rhs._p1->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    rhs_p2 "<<rhs._p2->distance<<" "<<rhs._p2->height<<std::endl;
                                    ++nextItr;
                                }
                                else if (dh1 <= 0.0 && dh2 <= 0.0)
                                {
                                    osg::notify(osg::INFO)<<"OVERLAPPING : lhs below rhs "<<dh1<<" "<<dh2<<std::endl;
                                    osg::notify(osg::INFO)<<"    lhs_p1 "<<lhs._p1->distance<<" "<<lhs._p1->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    lhs_p2 "<<lhs._p2->distance<<" "<<lhs._p2->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    rhs_p1 "<<rhs._p1->distance<<" "<<rhs._p1->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    rhs_p2 "<<rhs._p2->distance<<" "<<rhs._p2->height<<std::endl;
                                    ++nextItr;
                                }
                                else if (dh1 >= 0.0 && dh2 >= 0.0)
                                {
                                    osg::notify(osg::INFO)<<"OVERLAPPING : lhs above rhs "<<dh1<<" "<<dh2<<std::endl;
                                    osg::notify(osg::INFO)<<"    lhs_p1 "<<lhs._p1->distance<<" "<<lhs._p1->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    lhs_p2 "<<lhs._p2->distance<<" "<<lhs._p2->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    rhs_p1 "<<rhs._p1->distance<<" "<<rhs._p1->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    rhs_p2 "<<rhs._p2->distance<<" "<<rhs._p2->height<<std::endl;
                                    ++nextItr;
                                }
                                else
                                {
                                    osg::notify(osg::INFO)<<"OVERLAPPING : unidentified "<<dh1 <<" "<<dh2<<std::endl;
                                    osg::notify(osg::INFO)<<"    lhs_p1 "<<lhs._p1->distance<<" "<<lhs._p1->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    lhs_p2 "<<lhs._p2->distance<<" "<<lhs._p2->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    rhs_p1 "<<rhs._p1->distance<<" "<<rhs._p1->height<<std::endl;
                                    osg::notify(osg::INFO)<<"    rhs_p2 "<<rhs._p2->distance<<" "<<rhs._p2->height<<std::endl;
                                    ++nextItr;
                                }
                            }
                        }
                        else
                        {
                            osg::notify(osg::INFO)<<" OVERLAPPING problem, !rhs_p1_inside || !lhs_p2_inside - unhandled case" <<std::endl;
                            ++nextItr;
                        }


                        break;
                    }
                    case(Segment::ENCLOSING):
                    {
                        // need to work out if rhs is below lhs or rhs is above lhs or crossing

                        const Segment& enclosing = *itr;
                        const Segment& enclosed = *nextItr;
                        double dh1 = enclosing.deltaHeight(*enclosed._p1);
                        double dh2 = enclosing.deltaHeight(*enclosed._p2);

                        if (dh1<=epsilon && dh2<=epsilon)
                        {
                            // osg::notify(osg::INFO)<<"+++ ENCLOSING: ENCLOSING is above enclosed "<<dh1<<" "<<dh2<<std::endl;

                            _segments.erase(nextItr);

                            nextItr = itr;
                            ++nextItr;

                        }
                        else if (dh1>=0.0 && dh2>=0.0)
                        {
                            osg::notify(osg::INFO)<<"ENCLOSING: ENCLOSING is below enclosed "<<dh1<<" "<<dh2<<std::endl;
                            
                            osg::notify(osg::INFO)<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;

                            ++nextItr;
                        }
                        else if (dh1 * dh2 < 0.0)
                        {
                            osg::notify(osg::INFO)<<"ENCLOSING: ENCLOSING is crossing enclosed "<<dh1<<" "<<dh2<<std::endl;
                            
                            osg::notify(osg::INFO)<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;

                            ++nextItr;
                        }
                        else
                        {
                            osg::notify(osg::INFO)<<"ENCLOSING: ENCLOSING - not sure "<<dh1<<" "<<dh2<<std::endl;
                            
                            osg::notify(osg::INFO)<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
                            ++nextItr;
                        }

                        break;
                    }
                    case(Segment::ENCLOSED):
                    {
                        // need to work out if lhs is below rhs or lhs is above rhs or crossing
                        const Segment& enclosing = *nextItr;
                        const Segment& enclosed = *itr;
                        double dh1 = enclosing.deltaHeight(*enclosed._p1);
                        double dh2 = enclosing.deltaHeight(*enclosed._p2);

                        if (dh1<=epsilon && dh2<=epsilon)
                        {
                            // osg::notify(osg::INFO)<<"+++ ENCLOSED: ENCLOSING is above enclosed "<<dh1<<" "<<dh2<<std::endl;
                            _segments.erase(itr);

                            itr = nextItr;
                            ++nextItr;
                        }
                        else if (dh1>=0.0 && dh2>=0.0)
                        {
                            osg::notify(osg::INFO)<<"ENCLOSED: ENCLOSING is below enclosed "<<dh1<<" "<<dh2<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
                            ++nextItr;
                        }
                        else if (dh1 * dh2 < 0.0)
                        {
                            osg::notify(osg::INFO)<<"ENCLOSED: ENCLOSING is crossing enclosed "<<dh1<<" "<<dh2<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
                            ++nextItr;
                        }
                        else
                        {
                            osg::notify(osg::INFO)<<"ENCLOSED: ENCLOSING - not sure "<<dh1<<" "<<dh2<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
                            osg::notify(osg::INFO)<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
                            ++nextItr;
                        }

                        break;
                    }
                    default: 
                        ++nextItr;
                        break;
                }
                
                classification = nextItr != _segments.end() ?  itr->compare(*nextItr) : Segment::UNCLASSIFIED;
            }
        }
    }
    
    unsigned int numOverlapping(SegmentSet::iterator startItr)
    {
        if (startItr==_segments.end()) return 0;
    
        SegmentSet::iterator nextItr = startItr;
        ++nextItr;
        
        unsigned int numOverlapping = 0;
        while (nextItr!=_segments.end() && startItr->compare(*nextItr)>=Segment::OVERLAPPING)
        {
            ++numOverlapping;
            ++nextItr;
        }
        return numOverlapping;
    }
    
    void copyPoints(ElevationSlice::Vec3dList& intersections, ElevationSlice::DistanceHeightList& distanceHeightIntersections)
    {
        SegmentSet::iterator prevItr = _segments.begin();
        SegmentSet::iterator nextItr = prevItr;
        ++nextItr;
        
        intersections.push_back( prevItr->_p1->position );
        distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(prevItr->_p1->distance, prevItr->_p1->height) );
        
        intersections.push_back( prevItr->_p2->position );
        distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(prevItr->_p2->distance, prevItr->_p2->height) );

        for(; 
            nextItr != _segments.end();
            ++nextItr,++prevItr)
        {
            Segment::Classification classification = prevItr->compare(*nextItr);
            switch(classification)
            {
                case(Segment::SEPERATE):
                {
                    intersections.push_back( nextItr->_p1->position );
                    distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p1->distance, nextItr->_p1->height) );
        
                    intersections.push_back( nextItr->_p2->position );
                    distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p2->distance, nextItr->_p2->height) );
                    break;
                }
                case(Segment::JOINED): 
                {
                    intersections.push_back( nextItr->_p2->position );
                    distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p2->distance, nextItr->_p2->height) );
                    break;
                }
                default : break;
            }
            
        }
        
    }
    
    SegmentSet _segments;
    osg::ref_ptr<Point> _previousPoint;
    osg::Plane  _plane;
    osg::ref_ptr<osg::EllipsoidModel>   _em;
    
};

}

ElevationSlice::ElevationSlice()
{
    setDatabaseCacheReadCallback(new DatabaseCacheReadCallback);
}

void ElevationSlice::computeIntersections(osg::Node* scene)
{
    osg::CoordinateSystemNode* csn = dynamic_cast<osg::CoordinateSystemNode*>(scene);
    osg::EllipsoidModel* em = csn ? csn->getEllipsoidModel() : 0;


    osg::Plane plane;
    osg::Polytope boundingPolytope;

    if (em)
    {

        osg::Vec3d start_upVector = em->computeLocalUpVector(_startPoint.x(), _startPoint.y(), _startPoint.z());
        osg::Vec3d end_upVector = em->computeLocalUpVector(_endPoint.x(), _endPoint.y(), _endPoint.z());

        double start_latitude, start_longitude, start_height;
        em->convertXYZToLatLongHeight(_startPoint.x(), _startPoint.y(), _startPoint.z(),
                                      start_latitude, start_longitude, start_height);

        osg::notify(osg::INFO)<<"start_lat = "<<start_latitude<<" start_longitude = "<<start_longitude<<" start_height = "<<start_height<<std::endl;

        double end_latitude, end_longitude, end_height;
        em->convertXYZToLatLongHeight(_endPoint.x(), _endPoint.y(), _endPoint.z(),
                                      end_latitude, end_longitude, end_height);

        osg::notify(osg::INFO)<<"end_lat = "<<end_latitude<<" end_longitude = "<<end_longitude<<" end_height = "<<end_height<<std::endl;
        
        // set up the main intersection plane
        osg::Vec3d planeNormal = (_endPoint - _startPoint) ^ start_upVector;
        planeNormal.normalize();        
        plane.set( planeNormal, _startPoint );
        
        // set up the start cut off plane
        osg::Vec3d startPlaneNormal = start_upVector ^ planeNormal;
        startPlaneNormal.normalize();
        boundingPolytope.add( osg::Plane(startPlaneNormal, _startPoint) );
        
        // set up the end cut off plane
        osg::Vec3d endPlaneNormal = planeNormal ^ end_upVector;
        endPlaneNormal.normalize();
        boundingPolytope.add( osg::Plane(endPlaneNormal, _endPoint) );
    }
    else
    {
        osg::Vec3d upVector (0.0, 0.0, 1.0);

        // set up the main intersection plane
        osg::Vec3d planeNormal = (_endPoint - _startPoint) ^ upVector;
        planeNormal.normalize();        
        plane.set( planeNormal, _startPoint );
        
        // set up the start cut off plane
        osg::Vec3d startPlaneNormal = upVector ^ planeNormal;
        startPlaneNormal.normalize();
        boundingPolytope.add( osg::Plane(startPlaneNormal, _startPoint) );
        
        // set up the end cut off plane
        osg::Vec3d endPlaneNormal = planeNormal ^ upVector;
        endPlaneNormal.normalize();
        boundingPolytope.add( osg::Plane(endPlaneNormal, _endPoint) );
    }
    
    osg::ref_ptr<osgUtil::PlaneIntersector> intersector = new osgUtil::PlaneIntersector(plane, boundingPolytope);


    intersector->setRecordHeightsAsAttributes(true);
    intersector->setEllipsoidModel(em);

    _intersectionVisitor.reset();
    _intersectionVisitor.setIntersector( intersector.get() );
    
    scene->accept(_intersectionVisitor);
    
    osgUtil::PlaneIntersector::Intersections& intersections = intersector->getIntersections();

    typedef osgUtil::PlaneIntersector::Intersection::Polyline Polyline;
    typedef osgUtil::PlaneIntersector::Intersection::Attributes Attributes;

    if (!intersections.empty())
    {

        osgUtil::PlaneIntersector::Intersections::iterator itr;
        for(itr = intersections.begin();
            itr != intersections.end();
            ++itr)
        {
            osgUtil::PlaneIntersector::Intersection& intersection = *itr;

            if (intersection.matrix.valid())
            {
                // osg::notify(osg::INFO)<<"  transforming "<<std::endl;
                // transform points on polyline 
                for(Polyline::iterator pitr = intersection.polyline.begin();
                    pitr != intersection.polyline.end();
                    ++pitr)
                {
                    *pitr = (*pitr) * (*intersection.matrix);
                }
                
                // matrix no longer needed.
                intersection.matrix = 0;
            }
        }

#if 0
        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
        
        for(itr = intersections.begin();
            itr != intersections.end();
            ++itr)
        {

            osgUtil::PlaneIntersector::Intersection& intersection = *itr;
            osg::Geometry* geometry = new osg::Geometry;

            osg::Vec3Array* vertices = new osg::Vec3Array;
            vertices->reserve(intersection.polyline.size());
            for(Polyline::iterator pitr = intersection.polyline.begin();
                pitr != intersection.polyline.end();
                ++pitr)
            {
                vertices->push_back(*pitr);
            }

            geometry->setVertexArray( vertices );
            geometry->addPrimitiveSet( new osg::DrawArrays(GL_LINE_STRIP, 0, vertices->size()) );
            
            osg::Vec4Array* colours = new osg::Vec4Array;
            colours->push_back( osg::Vec4(1.0f,1.0f,1.0f,1.0f) );
            
            geometry->setColorArray( colours );

            geode->addDrawable( geometry );
            geode->getOrCreateStateSet()->setMode( GL_LIGHTING, osg::StateAttribute::OFF );
        }
        
        osgDB::writeNodeFile(*geode, "raw.osg");
#endif

        ElevationSliceUtils::LineConstructor constructor;
        constructor._plane = plane;
        constructor._em = em;

        if (em)
        {
            ElevationSliceUtils::DistanceHeightCalculator dhc(em, _startPoint, _endPoint);

            // convert into distance/height
            for(itr = intersections.begin();
                itr != intersections.end();
                ++itr)
            {
                osgUtil::PlaneIntersector::Intersection& intersection = *itr;
                
                if (intersection.attributes.size()!=intersection.polyline.size()) continue;
                
                Attributes::iterator aitr = intersection.attributes.begin();
                for(Polyline::iterator pitr = intersection.polyline.begin();
                    pitr != intersection.polyline.end();
                    ++pitr, ++aitr)
                {
                    const osg::Vec3d& v = *pitr;
                    double distance, height;
                    dhc.computeDistanceHeight(v, distance, height);
                    
                    double pi_height = *aitr;
                    
                    // osg::notify(osg::INFO)<<"computed height = "<<height<<" PI height = "<<pi_height<<std::endl;

                    constructor.add( distance, pi_height, v);

                }
                constructor.endline();
            }


        }
        else
        {
            // convert into distance/height
            for(itr = intersections.begin();
                itr != intersections.end();
                ++itr)
            {
                osgUtil::PlaneIntersector::Intersection& intersection = *itr;
                for(Polyline::iterator pitr = intersection.polyline.begin();
                    pitr != intersection.polyline.end();
                    ++pitr)
                {
                    const osg::Vec3d& v = *pitr;
                    osg::Vec2d delta_xy( v.x() - _startPoint.x(), v.y() - _startPoint.y());
                    double distance = delta_xy.length();

                    constructor.add( distance, v.z(), v);
                }
                constructor.endline();
            }
        } 

        // copy final results
        _intersections.clear();
        _distanceHeightIntersections.clear();

        constructor.report();
        constructor.pruneOverlappingSegments();
        constructor.report();

        constructor.copyPoints(_intersections, _distanceHeightIntersections);
        
#if 0
        {
            osg::ref_ptr<osg::Geode> geode = new osg::Geode;

            osg::Geometry* geometry = new osg::Geometry;

            osg::Vec3Array* vertices = new osg::Vec3Array;
            vertices->reserve(_intersections.size());
            for(Vec3dList::iterator pitr = _intersections.begin();
                pitr != _intersections.end();
                ++pitr)
            {
                vertices->push_back(*pitr);
            }

            geometry->setVertexArray( vertices );
            geometry->addPrimitiveSet( new osg::DrawArrays(GL_LINE_STRIP, 0, _intersections.size()) );

            osg::Vec4Array* colours = new osg::Vec4Array;
            colours->push_back( osg::Vec4(1.0f,0.5f,0.5f,1.0f) );

            geometry->setColorArray( colours );

            geode->addDrawable( geometry );
            geode->getOrCreateStateSet()->setMode( GL_LIGHTING, osg::StateAttribute::OFF );

            osgDB::writeNodeFile(*geode, "processed.osg");
        }
#endif

    }
    else
    {
        osg::notify(osg::INFO)<<"No intersections found."<<std::endl;
    }
    
}

ElevationSlice::Vec3dList ElevationSlice::computeElevationSlice(osg::Node* scene, const osg::Vec3d& startPoint, const osg::Vec3d& endPoint)
{
    ElevationSlice es;
    es.setStartPoint(startPoint);
    es.setEndPoint(endPoint);
    es.computeIntersections(scene);
    return es.getIntersections();
}

void ElevationSlice::setDatabaseCacheReadCallback(DatabaseCacheReadCallback* dcrc)
{
    _dcrc = dcrc;
    _intersectionVisitor.setReadCallback(dcrc);
}