#include <iostream>
#include <cmath>
#include <utility>

#include "IMeshPoint.hxx"

COMET::IMeshPoint::IMeshPoint() {}

COMET::IMeshPoint::~IMeshPoint() {
    // This is truly weird code... BE AWARE the COMET::IMeshEdge object will remove
    // itself from fEdges when it is deleted.
    while (!fEdges.empty()) {
        COMET::IMeshEdge *e = *fEdges.begin();
        fEdges.erase(e);
        delete e;
    }
}

COMET::IMeshXYPoint::~IMeshXYPoint() {}

bool COMET::IMeshPoint::operator < (const COMET::IMeshPoint& rhs) const {
    if (this == &rhs) return false;
    if (X() < rhs.X()) return true;
    if (X() > rhs.X()) return false;
    if (Y() < rhs.Y()) return true;
    return false;
}

void COMET::IMeshPoint::AddEdge(COMET::IMeshEdge* edge) {
    fEdges.insert(edge);
}

void COMET::IMeshPoint::RemoveEdge(COMET::IMeshEdge* edge) {
    fEdges.erase(edge);
}

static bool findToBeChecked(const std::deque<COMET::IMeshPoint::IPathPoint*>& points,
                         const COMET::IMeshPoint* point) {
    for (std::deque<COMET::IMeshPoint::IPathPoint*>::const_iterator p = points.begin();
         p != points.end();
         ++p) {
        if ((*p)->GetPoint() == point) return true;
    }
    return false;
}

static bool findAlreadyChecked(const std::set<COMET::IMeshPoint::IPathPoint*>& points,
                         const COMET::IMeshPoint* point) {
    for (std::set<COMET::IMeshPoint::IPathPoint*>::const_iterator p = points.begin();
         p != points.end();
         ++p) {
        if ((*p)->GetPoint() == point) return true;
    }
    return false;
}

COMET::IMeshPoint::PathVector COMET::IMeshPoint::GetPaths(const COMET::IMeshPoint* end,
                                            int maxPaths,
                                            int maxSteps) {
    std::deque<COMET::IMeshPoint::IPathPoint*> toBeChecked;
    std::set<COMET::IMeshPoint::IPathPoint*> pointsChecked;

    // Add the beginning point to the deque to get the entire process started.
    IPathPoint* firstPoint = new IPathPoint(this,NULL);
    toBeChecked.push_back(firstPoint);

    maxPaths = std::min(maxPaths,end->GetEdgeCount());
    
    // Check for complete paths.  All completed paths that are found will be
    // saved in completedPaths.
    std::vector<COMET::IMeshPoint::IPathPoint*> completedPaths;
    while (0<toBeChecked.size()) {
        // Get the new point to be checked, and see if we've looked at it
        // before.
        IPathPoint *current = toBeChecked.front();
        toBeChecked.pop_front();
        if (pointsChecked.find(current) != pointsChecked.end()) continue;
        pointsChecked.insert(current);
        // Check that the path isn't too long.
        if (current->GetLength() > maxSteps) continue;
        // Add the neighbors of the current point to the toBeChecked deque.
        for (COMET::IMeshPoint::EdgeIterator e = current->GetPoint()->BeginEdges(); 
             e != current->GetPoint()->EndEdges(); 
             ++e) {
            COMET::IMeshPoint *otherEnd = (*e)->GetOther(current->GetPoint());
            if (otherEnd == end) {
                // We've found a complete path.  Save the path, and don't look
                // further in this direction.  The new point doesn't get added
                // to the "pointsChecked" set.
                completedPaths.push_back(new IPathPoint(otherEnd,current));
            }
            else {
                if (!findToBeChecked(toBeChecked,otherEnd)
                    && !findAlreadyChecked(pointsChecked,otherEnd)) {
                    toBeChecked.push_back(new IPathPoint(otherEnd,current));
                }
            }
        }
        if (maxPaths <= (int) completedPaths.size()) break;
    }
    
    // We've looked at all of the possible paths.  The completed paths are
    // saved in the completedPaths vector.  Save the points in completed paths
    // for return.  This also deletes the IPathPoint objects allocated to
    // completedPaths. 
    PathVector pathsFound;
    for (std::vector<COMET::IMeshPoint::IPathPoint*>::iterator 
             p = completedPaths.begin();
         p != completedPaths.end();
         ++p) {
        PointVector path;
        for (const COMET::IMeshPoint::IPathPoint* t = *p; t!= NULL; t = t->GetPath()) {
            path.push_back(t->GetPoint());
        }
        pathsFound.push_back(path);
        delete (*p);
    }

    // Clean up the memory for the set.
    for (std::set<COMET::IMeshPoint::IPathPoint*>::iterator p = pointsChecked.begin();
         p != pointsChecked.end();
         ++p) {
        delete (*p);
    }

    return pathsFound;
}

COMET::IMeshPoint::Walker COMET::IMeshPoint::BeginPoints() {
    return COMET::IMeshPoint::Walker(this);
}

COMET::IMeshPoint::Walker COMET::IMeshPoint::fEnd;
COMET::IMeshPoint::Walker& COMET::IMeshPoint::EndPoints() {
    return fEnd;
}

COMET::IMeshPoint::Walker::Walker() {}

COMET::IMeshPoint::Walker::Walker(COMET::IMeshPoint* node) {
    fQueue.push(node);
    fSeen.insert(node);
    PackQueue();
}

COMET::IMeshPoint::Walker::Walker(const COMET::IMeshPoint::Walker& i) 
    : fQueue(i.fQueue), fSeen(i.fSeen) { }

COMET::IMeshPoint::Walker::~Walker() {}

COMET::IMeshPoint* COMET::IMeshPoint::Walker::operator *() const {
    return fQueue.front();
}

COMET::IMeshPoint::Walker& COMET::IMeshPoint::Walker::operator ++() {
    fQueue.pop();
    PackQueue();
    return *this;
}

bool COMET::IMeshPoint::Walker::operator == (COMET::IMeshPoint::Walker& rhs) {
    if (fQueue.size()<1 && rhs.fQueue.size()<1) return true;
    if (fQueue.size()<1) return false;
    if (rhs.fQueue.size()<1) return false;
    return (fQueue.front() == rhs.fQueue.front());
}

bool COMET::IMeshPoint::Walker::operator != (COMET::IMeshPoint::Walker &rhs) {
    return !(operator ==(rhs));
}

void COMET::IMeshPoint::Walker::PackQueue() {
    if (fQueue.size()<1) return;
    COMET::IMeshPoint* t = fQueue.front();
    for (COMET::IMeshPoint::EdgeIterator e = t->BeginEdges(); 
         e != t->EndEdges(); 
         ++e) {
        COMET::IMeshPoint *other = (*e)->GetOther(t);
        if (fSeen.find(other) != fSeen.end()) continue;
        fQueue.push(other);
        fSeen.insert(other);
    }
}

std::ostream& operator << (std::ostream& s, COMET::IMeshPoint& p) {
    s << &p << "->(" << p.X() << ", " << p.Y() << ")";
    for (COMET::IMeshPoint::EdgeIterator e = p.BeginEdges();
         e != p.EndEdges();
         ++e) {
        s << " " << *e;
    }
    return s;
}