#include ".\astarmodified.h"

AStarModified::AStarModified(void) : PathAlgorithm(){
}
/*
AStarModified::AStarModified(POINT* origin, POINT* destination) : PathAlgorithm(origin, destination){
}
AStarModified::AStarModified(Node* root, POINT* destination, vector<Node*>* open, vector<Node*>* closed, vector<Node*>* solution)
              : PathAlgorithm(root, destination, open, closed, solution){
}
*/
AStarModified::~AStarModified(void){
}

void AStarModified::run(POINT* origin, POINT* _destination){
    clean(); // will destroy root
    if(destination){
        delete destination;
        destination = 0;
    }
    root = new Node(0, origin);
    setDestination(_destination);
    run();
}

void AStarModified::run(){
    Node*                   current;
    vector<Node*>::iterator iter;

    if(open->size() == 0){
        open->push_back(root);
    }
    while(open->size() > 0){
        iter = open->begin();
        current = (*iter);
        // limit path length
        if(current->getTotalCost() > 25){
            break;
        }
        open->erase(iter);
        if( (current->getPoint()->x == destination->x) &&
            (current->getPoint()->y == destination->y) ){
            pushSolution(current);
            break;
        } else {
            // point up
            TILE*  up = getNeighbor(current, PathAlgorithm::NEIGHBOR::UP);
            addNeighbor(current, up);
            //*/
            // point up right
            TILE*  upRight = getNeighbor(current, PathAlgorithm::NEIGHBOR::UP_RIGHT);
            addNeighbor(current, upRight);
            // point right
            TILE*  right = getNeighbor(current, PathAlgorithm::NEIGHBOR::RIGHT);
            addNeighbor(current, right);
            //*/
            // point down right
            TILE*  downRight = getNeighbor(current, PathAlgorithm::NEIGHBOR::DOWN_RIGHT);
            addNeighbor(current, downRight);
            // point down
            TILE*  down = getNeighbor(current, PathAlgorithm::NEIGHBOR::DOWN);
            addNeighbor(current, down);
            //*/
            // point down left
            TILE*  downLeft = getNeighbor(current, PathAlgorithm::NEIGHBOR::DOWN_LEFT);
            addNeighbor(current, downLeft);
            // point left
            TILE*  left = getNeighbor(current, PathAlgorithm::NEIGHBOR::LEFT);
            addNeighbor(current, left);
            //*/
            // point up left
            TILE*  upLeft = getNeighbor(current, PathAlgorithm::NEIGHBOR::UP_LEFT);
            addNeighbor(current, upLeft);
        }
        closed->insert(closed->begin(), current);
    }
}

void AStarModified::pushSolution(Node* current){
    Node*  path = current;
	POINT* p;

    while(path->getParent() != 0){
		p = new POINT();
		p->x = path->getPoint()->x;
		p->y = path->getPoint()->y;
        solution->insert(solution->begin(), p);
        path = path->getParent();
    }
}

TILE* AStarModified::getNeighbor(Node* node, PathAlgorithm::NEIGHBOR neighbor){
    bool even = false;
    TILE* result = 0;
    int x;
    int y;

    if( node ){
        if(node->getPoint()->y % 2 == 0){
            even = true;
        }
        switch (neighbor){
            case PathAlgorithm::NEIGHBOR::UP :
                x = node->getPoint()->x;
                y = node->getPoint()->y - 2;
                break;
            case PathAlgorithm::NEIGHBOR::UP_RIGHT :
                x = node->getPoint()->x;
                y = node->getPoint()->y - 1;
                if(!even){
                    x++;
                }
                break;
            case PathAlgorithm::NEIGHBOR::RIGHT :
                x = node->getPoint()->x + 1;
                y = node->getPoint()->y;
                break;
            case PathAlgorithm::NEIGHBOR::DOWN_RIGHT :
                x = node->getPoint()->x;
                y = node->getPoint()->y + 1;
                if(!even){
                    x++;
                }
                break;
            case PathAlgorithm::NEIGHBOR::DOWN :
                x = node->getPoint()->x;
                y = node->getPoint()->y + 2;
                break;
            case PathAlgorithm::NEIGHBOR::DOWN_LEFT :
                x = node->getPoint()->x;
                y = node->getPoint()->y + 1;
                if(even){
                    x--;
                }
                break;
            case PathAlgorithm::NEIGHBOR::LEFT :
                x = node->getPoint()->x - 1;
                y = node->getPoint()->y;
                break;
            case PathAlgorithm::NEIGHBOR::UP_LEFT :
                x = node->getPoint()->x;
                y = node->getPoint()->y - 1;
                if(even){
                    x--;
                }
                break;
        }
        if((x >= 0) && (y >= 0)){
            result = new TILE();
            result->x = x;
            result->y = y;
        }
    }
    return result;
}

// get node and remove it from collection
Node* AStarModified::getNode(vector<Node*>* v, POINT* p){
    Node* result = 0;
    vector<Node*>::iterator iter = v->begin();
    bool found = false;

    while(!found && ( iter != v->end() )){
        if( ((*iter)->getPoint()->x == p->x) && 
            ((*iter)->getPoint()->y == p->y) ){
            found = true;
        } else {
            iter++;
        }
    }
    if(iter == v->end()){
        result = 0;
    } else {
        result = (*iter);
        v->erase(iter);
    }
    return result;
}

// if neighbor is good add them to open
void AStarModified::addNeighbor(Node* current, TILE* tile){
    Node*  node = 0;
    bool   fromClosed = true;

    if(tile){
        if( screenManager->getWalkability( screenManager->getTextureId( tile ), 0, false ) ){
            POINT* p = new POINT();
            p->x = tile->x;
            p->y = tile->y;
            node = getNode(getClosed(), p);
            if( !node ){
                fromClosed = false;
                node = getNode(getOpen(), p);
            }
            if(node){ // node already exists and its not root
                // if new node better insert into open list
                if( (current->getGivenCost() + TILE_COST) < node->getGivenCost() ){
                    node->setParent( current );
                    node->setGivenCost( current->getGivenCost() + TILE_COST );
                    insertNode(open, node);
                }
                else {
                    // if it came from closed list put it back
                    if(fromClosed){
                        closed->push_back(node);
                    }
                    // if it came from open list put it back
                    else{
                        insertNode(open, node);
                    }
                }
                if(p){
                    delete p;
                    p = 0;
                }
            } else { // doesnt already exist
                node = new Node(current, p);
                node->setGivenCost( current->getGivenCost() + TILE_COST );
                node->setHeuristicCost(destination);
                insertNode(open, node);
            }
        } else {
            if(tile){
                delete tile;
                tile = 0;
            }
        }
    }
}

ScreenManager*  AStarModified::getScreenManager(){
    return screenManager;
}
void AStarModified::setScreenManager(ScreenManager* _screenManager){
    screenManager = _screenManager;
}

void AStarModified::insertNode(vector<Node*>* v, Node* n){
    vector<Node*>::iterator iter = v->begin();
    bool found = false;

    while(!found && (iter != v->end())){
        if(n->getTotalCost() > (*iter)->getTotalCost()){
            iter++;
        }
        else{
            found = true;
        }
    }
    if(iter == v->end()){
        v->push_back(n);
    }
    else {
        v->insert(iter, n);
    }
}