#ifndef ISRG_DAQ_20060318_VZRGNTERNGTVEYGUVFFRZRFGREOHGFURVFYRNIVATFBBASBBONE
#define ISRG_DAQ_20060318_VZRGNTERNGTVEYGUVFFRZRFGREOHGFURVFYRNIVATFBBASBBONE

//#include <rtai_pthread.h>
//#include <rtai_sched.h>
//#include <rtai_mbx.h>

#ifdef __cplusplus
extern "C" {
#endif

#define ISRG_DAQ_BASE_HZ 10
#define ISRG_DAQ_N_ENCODER_FAST 10
// encoders are sampled at a rate of
// ISRG_DAQ_SAMPLE_RATE_HZ * ISRG_DAQ_N_ENCODER_FAST
#define ISRG_DAQ_BUFFER_SIZE 64 // must be a power of 2
// The buffer repeats every:
// ISRG_DAQ_BUFFER_SIZE / ISRG_DAQ_BASE_HZ seconds
#define ISRG_DAQ_SHMNAME "isrgda"

#define ISRG_DAQ_DEAD_RECKONING_PROCESS 0
#define ISRG_DAQ_GPS_PROCESS 1
#define ISRG_DAQ_COMPASS_PROCESS 2
#define ISRG_DAQ_CONTROL_PROCESS 3
#define ISRG_DAQ_OTHER_PROCESS 4
#define ISRG_DAQ_HIGHLEVEL_PROCESS 5
#define ISRG_DAQ_N_PROCESSES 6

#define ISRG_DAQ_FIRST_FIFO 2

// F = Front
// D/F = Down/Forward
// L/C/R = Left/Center/Right
#define ISRG_DAQ_N_ULTRASONICS 8
#define ISRG_DAQ_US_FDL 4 // Front, facing down, on the left
#define ISRG_DAQ_US_FDC 2
#define ISRG_DAQ_US_FDR 0
#define ISRG_DAQ_US_FFL 5 // Front, facing forward, on the left
#define ISRG_DAQ_US_FFC 3
#define ISRG_DAQ_US_FFR 1
#define ISRG_DAQ_US_FR 6 // Front, facing right
#define ISRG_DAQ_US_FL 7 // Front, facing left

#define ISRG_DAQ_US_ONE_METER 550 // ...approximate...

typedef unsigned long isrg_daq_sample_t;

typedef struct {
    int left;
    int right;
} isrg_daq_encoder_t;

typedef struct {
    int craziness_timeout;
    int left;
    int right;
} isrg_daq_pwm_t;

typedef struct {
    int valid;
    int left;
    int right;
    int safety_enforced;
    int safety_stopped;
    int move_goal_met;
} isrg_daq_pwm_control_t;

typedef enum {
    LL_CMD_NOP,
    LL_CMD_RESET, // stop all controllers and reset them
    LL_CMD_MOVE, // move robot to location specified by distance and angle
    LL_CMD_ALIGN_NORTH,
    LL_CMD_ALIGN_EAST
} isrg_daq_ll_cmd_t;

typedef struct {
    int valid;
    isrg_daq_ll_cmd_t command;
    double distance;
    double angle;
} isrg_daq_low_level_command_t;

typedef struct {
    double x;
    double y;
    double heading;
    double dx;
    double dy;
    double dd; // distance travelled (+ for forward, - for reverse)
    double dheading; // radians rotated
    double abs_distance; // not used yet....
    double abs_heading; // not used yet....
    double dl;
    double dr;
    // WARNING> the compass fields don't work:
    double dheading_compass; // dheading * kcompass
    float kcompass; // compass->dheading / encoder_position->dheading
    float kcompass_iir; // IIR filtered version of kcompass
} isrg_daq_encoder_dead_reckoning_t;

typedef struct {
    int age; // number of samples elapsed since the last data point
    int satellites_tracked;
    double latitude;
    double longitude;
    double altitude;
    double northing;
    double easting;
} isrg_daq_gps_t;

typedef struct {
    int ultrasonic[ISRG_DAQ_N_ULTRASONICS];
} isrg_daq_ultrasonics_t;

typedef struct {
    int good; // 1 = data is okayish, 0 = data is bad
    float heading; // deprecated (use north_heading instead)
    float north_heading;
    float east_heading;
    float tilt;
    float roll;
    float dheading;
} isrg_daq_compass_t;

typedef struct {
    int age;
    long long seconds_since_epoch;
} isrg_daq_gps_time_t;

typedef struct {
    // run :=
    //      0 if the process did not start
    //      1 if the process started
    //      2 if the process finished
    int run[ISRG_DAQ_N_PROCESSES];
    // finished :=
    //      the sample number at which the process finished (in general,
    //      this should be the current sample if the process is meeting
    //      realtime deadlines)
    isrg_daq_sample_t finished[ISRG_DAQ_N_PROCESSES];
} isrg_daq_processes_t;

typedef struct {
    isrg_daq_sample_t sample_number;
    isrg_daq_encoder_t                  encoder;
    isrg_daq_encoder_t                  encoder_fast[ISRG_DAQ_N_ENCODER_FAST];
    isrg_daq_ultrasonics_t              ultrasonics;
    isrg_daq_encoder_dead_reckoning_t   encoder_position;
    isrg_daq_compass_t                  compass;
    isrg_daq_gps_t                      gps;
    isrg_daq_pwm_t                      pwm;
    isrg_daq_pwm_control_t              pwm_control;
    // sent by high level control process:
    isrg_daq_low_level_command_t        low_level_command;
} isrg_daq_record_t;

typedef struct {
    isrg_daq_sample_t current_sample;
    isrg_daq_record_t record[ISRG_DAQ_BUFFER_SIZE];

    // Process synchronization:
    int current_process;
    int process_ctr[ISRG_DAQ_N_PROCESSES];
    int process_running[ISRG_DAQ_N_PROCESSES];
    // Look at all this stuff that doesn't work!
    //MBX process_mbox[ISRG_DAQ_N_PROCESSES];
    //MBX *process_mboxp[ISRG_DAQ_N_PROCESSES];
    //pthread_mutex_t process_mutex[ISRG_DAQ_N_PROCESSES];
    //pthread_cond_t process_cond[ISRG_DAQ_N_PROCESSES];
    int process_fifo[ISRG_DAQ_N_PROCESSES];

} isrg_daq_buffer_t;

void isrg_daq_connect (int process_number);
void isrg_daq_disconnect ();
isrg_daq_sample_t isrg_daq_wait_process ();
void isrg_daq_next_process ();
isrg_daq_sample_t isrg_daq_wait_process2 ();
const isrg_daq_record_t *isrg_daq_get_record (isrg_daq_sample_t sample_number);
isrg_daq_record_t *isrg_daq_get_record_for_write (isrg_daq_sample_t
                                                  sample_number);

// Called by a process whenever it gets a new gps or compass value:
void isrg_daq_update_gps (isrg_daq_gps_t *gps);
void isrg_daq_update_gps_time (isrg_daq_gps_time_t *gps_time);
void isrg_daq_update_compass (isrg_daq_compass_t *compass);

#ifdef __cplusplus
}
#endif

#endif