//-*-c++-*-
/**
 * @file AutoSensorBehavior.h.fsm
 * @brief Takes reading of sensor values across a range of joint positions.
 * outputs them to a file.
 * @author Charles Ruhland
 * @author Jin Su Kim
 */

#ifndef INCLUDED_AutoSensorBehavior_h_
#define INCLUDED_AutoSensorBehavior_h_

#include <fstream>

#include "Behaviors/StateMachine.h"

class AutoReadNode : public HeadPointerNode {
public:
    AutoReadNode() : HeadPointerNode("AutoReadNode") {}

    virtual void DoStart() {
        erouter->addListener(this, EventBase::sensorEGID);
    }

    virtual void processEvent(EventBase const &e) {
        ofstream outfile;

        outfile.open("ir-sensors.txt", ios_base::app);

        // Set initial position
        float init = -0.3;
        float end = 0.3;
        float inc = 0.01;
        for (float angle = init; angle <= end; angle += inc) {
            getMC()->setJoints(0.0, angle, 0.0);
            getMC()->updateOutputs();
            sleep(1);    // Doesn't work
            outfile << state->outputs[HeadPanOffset] << "\t"
                    << state->sensors[CenterIRDistOffset] << "\n";
        }

        postStateCompletion();
    }
};

class AutoSensorBehavior : public StateNode {
public:
    AutoSensorBehavior() : StateNode("AutoSensorBehavior") {}

    virtual void setup() {
#statemachine
    startnode: StateNode =B(GreenButOffset)=> AutoReadNode() =C=> startnode
#endstatemachine
    }
};

#endif // INCLUDED_AutoSensorBehavior_h_