Add a flag for that, it will make your life easier.
That seems to be what is wrong in your code, you are mixing the g cost (the ACTUAL cost to get to a node from the start) with the f cost (the estimated cost to get to the end).
Edit:
I got to go for today, here is my code that evaluates the neighbors:
static int processCell(pathFindingStruct* path, int xAdjust, int yAdjust, edgeRef* edge, edgeRef* end, const unsigned extraCost){
if(path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].inClosedList == 1){
return 1;
}
unsigned distance = path->mainGrid[edge->i][edge->j].costSoFar + extraCost;
if (path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].inOpenList == 0 || distance < path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].costSoFar){
path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].parent.i = edge->i;
path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].parent.j = edge->j;
path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].costSoFar = distance;
path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].totalCost = path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].costSoFar + path->heuristicDistance(edge->i + yAdjust,edge->j + xAdjust, end->i, end->j);
if (path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].inOpenList == 0){
edgeRef* newNode = getEdgeRef(path, edge->i + yAdjust, edge->j + xAdjust, &path->mainGrid[edge->i + yAdjust][edge->j + xAdjust]);
if (newNode == NULL || (modifiedHeapInsert(path->openList, newNode) == 0)){
free(newNode);
return 0;
}
path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].inOpenList = 1;
path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].myRef = newNode;
}
else{
modifiedHeapUpdatedValue(path->openList, path->mainGrid[edge->i + yAdjust][edge->j + xAdjust].myRef);
}
}
return 1;
}
It is in C, using pointers to avoid copies. Important notes:
Edge is the current node.
xAdjust and yAdjust is the move, for instance, if their values are (1, 1) I am moving right and down.
The grid is [y][x] aligned.
The return value is just to check memory errors.
"costSoFar" is g and "totalCost" is f.
extraCost is used because diagonal movements are more expensive.