//* Drive towards with advanced use of sensors *
#include "../00_nav_library/nav_aria_iface.h"
#include "../00_nav_library/nav_geometry.h"
#include "../00_nav_library/nav_robot2lines.h"
#include <iostream>
#include <iomanip>
#include <conio.h>
using namespace std;



/* 
 * Action that drives the robot forward, but goes towards a detected obstacle 
 * and stops in front of it. Uses sonar for obstacle detection
 */
class ActionToWallAdvanced : public ArAction
{
public:
//* May be customized, and more methods added *

    // constructor, sets myXXXXParameters
    ActionToWallAdvanced(double maxSpeed=1000, double stopDistance=600, double stopAccuracyDistance=25, double stopAccuracyAngle=10);

    // altering and accessing certain aspects of the action internal state
    bool haveAchievedGoal() const { return(myHaveAchievedGoal); }

//* Following need to be always implemented *
    // destructor. does not need to do anything
    virtual ~ActionToWallAdvanced(void) {};
    // called by the action resolver to obtain this action's requested behavior
    virtual ArActionDesired *fire(ArActionDesired currentDesired);
    // store the robot pointer, and it's ArSonarDevice object, or deactivate this action if there is no sonar.
    virtual void setRobot(ArRobot *robot);
protected:
    // the sonar device object obtained from the robot by setRobot()
    ArRangeDevice *mySonar;

  /* Our current desired action: fire() modifies this object and returns
      to the action resolver a pointer to this object.
      This object is kept as a class member so that it persists after fire()
      returns (otherwise fire() would have to create a new object each invocation,
      but would never be able to delete that object).
  */
    ArActionDesired myDesired;

    
//* Following are your parameters initialized in constructors and used among your functions *
    double myMaxSpeed;
    double myStopDistance;
    double myStopAccuracyDistance;
    double myStopAccuracyAngle;
    bool   myHaveAchievedGoal;
};


/*
  Note the use of constructor chaining with ArAction(actionName). 
  Also note how it uses setNextArgument, which makes it so that 
  other parts of the program could find out what parameters this action has, and possibly modify them.
*/
ActionToWallAdvanced::ActionToWallAdvanced(double maxSpeed, double stopDistance, double stopAccuracyDistance, double stopAccuracyAngle) :
ArAction("ToWall")
{
    mySonar = NULL;
    myMaxSpeed = maxSpeed;
    myStopDistance = stopDistance;
    myStopAccuracyDistance = stopAccuracyDistance;
    myStopAccuracyAngle = stopAccuracyAngle;
    myHaveAchievedGoal = false;
    setNextArgument(ArArg("maximum speed",          &myMaxSpeed,                "Maximum linear speed to go."));
    setNextArgument(ArArg("stop distance",          &myStopDistance,            "Distance to wall at which to stop."));
    setNextArgument(ArArg("stop accuracy distance", &myStopAccuracyDistance,    "Distance accuracy."));
    setNextArgument(ArArg("stop accuracy angle",    &myStopAccuracyAngle,       "Angle accuracy."));
}

/*
  Override ArAction::setRobot() to get the sonar device from the robot, or deactivate this action if it is missing.
  You must also call ArAction::setRobot() to properly store
  the ArRobot pointer in the ArAction base class.
*/
void ActionToWallAdvanced::setRobot(ArRobot *robot)
{
    ArAction::setRobot(robot);
    mySonar = robot->findRangeDevice("sonar");
    if (robot == NULL)
    {
        ArLog::log(ArLog::Terse, "actionExample: ActionToWallAdvanced: Warning: I found no sonar, deactivating.");
        deactivate();
    }
}

