/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <iostream>
#include <fstream>
#include <string>
#include <vector>

using namespace std;

#include <unistd.h>
#include "agent/WorldModel/BallTracker.h"
#include "agent/headers/Utility.h"

struct obs {
  double x;
  double y;
  unsigned long time;
};


void usage(const string & s) {
  cerr << "Usage: " << s << " <args>" << endl << endl;
  cerr << "args:" << endl;
  cerr << "     -f <input filename>" << endl;
}

int main(int argc, char * argv[]) {

  string pname=string(argv[0]);
  string filename;

  BallTracker ball_tracker(BallTracker::tMULTIPOS);

  // Read raw vision data from a log file and feed that into the tracker
  int c;
  while ((c= getopt(argc,argv,"f:")) >=0) {
    switch(c) {
    case 'f':
      filename = string(optarg);
      break;
    default:
      usage(pname);
      exit(1);
    }
  }

  if (filename.empty()) {
    cerr << "No input filename specified!" << endl;
    usage(pname);
    exit(1);
  }

  ifstream infile(filename.c_str());
  if (!infile) {
    cerr << "Could not open " << filename << " for reading" << endl;
    exit(1);
  }
  // Parse the data file from raw vision readings 
  // returned in odometric coordinates
  // 
  // Data file format: [x y timestamp]

  vector<obs> obs_v;

  string newline;
  while(getline(infile,newline)) {
    obs o;
    if (3!=sscanf(newline.c_str(),"%lf %lf %lu",&o.x,&o.y,&o.time)) {
      cerr << "Error, could not parse line: " << newline << endl;
      exit(1);
    }
    obs_v.push_back(o);
  }
  if (obs_v.empty()) { 
    cerr << "Error: no observations" << endl;
    exit(1); 
  }

  // Now, run it through the tracker and output the results

  Gaussian2 pos;
  ulong time = obs_v[0].time;

  for (uint i=1;i<obs_v.size();i++) {

    while (SecToTime(1.0/30.0) < (obs_v[i].time - time)) {
      cerr << "Propagate at time " << time << endl;
      ball_tracker.propagate(time,BallTracker::sUNOBS);
      cerr << "Propagate done!" << endl;
      time += SecToTime(1.0/30.0);
      if (!ball_tracker.getPosition(pos)) {
	cerr << "Couldn't get observation for time " << time << endl;
	exit(1);
      }
      cout << pos.mean.x << " " << pos.mean.y << " " << pos.time << endl;
    }

    cerr << "Observe at time " << time << endl;
    ball_tracker.observe(obs_v[i].time,
			 vector2d(obs_v[i].x,obs_v[i].y),
			 1.0,
			 500.0,
			 true);

    cerr << "Observe done!" << endl;

    cerr << "Getting position" << endl;
    if (!ball_tracker.getPosition(pos)) {
      cerr << "Couldn't get observation for time " << obs_v[i].time << endl;
      exit(1);
    }
    cerr << "Getting position done!" << endl;

    cout << "Position: " << pos.mean.x << " " << pos.mean.y << " " << pos.time << endl;
  }

  return 0;
}