#include "Aria.h"

int matriz[1001][1001];

class ActionGo : public ArAction {
  public:
    ActionGo (ArActionGoto go_to);
    virtual ~ActionGo(void) {};
    virtual ArActionDesired *fire(ArActionDesired currentDesired);
    virtual void setRobot(ArRobot *robot);

  protected:
  ArRangeDevice *mySonar;
  ArActionDesired myDesired;
  ArActionGoto myGoto;

};

ActionGo::ActionGo(ArActionGoto go_to) :
  ArAction("Go") {
  mySonar = NULL;
  myGoto = go_to;
}

void ActionGo::setRobot(ArRobot *robot)
{
  ArAction::setRobot(robot);
  mySonar = robot->findRangeDevice("sonar");
  if (robot == NULL)
    {
      ArLog::log(ArLog::Terse, "actionExample: ActionGo: Warning: I found no sonar, deactivating.");
      deactivate();
    }
}

ArActionDesired *ActionGo::fire(ArActionDesired currentDesired)
{
  double range;
  double speed;

  myDesired.reset();

  if (mySonar == NULL) {
    deactivate();
    return NULL;
  }

  return &myDesired;
}

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

    matriz[500][500] = -1;
    //-1 escolhido para representar a posição do robo
    //tamanho do robo é 500mm, ent de cada posicao da matriz pra outra
    //é 510mm
    //fazer modulo (%510)

  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArRobot robot;
  ArSonarDevice sonar;

  // Connect to the robot, get some initial data from it such as type and name,
  // and then load parameter files for this robot.
  ArRobotConnector robotConnector(&parser, &robot);
  if(!robotConnector.connectRobot()) {
    ArLog::log(ArLog::Terse, "actionExample: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed()) {
        // -help not given
        Aria::logOptions();
        Aria::exit(1);
    }
  }
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) {
    Aria::logOptions();
    Aria::exit(1);
  }
  ArLog::log(ArLog::Normal, "actionExample: Connected to robot.");

  //adicionando sonar
  robot.addRangeDevice(&sonar);
  robot.runAsync(true);

  ArActionStallRecover recover;
  ArActionAvoidFront AvoidFront("AvoidFront");
  ArActionGoto gotoPoseAction("goto");
  //ActionGo go(gotoPoseAction);

  //adicionando acoes para o robo
   ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
  ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
  ArActionLimiterTableSensor tableLimiterAction;
  robot.addAction(&recover, 500);
  robot.addAction(&AvoidFront, 400);
  robot.addAction(&gotoPoseAction, 350);

  /*robot.lock();
  gotoPoseAction.setGoal(ArPose(10000, 10000));
  robot.unlock();
  ArUtil::sleep(4000);*/

  robot.enableMotors();

  Aria::exit(0);

  return 0;
}