#include <cmath>
#include <stack>
#include "AStar.hpp"

using namespace std;

/*
Author : WB
*/

//初始化地圖
AStar::AStar(char *m, const int &rows, const int &cols)
    : map_(m),rows_(rows), cols_(cols){
        init();
}

//釋放所有分配的資源
AStar::~AStar(){
	for(ClosedTable::iterator itr=ct_.begin(); itr!=ct_.end(); ++itr){
		delete (*itr);
	}
	while(!ot_.empty()){
		delete ot_.top();
		ot_.pop();
	}
}

//一個(gè)簡(jiǎn)易的ID生成算法
int AStar::generateId(const int &x, const int &y) const{
    return x * 18 + y;
}

//確定地圖中的初始點(diǎn)和終點(diǎn)坐標(biāo)
void AStar::findSourceAndDestination(){
    for(int i=0; i<rows_; ++i){
        for(int j=0; j<cols_; ++j){
            if(map_[i * cols_ + j] == 's'){
                source_.x = i;
                source_.y = j;
            }
            if(map_[i * cols_ + j] == 'd'){
                destination_.x = i;
                destination_.y = j;
            }
        }
    }
}

//判斷地圖上某點(diǎn)是否可以到達(dá)
bool AStar::canAccess(const int &x, const int &y) const{
    return map_[x * cols_ + y] != 'o';
}

void AStar::initStartPoint(){
    PNode start = new ANode;
    start->x = source_.x;
    start->y = source_.y;
    start->childNum = 0;
    start->id = generateId(start->x, start->y);
    start->parent = 0;
    start->g = 0;
    start->h = abs(start->x - destination_.x) + abs(start->y - destination_.y);
    start->f = start->g + start->h;
    ot_.push(start);
}

void AStar::init(){
    findSourceAndDestination();
    initStartPoint();
}

//判斷某個(gè)Node是否在ClosedTable中
AStar::PNode AStar::isInClosedTable(const int &id){
    for(ClosedTable::iterator itr=ct_.begin(); itr!= ct_.end(); ++itr){
        if((*itr)->id == id)
            return *itr;
    }
    return 0;
}

//判斷某個(gè)Node是否在OpenTable中
AStar::PNode AStar::isInOpenTable(const int &id){
    return ot_.contain(id);
}

//采用類似深度優(yōu)先搜索的算法更新Node的所有子節(jié)點(diǎn)及其后繼節(jié)點(diǎn)
void AStar::updateNodes(PNode p){
    stack<PNode> pStack;
    PNode temp = 0;
    for(int i=0; i<p->childNum; ++i){
        temp = p->children[i];
        if((p->g + 1) < temp->g){
			reCombine(temp, p);
            pStack.push(temp);
        }
    }
    while(!pStack.empty()){
        temp = pStack.top();
        pStack.pop();
        PNode tempChild = 0;
        for(int i=0; i<temp->childNum; ++i){
            tempChild = temp->children[i];
            if((temp->g + 1) < tempChild->g){
				reCombine(tempChild, temp);
                pStack.push(tempChild);
            }
        }
    }
}

//重新關(guān)聯(lián)父子Node的關(guān)系
void AStar::reCombine(PNode child, PNode parent){
	child->g = parent->g + 1;
	child->f =child->g + child->h;
	child->parent = parent;
}

//算法的核心，尋找最短路徑
//如果找到最短路徑，就將路徑存儲(chǔ)在path中，并且返回true
//否則返回false表示沒有路徑可達(dá)
bool AStar::findPath(vector<Point> &path){
    PNode node = ot_.top();
	//A*搜索算法
    while(!ot_.empty() && !(node->x == destination_.x && node->y == destination_.y)){
		ot_.pop();
		//向8個(gè)方向搜索節(jié)點(diǎn)
        for(int m=-1; m<2; ++m)
            for(int n=-1; n<2; ++n){
                if(m|n){
                    int tempx = node->x + m;
                    int tempy = node->y + n;
                    int tempid = generateId(tempx, tempy);
                    if(canAccess(tempx, tempy)){
                        PNode temp;
                        if((temp = isInOpenTable(tempid)) != 0){
                            node->children[node->childNum++] = temp;
                            if((node->g + 1) < temp->g){
								reCombine(temp, node);
                            }
                        }else if((temp = isInClosedTable(tempid)) != 0){
                            node->children[node->childNum++] = temp;
                            if((node->g + 1) < temp->g){
								reCombine(temp, node);
                                updateNodes(temp);
                            }
                        }else{
                            temp = new ANode;
                            temp->x = tempx;
                            temp->y = tempy;
                            temp->childNum = 0;
                            temp->id = tempid;
                            temp->parent = node;
                            temp->g = node->g + 1;
                            temp->h = abs(temp->x - destination_.x) + abs(temp->y - destination_.y);
                            temp->f = temp->g + temp->h;
							node->children[node->childNum++] = temp;
                            ot_.push(temp);
                        }
                    }
                }
            }
//        ot_.pop();
        ct_.push_back(node);
		node = ot_.top();
    }
    if(ot_.empty())
        return false;
	//將找到的路徑存儲(chǔ)在path中
    PNode bestNode = node->parent;
    while(bestNode->parent != 0){
        Point t;
        t.x = bestNode->x;
        t.y = bestNode->y;
        path.insert(path.begin(),t);
        bestNode = bestNode->parent;
    }
    return true;
}