/* the data structure to hold landmarks and routines to proces them*/


#define MAXLANDMARKS 100

struct LandmarkEntry {
  ArPose vp;
  float x, y, z; // odometry/terrain 
  float r, p, c; // roll pitch comp[ass */
  float n,w; // gps
  float rad; // terrain extent
};

class LandmarkList {

 public:
  LandmarkList(char *logname) {
    landmarkIndex=0;
    log = fopen(logname,"w"); 
    if (log==0) printf("Landmark list unable to open log file called: %s\n",logname);
  }
  ~LandmarkList() {
    fclose(log);
  }

  int getIndex() { return landmarkIndex; }
 
void  addToList(float xi, float yi, float zi, float ri, float pi, float ci, float ni, float wi, float radi){
    struct LandmarkEntry *le=&list[landmarkIndex];
    le->x=xi; le->y = yi; le->z=zi; le->r=ri; le->p=pi; le->c=ci; le->n=ni; le->w=wi; le->rad=radi;
    if (++landmarkIndex>=MAXLANDMARKS) printf("Landmark list now full!\n");
  }

void   addViewingLoc(ArPose vpi) {
    struct LandmarkEntry *le=&list[landmarkIndex];
    le->vp = vpi;
  }

void  logLandmark(char *ifilename, char *sdatafilename) {
    struct LandmarkEntry *le=&list[landmarkIndex-1];
    fprintf(log,"%d, %s, %s, %5.1f,%5.1f,%5.1f,   %5.1f,%5.1f,%5.1f,   %5.1f,%5.1f,  %5.1f,  %5.1f,%5.1f,%5.1f\n",
	   landmarkIndex-1, ifilename, sdatafilename, 
	   le->x, le->y, le->z,   le->r, le->p, le->c,  le->n, le->w, le->rad, 
	   le->vp.getX(), le->vp.getY(), le->vp.getTh());
  }

 private:
  int landmarkIndex;
  LandmarkEntry list[MAXLANDMARKS];
  FILE *log;
};