#include "vrpn_Poser_Analog.h"


vrpn_Poser_AnalogParam::vrpn_Poser_AnalogParam() {
    // set workspace values to defaults
    for (int i = 0; i < 3; i ++) {
        pos_min[i] = vel_min[i] = -1;
        pos_max[i] = vel_max[i] = 1;
        pos_rot_min[i] = vel_rot_min[i] = -45;
        pos_rot_max[i] = vel_rot_max[i] = 45;
    }
}


bool vrpn_Poser_Analog::setup_channel(vrpn_PA_fullaxis *full)
{
    // If the name is NULL, we're done.
    if (full->axis.ana_name == NULL) { return 0; }

    // Create the Analog Output Remote 
    // If the name starts with the '*' character, use the server
    // connection rather than making a new one.
    if (full->axis.ana_name != NULL) {
        if (full->axis.ana_name[0] == '*') {
            full->ana = new vrpn_Analog_Output_Remote(&(full->axis.ana_name[1]), d_connection);
        } else {
            full->ana = new vrpn_Analog_Output_Remote(full->axis.ana_name);
        }

        if (full->ana == NULL) {
            fprintf(stderr, "vrpn_Poser_Analog: Can't open Analog %s\n", full->axis.ana_name);
	    return false;
        }
    } else {
	full->ana = NULL;
	fprintf(stderr, "vrpn_Poser_Analog: Can't open Analog: No name given\n");
	return false;
    }
    return true;
}

vrpn_Poser_Analog::vrpn_Poser_Analog(const char* name, vrpn_Connection * c, vrpn_Poser_AnalogParam* p, bool act_as_tracker) :
    vrpn_Poser(name, c),
    vrpn_Tracker(name, c),
    d_act_as_tracker(act_as_tracker)
{
    int i;

    //	register_server_handlers();

    // Make sure that we have a valid connection
    if (d_connection == NULL) {
	    fprintf(stderr,"vrpn_Poser_Analog: No connection\n");
	    return;
    }

    // Register a handler for the position change callback for this device
    if (register_autodeleted_handler(req_position_m_id,
		    handle_change_message, this, d_sender_id)) {
	    fprintf(stderr,"vrpn_Poser_Analog: can't register position handler\n");
	    d_connection = NULL;
    }

    // Register a handler for the velocity change callback for this device
    if (register_autodeleted_handler(req_velocity_m_id,
			handle_vel_change_message, this, d_sender_id)) {
	    fprintf(stderr,"vrpn_Poser_Analog: can't register velocity handler\n");
	    d_connection = NULL;
    }

    // Set up the axes
    x.axis = p->x; y.axis = p->y; z.axis = p->z;
    rx.axis = p->rx; ry.axis = p->ry; rz.axis = p->rz;

    x.ana = y.ana = z.ana = NULL;
    rx.ana = ry.ana = rz.ana = NULL;

    x.value = y.value = z.value = 0.0;
    rx.value = ry.value = rz.value = 0.0;

    x.pa = this; y.pa = this; z.pa = this;
    rx.pa = this; ry.pa = this; rz.pa = this;

    //--------------------------------------------------------------------
    // Open analog remotes for any channels that have non-NULL names.
    // If the name starts with the "*" character, use tracker
    //      connection rather than getting a new connection for it.
    setup_channel(&x);
    setup_channel(&y);
    setup_channel(&z);
    setup_channel(&rx);
    setup_channel(&ry);
    setup_channel(&rz);

    // Set up the workspace max and min values
    for (i = 0; i < 3; i++) {
        p_pos_min[i] = p->pos_min[i];
        p_pos_max[i] = p->pos_max[i];
        p_vel_min[i] = p->vel_min[i];
        p_vel_max[i] = p->vel_max[i];
        p_pos_rot_min[i] = p->pos_rot_min[i];
        p_pos_rot_max[i] = p->pos_rot_max[i];
        p_vel_rot_min[i] = p->vel_rot_min[i];
        p_vel_rot_max[i] = p->vel_rot_max[i];
    }

    // Check the pose for each channel against the max and min values of the workspace
    // and set it to the location closest to the origin.
    p_pos[0] = p_pos[1] = p_pos[2] = 0.0;
    p_quat[0] = p_quat[1] = p_quat[2] = 0.0; p_quat[3] = 1.0;
    for (i = 0; i < 3; i++) {
        if (p_pos[i] < p_pos_min[i]) {
            p_pos[i] = p_pos_min[i];
        }
        else if (p_pos[i] > p_pos_max[i]) {
            p_pos[i] = p_pos_max[i];
        }
    }
}

