#include "act_library.h"
#include "mag_modectrl.h"
#include "../../00_nav_library/nav_random.h"
#include "../../00_nav_library/nav_field_potential.h"
#include "../../00_nav_library/nav_path_flood.h"

#include <iostream>
#include <iomanip>
#include <conio.h>


// STOP can be used as a template for varius operations 
void actStopAndManual(NAV_Robot& R, NAV_Map & ML, NAV_Map & MG) {
    MagMode initial_mode = robotMode.getValue();

    NAV_FIELD_Potential OBST(&ML); 
    OBST.register_map(&MG);
    OBST.setUseGoal(false);

    // do actual STOP as requested 
    R.lock();
    R.Stop();
    R.unlock();
    //and stay in the state until mode changes

    R.lock();
    while ( needtorestart.getValue()==0 && robotMode.getValue()==initial_mode ) {
        if ( !R.is_connected() ) {
            needtorestart.setValue(1);
            break;
        }
        if ( R.isStalled() ) {
            needtorestart.setValue(1);
            break;
        }

        // insert robot control instructions here for other operations
        // remember to unlock for the duration of wait or lengthy computations

        R.unlock();
        R.Sleep(100);
        R.lock();
#ifdef _DEBUG
        std::cerr <<"STPM "<<" X="<<std::setw(6)<< R.getX()
                           <<" Y="<<std::setw(6)<< R.getY() 
                           <<" T="<<std::setw(6)<< R.getTh() << std::endl;
#endif
    }
    R.unlock();
}


void actErratic_Direct(NAV_Robot& R, NAV_Map & ML, NAV_Map & MG) {
    const long int drive_timeStep        =  50;     // Minimum time step used for driving computations
    const int      drive_driveSpeed      = 200;     // linear speed when mapping
    const int      drive_rotateSpeed     =  20;     // rotational speed when turning

    MagMode initial_mode = robotMode.getValue();

    R.lock();
    while ( needtorestart.getValue()==0 && robotMode.getValue()==initial_mode ) {
        if ( !R.is_connected() ) {
            needtorestart.setValue(1);
            break;
        }
        if ( R.isStalled() ) {
            needtorestart.setValue(1);
            break;
        }

        if ( R.getDistanceAhead() < R.getRobotLengthFront()+300. ||
             R.getSonarDistance(-100, 100) < R.getRobotRadius()+200. ) {

            R.doDriveSpeed(0);
            if ( R.getSonarDistance(-100, -10) < R.getSonarDistance(10, 100) )
                R.doRotateSpeed( getRandomInt(drive_rotateSpeed/2, drive_rotateSpeed) );
            else
                R.doRotateSpeed( getRandomInt( -drive_rotateSpeed, -drive_rotateSpeed/2) );

            do {
                R.unlock();
                R.Sleep(drive_timeStep);
                R.lock();
            } while ( R.getDistanceAhead() < R.getRobotLengthFront()+300. ||
                      R.getSonarDistance(-100, 100) < R.getRobotRadius()+200. );
        }
        R.doRotateSpeed(0) ;

        R.doDriveSpeed(drive_driveSpeed);

        R.unlock();
        R.Sleep(drive_timeStep);
        R.lock();

#ifdef _DEBUG
        std::cerr <<"RNDD "<<" X="<<std::setw(6)<< R.getX()
                           <<" Y="<<std::setw(6)<< R.getY() 
                           <<" T="<<std::setw(6)<< R.getTh() << std::endl;
#endif

    }
    R.unlock();
}


void actErratic_Action(NAV_Robot& R, NAV_Map & ML, NAV_Map & MG) {
    MagMode initial_mode = robotMode.getValue();

    // add a set of actions that combine together to effect the wander behavior
    ArActionStallRecover recover;
    ArActionBumpers bumpers;
    ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
    ArActionAvoidFront avoidFrontFar("Avoid Front Far");
    ArActionConstantVelocity constantVelocity("Constant Velocity", 400);

    R.lock();
    R.clearDirectMotion(); // cancel drive and/or rotation speed and stop directives if such issued
    R.addAction(&recover, 100);
    R.addAction(&bumpers, 75);
    R.addAction(&avoidFrontNear, 50);
    R.addAction(&avoidFrontFar, 49);
    R.addAction(&constantVelocity, 25);
    R.unlock();

    R.lock();
    while ( needtorestart.getValue()==0 && robotMode.getValue()==initial_mode  ) {
        if ( !R.is_connected() ) {
            needtorestart.setValue(1);
            break;
        }
        if ( R.isStalled() ) {
            needtorestart.setValue(1);
            break;
        }

        R.unlock();
        R.Sleep(100);
        R.lock();

#ifdef _DEBUG
        std::cerr <<"RNDA "<<" X="<<std::setw(6)<< R.getX()
                           <<" Y="<<std::setw(6)<< R.getY() 
                           <<" T="<<std::setw(6)<< R.getTh() << std::endl;
#endif
    }
    R.unlock();


    R.lock();
    R.Stop();
    R.remAction(&recover);
    R.remAction(&bumpers);
    R.remAction(&avoidFrontNear);
    R.remAction(&avoidFrontFar);
    R.remAction(&constantVelocity);
    R.unlock();
}


