#include <stdio.h>
#include "MissionPlan.h"
#include "Behavior.h"
#include "System.h"

/*

*/

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

  if (argc != 2) {
    fprintf(stderr, "usage: %s planfile\n", argv[0]);
    return -1;
  }

  char *planFile = argv[1];
  BehaviorStack behaviors;

  MissionPlan *plan = new MissionPlan();
  int result = plan->load(planFile, &behaviors);

  if (result == -1) {
    fprintf(stderr, "Error in mission plan file\n");
  }
  else {
    fprintf(stderr, "Mission plan file is okay\n");
  }


  Behavior *b;

  for (int i = 0; i < behaviors.size(); i++) {
    
    behaviors.get(i, &b);

    for (int j = 0; j < 30; j++)
    {
      if (b->shouldBehaviorStart()) break;
      sleep(1);
    }

    for (j = 0; j < 50; j++)
    {
      b->execute();
      if (b->state() == Behavior::Finished) break;
      System::milliSleep(200);
    }

  }


  delete plan;
  return result;
}