/* Our implementation of the A* search algorithm */
  

  CCArray*  AStarPathNode::findPathFromTo(AStarNode *fromNode, AStarNode *toNode) {
  

  CCArray *foundPath = new CCArray(3);
  

  foundPath->init();
  

  

  if(fromNode->position.x == toNode->position.x && fromNode->position.y == toNode->position.y){
  

  return NULL;
  

  }
  

  

  CCArray *openList = CCArray::create();
  

  CCArray *closedList = CCArray::create();
  

  

  

  AStarPathNode *currentNode = NULL;
  

  AStarPathNode *aNode = NULL;
  

  

  AStarPathNode *startNode = AStarPathNode::createWithAStarNode(fromNode);
  

  AStarPathNode *endNode = AStarPathNode::createWithAStarNode(toNode);
  

  openList->addObject(startNode);
  

  while(openList->count() > 0){
  

  currentNode = AStarPathNode::lowestCostNodeInArray(openList);
  

  

  if( currentNode->node->position.x == endNode->node->position.x &&
  

  currentNode->node->position.y == endNode->node->position.y){
  

  

  //Path Found!
  

  aNode = currentNode;
  

  

  while(aNode!=NULL&& NULL != aNode->previous){
  

  //Mark path
  

  foundPath->addObject(CCValue::valueWithCCPoint(ccp(aNode->node->position.x, aNode->node->position.y)));
  

  aNode = aNode->previous;
  

  }
  

  

  foundPath->addObject(CCValue::valueWithCCPoint(ccp(aNode->node->position.x, aNode->node->position.y)));
  

  openList->removeAllObjects();
  

  closedList->removeAllObjects();
  

  return foundPath;
  

  }else{
  

  //Still searching
  

  CCLog(" add before closedList->count() = %d", closedList->count());
  

  closedList->addObject(currentNode);
  

  CCLog(" add after closedList->count() = %d", closedList->count());
  

  CCLog(" remove before openList->count() = %d", openList->count());
  

  openList->removeObject(currentNode);
  

  CCLog(" remove after openList->count() = %d", openList->count());
  

  

  for(int i=0; i<currentNode->node->neighbors->count(); i++){
  

  AStarPathNode *aNode = AStarPathNode::createWithAStarNode((AStarNode*)currentNode->node->neighbors->objectAtIndex(i));
  

  aNode->cost = currentNode->cost+currentNode->node->costToNode(aNode->node)+aNode->node->costToNode(endNode->node);
  

  aNode->previous = currentNode;
  

  if(aNode->node->active && ! AStarPathNode::isPathNodeinList(aNode, openList) && !AStarPathNode::isPathNodeinList(aNode, closedList)){
  

  openList->addObject(aNode);
  

  }
  

  }
  

  

  

  }
  

  }
  

  openList->removeAllObjects();
  

  closedList->removeAllObjects();
  

  //No Path Found
  

  return NULL;
  

  }