
#include "romaa.h"

// PLAYER_MSGx(level, ...)
// General messages. Use level to indicate the message importance
// - 0 : important
// - 1 : informative
// - 2+: diagnostic

////////////////////////////////////////////////////////////////////////////////
// Constructor.  Retrieve options from the configuration file and do any
// pre-Setup() setup.
Romaa::Romaa(ConfigFile* cf, int section)
    : ThreadedDriver(cf, section)
{
		memset(&position2d_addr, 0, sizeof(player_devaddr_t));
		memset(&position2d_geom, 0, sizeof(player_position2d_geom_t));
		memset(&position2d_data, 0, sizeof(player_position2d_data_t));

		if ( cf->ReadDeviceAddr(&position2d_addr, section, "provides",
								PLAYER_POSITION2D_CODE, -1, NULL) == 0 )
		{
				if ( this->AddInterface(position2d_addr) != 0 )
				{
						PLAYER_ERROR("Error adding position2d interface");
						this->SetError(-1);
						return;
				}
		}

		// Read an option from the configuration file
		port = cf->ReadString(section, "port", DEFAULT_SERIALPORT);
		baudrate = cf->ReadInt(section, "baudrate", DEFAULT_BAUDRAE);

		motor_pid_kp = cf->ReadTupleFloat(section, "motorpid", 0, DEFAULT_MOTOR_PID_KP);
		motor_pid_ki = cf->ReadTupleFloat(section, "motorpid", 1, DEFAULT_MOTOR_PID_KI);
		motor_pid_kd = cf->ReadTupleFloat(section, "motorpid", 2, DEFAULT_MOTOR_PID_KD);

		vw_pid_kp = cf->ReadTupleFloat(section, "vwpid", 0, DEFAULT_VW_PID_KP);
		vw_pid_ki = cf->ReadTupleFloat(section, "vwpid", 1, DEFAULT_VW_PID_KI);
		vw_pid_kd = cf->ReadTupleFloat(section, "vwpid", 2, DEFAULT_VW_PID_KD);

		t_odometry = cf->ReadInt(section, "todometry", DEFAULT_T_ODOMETRY);
		t_loop = cf->ReadInt(section, "tloop", DERAULT_T_LOOP);

    wheel_control = cf->ReadBool(section, "wheelcontrol", 0);
//    speed_enccount = cf->ReadBool(section, "speedasenccount", 0);
		return;
}

// A factory creation function, declared outside of the class so that it
// can be invoked without any object context (alternatively, you can
// declare it static in the class).  In this function, we create and return
// (as a generic Driver*) a pointer to a new instance of this driver.
Driver* 
Romaa_Init(ConfigFile* cf, int section)
{
  // Create and return a new instance of this driver
  return((Driver*)(new Romaa(cf, section)));
}

