/*
 * LocationNode.h
 *
 *  Created on: Jan 6, 2014
 *      Author: godin
 */

#ifndef LOCATIONNODE_H_
#define LOCATIONNODE_H_

#include "data/Location.h"
#include "utils/FastMap.h"

class LocationNode: public Location
{
public:

    enum NodeColor
    {
        WHITE,
        GRAY,
        BLACK
    };

    LocationNode( double latitude, double longitude, int latIndex, int lonIndex )
        : Location( Units::RADIAN, latitude, longitude ),
          latIndex_( latIndex ),
          lonIndex_( lonIndex ),
          cameFrom_( NULL ),
          nodeColor_( WHITE ),
          dScore_( INFINITY ),
          fScore_( INFINITY )
    {}

    void setDScoreAndHeuristic( float dScore, float heuristic )
    {
        dScore_ = dScore;
        fScore_ = dScore + heuristic;
    }

    int getLatIndex() const
    {
        return latIndex_;
    }

    int getLonIndex() const
    {
        return lonIndex_;
    }

    void setNodeColor( NodeColor nodeColor )
    {
        nodeColor_ = nodeColor;
    }

    void setCameFrom( LocationNode* cameFrom, float dScore, float heuristic, FastMap<const float, LocationNode*>& nodeQueue )
    {
        if( NULL != cameFrom && nodeColor_ == GRAY )
        {
            removeFromQueue( nodeQueue );
        }
        else if( NULL != cameFrom )
        {
            nodeColor_ = GRAY;
        }
        else
        {
            nodeColor_ = WHITE;
        }

        cameFrom_ = cameFrom;
        dScore_ = dScore;
        fScore_ = dScore_ + heuristic;

        if( NULL != cameFrom )
        {
            nodeQueue.put( fScore_, this );
        }
    }


    const double getLat() const
    {
        return lat_;
    }

    const double getLon() const
    {
        return lon_;
    }

    LocationNode* getCameFrom( )
    {
        return cameFrom_;
    }

    float getDScore()
    {
        return dScore_;
    }

    float getFScore()
    {
        return fScore_;
    }

    void removeFromQueue( FastMap<const float, LocationNode*>& nodeQueue )
    {
        int index = nodeQueue.findIndex( fScore_ );
        if( index == -1 )
        {
            // Not in the queue at all
            return;
        }
        if( nodeQueue.getIndexed( index ) == this )
        {
            // Uniquely at the index
            nodeQueue.popIndexed( index );
            return;
        }
        // Look forward
        for( int i = index + 1; i < ( int ) nodeQueue.size(); ++i )
        {
            if( nodeQueue.getIndexed( i ) == this )
            {
                nodeQueue.popIndexed( i );
                return;
            }
            else if( nodeQueue.getIndexedEntry( i )->getKey() != fScore_ )
            {
                break;
            }
        }
        // Look backward
        for( int i = index - 1; i >= 0; --i )
        {
            if( nodeQueue.getIndexed( i ) == this )
            {
                nodeQueue.popIndexed( i );
                return;
            }
            else if( nodeQueue.getIndexedEntry( i )->getKey() != fScore_ )
            {
                break;
            }
        }

    }

private:
    // Note that the copy constructor below is private and not given a body.
    // Any attempt to call it will return a compiler error.
    LocationNode( const LocationNode& old ); // disallow copy constructor

    int latIndex_;
    int lonIndex_;
    LocationNode* cameFrom_;
    NodeColor nodeColor_;
    float dScore_;
    float fScore_;
};

#endif /* LOCATIONNODE_H_ */