 int FindPath(waypoint *start, waypoint *destination)
 {
 
-  long ClosedCost[MAP_POINTS];
+  long ClosedCost[map_points];
 //  int OpenIndex = 0;
 //  int ClosedIndex = 1;
   int  i, j, k;

Input from C5: Speed and position of obstacles.
Input from C6: Position, orientation, velocity and acceleration.


Files:
RNDF (Route Network Definition File). Format is defined on 
http://archive.darpa.mil/grandchallenge/docs/RNDF_MDF_Formats_031407.pdf. 
This is a digital map of all roads in the area where the vehicle will be operating. 
The camera will locate road edges or lane markers and the vehicle will follow them. 
Thus location can be determined primarily from odometry.

MDF (Mission Definition File). These are latitudes and longitudes that the vehicle 
is required to visit.

Initial position. Specifies the starting location and orientation. Velocity is zero. 
If this is a file, it is read by C4 (Path Planner) and passed to C6 (Navigator). 

    Route is a finer scale list of waypoints from present or recent position.
    Exit is the route from Path[last]-> location to Destination.
*/
int map_points = 16;
// struct curve Links[20];
struct junction Nodes[MAX_WAYPOINTS];
struct AStar
{
    int ParentID;
    long CostFromStart;
    long CostToGoal;
    long TotalCost;
} Open[MAX_WAYPOINTS];

class waypoint Origin, Start;
waypoint Path[MAX_WAYPOINTS];  // course route to goal
