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

/*

Checking for valid behavior attributes requires that the behavior
object is already created... this is a real drawback, as some behavior
construction may require presence of servers such as Navigation!

Each Behavior object contains an Attributes object, which is filled
in at Behavior construction time. Attributes::parse() is called to 
validate a user-input attribute=value pair. 

*/

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");

    printf("Behavior stack size is %d\n", behaviors.size());

    char response[5];
    char quit = 0;
    char Go = 0;
    for (int i = behaviors.size() - 1; i>=0; i--) {

      Behavior *b;
      behaviors.get(i, &b);

      char go = 0, skip = 0;

      if (!Go)
      {
        printf("***\n*** Running behavior %d: %s\n***\n", i, b->name()); 
        printf("Press any key to step through behavior except...\n");
        printf("'g' to run through this behavior...\n");
        printf("'s' to skip this behavior and go to the next...\n");
        printf("'G' to run through the whole mission plan...\n");
        printf("'q' to quit.\n"); 
      }

      while (!quit && (Go || go || strncmp(gets(response), "s", 1)) )
      {
        if (!strncmp("q", response, 1))
        {
          quit = 1;
          break;
        }

        b->execute();
        System::milliSleep(200);
        if (!strncmp("g", response, 1)) go = 1;
        if (!strncmp("G", response, 1)) Go = 1;
        if (b->state() == Behavior::Finished) break;
      }
		if (i > 0) sleep(2);  // Pause between the behaviors
    }
  }

  delete plan;
  return result;
}