vrpn_Poser_Analog::~vrpn_Poser_Analog() {}

void vrpn_Poser_Analog::mainloop()
{
    // Call generic server mainloop, since we are a server
    server_mainloop();

    // Call the Analog outputs' mainloops
    if (x.ana != NULL) { x.ana->mainloop(); };
    if (y.ana != NULL) { y.ana->mainloop(); };
    if (z.ana != NULL) { z.ana->mainloop(); };
    if (rx.ana != NULL) { rx.ana->mainloop(); };
    if (ry.ana != NULL) { ry.ana->mainloop(); };
    if (rz.ana != NULL) { rz.ana->mainloop(); };
}

int vrpn_Poser_Analog::handle_change_message(void* userdata,
	    vrpn_HANDLERPARAM p)
{
    vrpn_Poser_Analog* me = (vrpn_Poser_Analog*)userdata;
    const char* params = (p.buffer);
    int	i;
    bool outside_bounds = false;

    // Fill in the parameters to the poser from the message
    if (p.payload_len != (7 * sizeof(vrpn_float64)) ) {
	    fprintf(stderr,"vrpn_Poser_Server: change message payload error\n");
	    fprintf(stderr,"             (got %d, expected %d)\n",
		    p.payload_len, static_cast<int>(7 * sizeof(vrpn_float64)) );
	    return -1;
    }
    me->p_timestamp = p.msg_time;

    for (i = 0; i < 3; i++) {
	    vrpn_unbuffer(&params, &me->p_pos[i]);
    }
    for (i = 0; i < 4; i++) {
	    vrpn_unbuffer(&params, &me->p_quat[i]);
    }

    // Check the pose against the max and min values of the workspace
    for (i = 0; i < 3; i++) {
        if (me->p_pos[i] < me->p_pos_min[i]) {
            me->p_pos[i] = me->p_pos_min[i];
            outside_bounds = true;
        }
        else if (me->p_pos[i] > me->p_pos_max[i]) {
            me->p_pos[i] = me->p_pos_max[i];
            outside_bounds = true;
        }
    }

    // Update the analog values based on the request we just got.
    if (!me->update_Analog_values()) {
      fprintf(stderr, "vrpn_Poser_Analog: Error updating Analog values\n");
    }

    if (me->d_act_as_tracker) {
      // Tell the client where we actually went (clipped position and orientation).
      // using sensor 0 as the one to use to report.
      me->d_sensor = 0;
      me->pos[0] = me->p_pos[0];
      me->pos[1] = me->p_pos[1];
      me->pos[2] = me->p_pos[2];
      me->d_quat[0] = me->p_quat[0];
      me->d_quat[1] = me->p_quat[1];
      me->d_quat[2] = me->p_quat[2];
      me->d_quat[3] = me->p_quat[3];
      vrpn_gettimeofday(&me->vrpn_Tracker::timestamp, NULL);
      char	msgbuf[1000];
      vrpn_int32	len;
      len = me->vrpn_Tracker::encode_to(msgbuf);
      if (me->d_connection->pack_message(len, me->vrpn_Tracker::timestamp,
        me->position_m_id, me->d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
         fprintf(stderr,"vrpn_Poser_Analog::handle_change_message(): can't write message: tossing\n");
         return -1;
      }
    }

    return 0;
}

int vrpn_Poser_Analog::handle_vel_change_message(void* userdata,
	    vrpn_HANDLERPARAM p)
{
      vrpn_Poser_Analog* me = (vrpn_Poser_Analog*)userdata;
      const char* params = (p.buffer);
      int	i;
    bool outside_bounds = false;

    // Fill in the parameters to the poser from the message
    if (p.payload_len != (8 * sizeof(vrpn_float64)) ) {
	    fprintf(stderr,"vrpn_Poser_Server: velocity message payload error\n");
	    fprintf(stderr,"             (got %d, expected %d)\n",
		    p.payload_len, static_cast<int>(8 * sizeof(vrpn_float64)) );
	    return -1;
    }
    me->p_timestamp = p.msg_time;

    for (i = 0; i < 3; i++) {
	    vrpn_unbuffer(&params, &me->p_vel[i]);
    }
    for (i = 0; i < 4; i++) {
	    vrpn_unbuffer(&params, &me->p_vel_quat[i]);
    }
    vrpn_unbuffer(&params, &me->p_vel_quat_dt);

    // Check the velocity against the max and min values of the workspace
    for (i = 0; i < 3; i++) {
        if (me->p_vel[i] < me->p_vel_min[i]) {
            me->p_vel[i] = me->p_vel_min[i];
            outside_bounds = true;
        }
        else if (me->p_vel[i] > me->p_vel_max[i]) {
            me->p_vel[i] = me->p_vel_max[i];
            outside_bounds = true;
        }
    }

    // XXX Update the values now.

    if (me->d_act_as_tracker) {
      // Tell the client where we actually went (clipped position and orientation).
      // using sensor 0 as the one to use to report.
      me->d_sensor = 0;
      me->vel[0] = me->p_vel[0];
      me->vel[1] = me->p_vel[1];
      me->vel[2] = me->p_vel[2];
      me->vel_quat[0] = me->p_vel_quat[0];
      me->vel_quat[1] = me->p_vel_quat[1];
      me->vel_quat[2] = me->p_vel_quat[2];
      me->vel_quat[3] = me->p_vel_quat[3];
      vrpn_gettimeofday(&me->vrpn_Tracker::timestamp, NULL);
      char	msgbuf[1000];
      vrpn_int32	len;
      len = me->vrpn_Tracker::encode_vel_to(msgbuf);
      if (me->d_connection->pack_message(len, me->vrpn_Tracker::timestamp,
        me->velocity_m_id, me->d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
         fprintf(stderr,"vrpn_Poser_Analog::handle_vel_change_message(): can't write message: tossing\n");
         return -1;
      }
    }

    return 0;
}

bool vrpn_Poser_Analog::update_Analog_values()
{
    vrpn_float64 value;
    bool ret = true;

    // XXX ONLY DOING TRANS FOR NOW...ADD ROT LATER
    if (x.axis.channel != -1 && x.axis.channel < vrpn_CHANNEL_MAX) {
      value = (p_pos[0] - x.axis.offset) * x.axis.scale;
      if (x.ana != NULL) { ret &= x.ana->request_change_channel_value(x.axis.channel, value); }
    }
    if (y.axis.channel != -1 && y.axis.channel < vrpn_CHANNEL_MAX) {
      value = (p_pos[1] - y.axis.offset) * y.axis.scale;
      if (y.ana != NULL) { ret &= y.ana->request_change_channel_value(y.axis.channel, value); }
    }
    if (z.axis.channel != -1 && z.axis.channel < vrpn_CHANNEL_MAX) {
      value = (p_pos[2] - z.axis.offset) * z.axis.scale;
      if (z.ana != NULL) { ret &= z.ana->request_change_channel_value(z.axis.channel, value); }
    }

    return ret;
}