// A driver registration function, again declared outside of the class so
// that it can be invoked without object context.  In this function, we add
// the driver into the given driver table, indicating which interface the
// driver can support and how to create a driver instance.
void Romaa_Register(DriverTable* table)
{
  table->AddDriver("romaa", Romaa_Init);
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device.  Return 0 if things go well, and -1 otherwise.
int 
Romaa::MainSetup()
{   
  PLAYER_MSG0(1, "romaa driver initialising");

  // Here you do whatever is necessary to setup the device, like open and
  // configure a serial port.
  PLAYER_MSG0(1, "Connecting at...");
  PLAYER_MSG1(1, "serial por device: %s", port.c_str());
  PLAYER_MSG1(1, "baudrate: %d", baudrate);

  romaa_comms = new romaa_comms_flexiport(port, baudrate);

  romaa_comms->reset_odometry();
  usleep(100000);
  romaa_comms->enable_motors();
  usleep(100000);
  romaa_comms->set_motor_pid(motor_pid_kp, motor_pid_ki, motor_pid_kd);
  PLAYER_MSG3(2, "setting motor PID: %.3f %.3f %.3f", motor_pid_kp, motor_pid_ki, motor_pid_kd);
  usleep(100000);
  romaa_comms->set_vw_pid(vw_pid_kp, vw_pid_ki, vw_pid_kd);
  PLAYER_MSG3(2, "setting vw PID: %.3f %.3f %.3f", vw_pid_kp, vw_pid_ki, vw_pid_kd);
  usleep(100000);
  romaa_comms->set_t_loop(t_loop);
  PLAYER_MSG1(2, "setting t_loop: %d", t_loop);
  usleep(100000);
  romaa_comms->set_t_odometry(t_odometry);
  PLAYER_MSG1(2, "setting t_odometry: %d", t_odometry);

  PLAYER_MSG0(1, "romaa driver ready");
  return(0);
}

////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
void 
Romaa::MainQuit()
{
  PLAYER_MSG0(1, "Shutting romaa driver down.");
  // Here you would shut the device down by, for example, closing a
  // serial port.
  usleep(100000);
  romaa_comms->set_speed(0.0, 0.0);
  usleep(100000);
  romaa_comms->set_loop_mode(OPEN_LOOP_MODE);
  usleep(500000);
  romaa_comms->disable_motors();
  delete romaa_comms;
  PLAYER_MSG0(1, "romaa driver has been shutdown.");
}

////////////////////////////////////////////////////////////////////////////////
int 
Romaa::ProcessMessage(QueuePointer & resp_queue, 
		player_msghdr * hdr,
		void * data)
{
  // Process messages here.  Send a response if necessary, using Publish().
  // If you handle the message successfully, return 0.  Otherwise,
  // return -1, and a NACK will be sent for you, if a response is required.

  if( Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,      // PLAYER_POSITION2D_REQ_GET_GEOM
        PLAYER_POSITION2D_REQ_GET_GEOM, 
        position2d_addr) )
  {
    PLAYER_MSG0(3, "Reading body geometry...");
    if( romaa_comms->get_body_geometry(size_w, size_l, size_h) == -1 )
    {
      PLAYER_ERROR("getting body geometry");
    }
    else
    {
      position2d_geom.size.sw = size_w;
      position2d_geom.size.sl = size_l;
      position2d_geom.size.sh = size_h;

      // READ FROM ROMAA
      position2d_geom.pose.px = -0.111;
      position2d_geom.pose.py = 0.0;
      position2d_geom.pose.pz = 0.0;
      position2d_geom.pose.proll = 0.0;
      position2d_geom.pose.ppitch = 0.0;
      position2d_geom.pose.pyaw = 0.0;

      Publish(position2d_addr, resp_queue, 
          PLAYER_MSGTYPE_RESP_ACK,
          PLAYER_POSITION2D_REQ_GET_GEOM,
          (void*)&position2d_geom, sizeof(player_position2d_geom_t));
      return (0);
    }
  }
  else if( Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,    // PLAYER_POSITION2D_REQ_RESET_ODOM
          PLAYER_POSITION2D_REQ_RESET_ODOM,
          this->position2d_addr) )
  {
    // reset position to 0,0,0: no args
    if( hdr->size != 0 )
    {
      PLAYER_WARN("Arg to reset position request is wrong size; ignorint");
      return (-1);
    }
    PLAYER_MSG0(3, "Reset odometry...");
    romaa_comms->reset_odometry();

    Publish(this->position2d_addr, resp_queue,
        PLAYER_MSGTYPE_RESP_ACK,
        PLAYER_POSITION2D_REQ_RESET_ODOM);
    return (0);
  }
  else if( Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, // PLAYER_POSITION2D_REQ_SET_ODOM
        PLAYER_POSITION2D_REQ_SET_ODOM,
        this->position2d_addr) )
  {
    if( hdr->size != sizeof(player_position2d_set_odom_req_t) )
    {
      PLAYER_WARN("Arg to odometry set requests wrong size; ignoring");
      return(-1);
    }
    player_position2d_set_odom_req_t* set_odom_req =
      (player_position2d_set_odom_req_t*)data;

    if ( ( set_odom_req->pose.px == 0) &&
        ( set_odom_req->pose.py == 0) &&
        ( set_odom_req->pose.pa == 0) )
    {
      PLAYER_MSG0(3, "Reset odometry...");
      romaa_comms->reset_odometry();
    }
    else
    {
      PLAYER_MSG0(3, "Set odometry...");
      romaa_comms->set_odometry((float)set_odom_req->pose.px,
          (float)set_odom_req->pose.py,
          (float)set_odom_req->pose.pa);
    }

    Publish(this->position2d_addr, resp_queue,
        PLAYER_MSGTYPE_RESP_ACK, 
        PLAYER_POSITION2D_REQ_SET_ODOM);
    return (0);
  }
  else if ( Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,   // PLAYER_POSITION2D_REQ_MOTOR_POWER
        PLAYER_POSITION2D_REQ_MOTOR_POWER,
        this->position2d_addr) )                    
  {
    // motor state change request
    // 1 = enable motors
    // 0 = disable motors
    if(hdr->size != sizeof(player_position2d_power_config_t))
    {
      PLAYER_WARN("Arg to motor state change request wrong size; ignoring");
      return(-1);
    }
    player_position2d_power_config_t* power_config =
            (player_position2d_power_config_t*)data;
    if( power_config->state == TRUE )
    {
      PLAYER_MSG0(3, "Enable motors...");
      romaa_comms->enable_motors();
    }
    else
    {
      PLAYER_MSG0(3, "Disable motors...");
      romaa_comms->disable_motors();
    }

    Publish(this->position2d_addr, resp_queue,
        PLAYER_MSGTYPE_RESP_ACK, 
        PLAYER_POSITION2D_REQ_MOTOR_POWER);
    return (0);
  }
  else if( Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, // PLAYER_POSITION2D_CMD_VEL
        PLAYER_POSITION2D_CMD_VEL,
        this->position2d_addr) )
  {
    memcpy(&position2d_cmd_vel, data, sizeof(player_position2d_cmd_vel_t));

    if( wheel_control == 0 )    // set v,w speed
    {
      PLAYER_MSG0(3, "Set speed...");
      romaa_comms->set_speed(position2d_cmd_vel.vel.px, 
          position2d_cmd_vel.vel.pa);
    }
    else                        // set wheel linear speed
    {
      PLAYER_MSG0(3, "Set wheel speed...");
      romaa_comms->set_wheel_speed(position2d_cmd_vel.vel.px, 
          position2d_cmd_vel.vel.py);

    }
    return (0);
  }
  else
  {
    return (-1);
  }
}