/*
  This fire is the whole point of the action.
  currentDesired is the combined desired action from other actions
  previously processed by the action resolver.  In this case, we're
  not interested in that, we will set our desired 
  forward velocity in the myDesired member, and return it.

  Note that myDesired must be a class member, since this method
  will return a pointer to myDesired to the caller. If we had
  declared the desired action as a local variable in this method,
  the pointer we returned would be invalid after this method
  returned.
*/
ArActionDesired *ActionToWallAdvanced::fire(ArActionDesired currentDesired)
{
    // reset the actionDesired (must be done), to clear its previous values.
    myDesired.reset();

    // if the sonar is null we can't do anything, so deactivate
    if (mySonar == NULL)
    {
        deactivate();
        return NULL;
    }

    /*
        Proportional Controller:

        [v(t), w(t)]T = K * [error]T = [Kro * errorDistance, Ka * errorHeading  + Kb * errorFinalHeading]T

        where:

            errorDistance       = sqrt(errorX*errorX + errorY*errorY)
            errorHeading        = -R.Theta() + atan2(errorY, errorX)
            errorFinalHeading   = -R.Theta() - errorHeading
            Strong stability: Kro>0, Kb<0, Ka-Kb>0, Ka+1.5*Kb-2*Kro/Pi>0
    */
    static const double Kro =  .25;
    static const double Ka  = 1.00;
    static const double Kb  = -.375;

    myHaveAchievedGoal = false;
    double v = myMaxSpeed;              // if no wall then proceed forward at maximum speed
    double w = 0;



    NAV_Line line_ahead;
//  NAV_FindCurrentLines(myRobot->getPose(), *mySonar, NULL, NULL, &line_ahead /*, NULL, NULL, NULL */); // -90 to 90 degree ahead
    NAV_FindCurrentLine(myRobot->getPose(), *mySonar, -60, 60, line_ahead); // -45 to +45 degree limited ahead

    if ( line_ahead.isValid() )
    {
        double dist_to_wall = line_ahead.distance(myRobot->getPose());
        double angle_to_wall= ArMath::atan2(line_ahead.getB(), line_ahead.getA());
        double robot_angle  = myRobot->getTh();

        double ro = dist_to_wall - myStopDistance;     // distance to travel
        double a = angle_to_wall - robot_angle;
        // the "directins" of line perpendicular to wall and robot axes may be reversed, fix that by rotating 180 degrees!
        if (a<-90) a+=180; else if (a>90) a-=180;

        double b = 0;                       // assuem that we arrive at 90 degree to the wall, need more pts to compute

        // the control law equations
        v  = Kro*ro;
        w  = Ka*a + Kb*b;
        if (v>myMaxSpeed) {
            // cannot drive too fast, limit speed
            v=myMaxSpeed;
        } else if (v<-myMaxSpeed) {
            v=-myMaxSpeed;
        }

#ifdef _DEBUG
        cerr << " ro="<<setw(8)<<ro <<" a="<<setw(8)<<a <<" b="<<setw(8)<<b <<" v="<<setw(8)<<v <<" w="<<setw(8)<<w <<endl;
#endif
        // if reached within desired accuracy?
        if (fabs(ro)<=myStopAccuracyDistance && fabs(a)<=myStopAccuracyAngle && fabs(b)<=myStopAccuracyAngle) {
            // goal acheived within accuracy
            myHaveAchievedGoal = true;
        }

    }


    if (!myHaveAchievedGoal)
    {
        // the control laws translated into the robot control input
        myDesired.setVel(v);
        myDesired.setRotVel(w);
    }

    // return a pointer to the actionDesired to the resolver to make our request
    return &myDesired;
}



int main(){
//  NAV_Robot R("gdansk.bradley.edu", 8101);
    NAV_Robot R("localhost", 8101);
//  NAV_Robot R("com1");
//  NAV_Robot R("com4");

    if (!R.is_connected()) {
        cout << "Cannot connect to the robot or robot simulator, exiting" << endl;
        return(0);
    }


    // Create instances of the actions defined above, plus ArActionStallRecover, 
    // a predefined action from Aria.
    ArActionStallRecover    recover;
    ArActionBumpers         bumpers;
    ArActionAvoidSide       avoidside("Avoid Side", 100);
    ArActionStop            stop;       // stop if nothing else to do

    ActionToWallAdvanced go(2200, 550);

    int state = 0; // 0 - approach, 1 - find new goal

    R.lock();
    R.addAction(&recover,  100);
    R.addAction(&bumpers,   95);
    R.addAction(&avoidside, 80);
    R.addAction(&go,        50);
    R.addAction(&stop,      10); // stop if nothing else to do
    R.unlock();



#ifdef _DEBUG
    cerr << showpoint << fixed << setprecision(2);      // for inspection purpose only
#endif

    while( R.is_connected()) {

        // keyboard exit
        if (_kbhit()) /* <conio.h> */ {
            char c=tolower(_getch());
            if (c=='q') break;
        }

        // state machine goes here
        R.lock();
        if (state==0) {
            if (go.haveAchievedGoal()) {
                state=1;
                // remove all actions that might be triggered and interfere before
                // taking over with driect robot control
                // R.remAction(..);
                R.remAction(&go);
                if (R.getDistanceL10()<R.getDistanceR10())  R.doRotateAngle(-170);  // direct motion overrides all actions
                else                                        R.doRotateAngle(170);   // direct motion overrides all actions
                // make sure that the robot finished the driect control before resuming actions
#ifdef _DEBUG
                cerr << "Wall reached! Turning" << endl;
#endif
            } else {
                // continue approaching the wall
            }
        } else if (state==1) {
            if (R.isCommandDone()) {
                R.clearDirectMotion(); // enables actions again as they were disabled by the issed direct motion command
                state=0;
                // R.remAction(..);
                // R.remAction(..);
                R.addAction(&go, 50);
#ifdef _DEBUG
                cerr << "Turning done! Searching for a new wall" << endl;
#endif
            } else {
                // let it finish the turn
            }
        } else {
            cerr << "Error: Invalid state!" << endl;
            // invalid state
        }


#ifdef false
        _DEBUG
        cerr <<" X="<<setw(6)<< R.getX() <<" Y="<<setw(6)<< R.getY() <<" T="<<setw(8)<< R.getTh()
             <<" D10L="<<setw(8)<< R.getDistanceL10() 
             <<" D10R="<<setw(8)<< R.getDistanceR10()
             << endl;
#endif
        R.unlock();

        R.Sleep(100);
    }

    if (R.is_connected()) R.disconnect(); 
    else cerr << "Lost connection, aborting!" << endl;

    return(0);
}