state.h

#ifndef STATE_H_LOADED
#define STATE_H_LOADED 1

#include "quaternion.h"
#include "kalman.h"
#include "qp.h"
#include "types.h"

// Angular speeds in this the Schedule subclasses are in rad/s; speeds
// in the State class are either in rad/s or in 'fractional' speeds,
// which are speeds expressed as a fraction of the drive's maximum
// speed.  This maximum speed is discovered (starting with an initial
// estimate) by the Kalman filters within the State object.

class State;

class Schedule {
 protected:
    double phidot_;
    double thetadot_;

    Schedule();

 public:
    virtual ms_t step(double current_phi, double current_theta, ms_t t) = 0;

    double phidot(void) const { return phidot_; }

    double thetadot(void) const { return thetadot_; }

#if PROVARINO
    virtual std::ostream& tostream(std::ostream& strm) const = 0;
#endif
};

class SlewSchedule : public Schedule
{
private:
    float target_phi_;          // the longitude we're aiming for
                                // (+ve eastwards, rad, may be outside of [0,2pi])
    float target_theta_;        // altitude (rad, less than pi/2)
    ms_t full_speed_time_;      // time we aim to be at full speed
    bool route_planned_p_;      // have we worked out the route?
    bool schedule_ok_p_;        // is this a feasible schedule?

    double calculate_angdot(double remaining_angle, ms_t current_time);

 public:
    SlewSchedule(const Position& target);
    ms_t step(double current_phi, double current_theta, ms_t t);
    bool schedule_ok() const { return schedule_ok_p_; };

#if PROVARINO
    std::ostream& tostream(std::ostream& strm) const;
#endif
};

class TrackingSchedule : public Schedule {
 private:
    // initial_ and initial_step_ are 'almost' const, but they're not
    // initialised until the first call to step(), so can't be
    // labelled const
    Quaternion initial_;       // where the rotation is from
    ms_t initial_step_;        // the time of the first step
    double maximum_angle_;     // the maximum angle we may move before encountering a stop

    Quaternion axis_;          // the axis being rotated about
    double hdot_;              // the intended rotation speed in rad/ms

 public:
    TrackingSchedule(const Quaternion& axis,
                     double hdot,
                     Position observatory);

    ms_t step(double current_phi, double current_theta, ms_t t);
    double speed(void);

    float limit_tracking(Quaternion& actual, const TelescopeController*);

#if PROVARINO
    std::ostream& tostream(std::ostream& strm) const;
#endif
};

class State {
private:
    Schedule* schedule;
    // schedule is not const, because we will occasionally
    // (in set_schedule()) update a current state with a new schedule/goal

    Quaternion to_observatory;

public:
    State(// const Quaternion& initial,
          // const Position& initial_max_speed,
          Schedule* schedule,
          const ms_t current_time,
          const Position* obs=NULL);

    State();

    ms_t step(ms_t t);
    void alt_click(ms_t t, double current_speed, double angle);
    void az_click(ms_t t, double current_speed, double angle);
    double suggested_phidot_frac(void);
    double suggested_thetadot_frac(void);

    void set_schedule(Schedule*);

    //Kalman* kalman_phi() { return φ }
    //Kalman* kalman_theta() { return θ }

    // Quaternion T(void) const;
    // Position position(Position::System);
};

#if PROVARINO
std::ostream& operator<<(std::ostream& strm, const State& s);
std::ostream& operator<<(std::ostream& strm, const Schedule& s);
#endif
#endif /* STATE_H_LOADED */