////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void 
Romaa::Main() 
{
  // The main loop; interact with the device here
  for(;;)
  {
    // test if we are supposed to cancel
    pthread_testcancel();

    // Process incoming messages.  Romaa::ProcessMessage() is
    // called on each message.
    ProcessMessages();

    // Interact with the device, and push out the resulting data, using
    // Driver::Publish()
   
    // Update position2d data
    PLAYER_MSG0(3, "Reading odometry...");
    if( romaa_comms->get_odometry(x, y, theta) == -1 )
    {
      PLAYER_ERROR("read odometry");
    }
    else
    {
      position2d_data.pos.px = x;
      position2d_data.pos.py = y;
      position2d_data.pos.pa = theta;
    }
    usleep(20000);
    PLAYER_MSG0(3, "Reading speed...");
    if( romaa_comms->get_speed(v, w) == -1 )
//    if( romaa_comms->get_encoder_counter(left_enccounter, right_enccounter) == -1 )
    {
      PLAYER_ERROR("read speed");
    }
    else
    {
      v = (double)left_enccounter;
      w = (double)right_enccounter;
      position2d_data.vel.px = v;
      position2d_data.vel.pa = w;
    }
    PLAYER_MSG3(2, "Odometry: %.3f %.3f %.3f", x, y, theta);

    Publish(position2d_addr,
        PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
        (void*)&position2d_data, sizeof(player_position2d_data_t), NULL);

    // Sleep (you might, for example, block on a read() instead)
    usleep(100000);
  }
}


////////////////////////////////////////////////////////////////////////////////
// Extra stuff for building a shared object.

/* need the extern to avoid C++ name-mangling  */
extern "C" {
  int player_driver_init(DriverTable* table)
  {
    PLAYER_MSG0(1, "romaa driver initializing");
    Romaa_Register(table);
    PLAYER_MSG0(1, "romaa driver done");
    return(0);
  }
}