void actPotField_Direct(NAV_Robot& R, NAV_Map & ML, NAV_Map & MG, AutoSynchronized<ArPose> & G) {
    MagMode initial_mode = robotMode.getValue();

    NAV_FIELD_Potential POT(&ML);
    POT.register_map(&MG);

    NAV_FIELD_Potential OBST(&ML);
    OBST.register_map(&MG);
    OBST.setUseGoal(false);

    ArPose GOAL = G.getValue();

    // This will set the goal only once, if desired to update then you need to do it every now and then
    POT.setGoal(GOAL);      // POT update has little numerical complexity

    R.lock();
    while ( needtorestart.getValue()==0 && robotMode.getValue()==initial_mode ) {
        if ( !R.is_connected() ) {
            needtorestart.setValue(1);
            break;
        }
        if ( R.isStalled() ) {
            needtorestart.setValue(1);
            break;
        }

        // recommendation from POT (uses MAP updates on the fly but you can get stack in a local minimum)
        double Pvelocity = 0;
        double Pheading  = 0;
        // this is what we will do
        double velocity = 0;
        double heading  = 0;

        ArPose position = R.getPose();

        if (POT.is_goal(position)) {
            break;
        }

        if ( GOAL!=G.getValue() ) {
            POT.setGoal(GOAL);      // POT update has little numerical complexity
        }

        Pvelocity = POT.getVelCoef(position);
        Pheading  = POT.getHeading(position);
        if (Pvelocity<0.05) {
            // local minimum
            break;
        }

        velocity = 200. * Pvelocity;
        heading = Pheading;
        R.doDriveSpeed(velocity);
        R.doRotateToAngle(heading);

        R.unlock();
        R.Sleep(100);
        R.lock();
#ifdef _DEBUG
        std::cerr <<"POTF "
                <<" X="<<std::setw(6)<< R.getX() <<" Y="<<std::setw(6)<< R.getY() <<" T="<<std::setw(6)<< R.getTh() 
                <<" V:"<<std::setw(4)<<velocity  <<" H:"<<std::setw(4)<< heading  <<" O:"<<OBST.getValue(position)
                << std::endl;
#endif
    }
    R.unlock();
}


void actPath_Direct(NAV_Robot& R, NAV_Map & ML, NAV_Map & MG, AutoSynchronized<ArPose> & G) {
    MagMode initial_mode = robotMode.getValue();

    NAV_FIELD_Potential POT(&ML);
    POT.register_map(&MG);

    NAV_FIELD_Potential OBST(&ML);
    OBST.register_map(&MG);
    OBST.setUseGoal(false);

    NAV_Path_Flood      FLOOD(&ML);

    ArPose GOAL = G.getValue();

    // This will set the goal only once, if desired to update then you need to do it every now and then
    POT.setGoal(GOAL);      // POT update has little numerical complexity
    FLOOD.setGoal(GOAL);    // FLOOD update may tkae some time -- O(n^2) where n is the current map size

    R.lock();
    unsigned int MODE = 0; // 0 == use POT, 1 == use FLOOD
    while ( needtorestart.getValue()==0 && robotMode.getValue()==initial_mode ) {
        if ( !R.is_connected() ) {
            needtorestart.setValue(1);
            break;
        }
        if ( R.isStalled() ) {
            needtorestart.setValue(1);
            break;
        }

        // recommendation from POT (uses MAP updates on the fly but you can get stack in a local minimum)
        double Pvelocity = 0;
        double Pheading  = 0;
        // recommendation from FLOOD (done quickly once path calculated but no MAP updates are used)
        double Fvelocity = 0;
        double Fheading  = 0;
        // this is what we will do
        double velocity = 0;
        double heading  = 0;

        ArPose position = R.getPose();

        if (POT.is_goal(position) || FLOOD.is_goal(position) ) {
            break;
        }

        if ( GOAL!=G.getValue() ) {
            R.Stop();
            R.unlock();
            GOAL = G.getValue();
            POT.setGoal(GOAL);      // POT update has little numerical complexity
            FLOOD.setGoal(GOAL);    // FLOOD update may tkae some time -- O(n^2) where n is the current map size
            R.lock();
        }


        Pvelocity = POT.getVelCoef(position);
        Pheading  = POT.getHeading(position);
        Fvelocity = FLOOD.getVelCoef(position);
        Fheading  = FLOOD.getHeading(position);

        if (MODE==0) {
            if (Pvelocity<0.05) {
                // Local Minimum or goal reached (that was already checked
                MODE=1;
            }
        } else if (MODE==1) {
            if ( ArMath::fabs( ArMath::fixAngle(Fheading-Pheading) ) < 50  && Pvelocity>0.05 ) {
                // FLOOD goes in the same direction that POT again and no local minimum, switch to POT
                MODE=0;
            }
        } else {
            std::cerr << "ERROR Internal program error: illegal MODE value" << std::endl;
            break;
        }

        if (MODE==0) {  // POT
            velocity = 200. * Pvelocity;
            heading = Pheading;
        } else if (MODE==1) {   // FLOOD
            if (OBST.getValue(position)>2.0) {
                // we are running into an obstacle! recompute from the the map
                FLOOD.setGoal(GOAL);
                std::cerr << "WARN: close obstacles, recomputing flood algorithm!" << std::endl;
                continue;
            }
            if (!FLOOD.is_reachable(position)) {
                std::cerr << "WARN: destination unreachable!" << std::endl;
                break; // destination unreachable
            }
            velocity = 100. * Fvelocity;
            heading = Fheading;
        } else {
            std::cerr << "ERROR Internal program error: illegal path MODE value" << std::endl;
            break;
        }
        R.doDriveSpeed(velocity);
        R.doRotateToAngle(heading);

        R.unlock();
        R.Sleep(100);
        R.lock();
#ifdef _DEBUG
        std::cerr <<"PATH "
                <<" X="<<std::setw(6)<< R.getX() <<" Y="<<std::setw(6)<< R.getY() <<" T="<<std::setw(6)<< R.getTh() 
                <<" V:"<<std::setw(4)<<velocity  <<" H:"<<std::setw(4)<< heading  <<" M="<<std::setw(1)<< MODE 
                <<" O:"<<OBST.getValue(position)
                << std::endl;
#endif
    }
    R.unlock();
}