/**
 * Copyright (c) 2011 Colony Project
 * 
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 * 
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

/**
 * @file MotorControl.cpp
 * @brief Contains motor declarations and functions
 * 
 * Contains functions and definitions for the use of
 * motors
 *
 * @author Colony Project, CMU Robotics Club
 * @author Ben Wasserman
 * @author Alex Zirbel
 **/

#include "MotorControl.h"

using namespace std;

/**
 * @brief Initialize the motors module of libscout.
 *
 * Initialize the libscout node as a publisher of set_motors and a client of
 * query_motors.
 */
MotorControl::MotorControl(const ros::NodeHandle& libscout_node,
                           string scoutname)
    : node(libscout_node)
{
    set_motors_pub =
        node.advertise<motors::set_motors>(scoutname + "/set_motors",
                                           QUEUE_SIZE);
    query_motors_client =
        node.serviceClient<motors::query_motors>(scoutname + "/query_motors");
}

MotorControl::~MotorControl()
{
    set_sides(0, 0, MOTOR_ABSOLUTE);
}

/**
 * @brief Sets all the speeds according to the specificiation of which motors
 *        to set.
 *
 * @param which A bitmask of which motors need to be set.
 * @param speed The value to set the motors to.
 * @param units The units the speed is expressed in.
 * @return Function status
 */
void MotorControl::set(int which, float speed, char units)
{
    float speed_fl, speed_fr, speed_bl, speed_br;
    speed_fl = speed_fr = speed_bl = speed_br = speed;

    if(which & MOTOR_FL_REV)
    {
        speed_fl = -speed;
    }
    if(which & MOTOR_FR_REV)
    {
        speed_fr = -speed;
    }
    if(which & MOTOR_BL_REV)
    {
        speed_bl = -speed;
    }
    if(which & MOTOR_BR_REV)
    {
        speed_br = -speed;
    }

    set_each(which, speed_fl, speed_fr, speed_bl, speed_br, units);
}

/**
 * @brief Sets the left and right sides of scout to different speeds.
 *
 * @param speed_l The speed of both left motors.
 * @param speed_r The speed of both right motors.
 * @param units The units to set to.
 * @return Function status
 */
void MotorControl::set_sides(float speed_l, float speed_r, char units)
{
    set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r, units);
}

/**
 * @brief Set motor speeds
 * Sets the speeds of the motors as a percentage of top speed. Can selectively
 * select which motors to set, and which to keep at previous speed.
 *
 * @param which A bitmask of which motors should be set.
 * @param speed The speed the motors should be set to.
 * @param units Optional units for the speeds.
 * @return Function status
 */
void MotorControl::set_each(int which,
                            float speed_fl, float speed_fr,
                            float speed_bl, float speed_br,
                            char units)
{
    check_which_ok(which);

    motors::set_motors msg;


    /* Tell the motors node which motors need to be updated */
    msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
    if(which & (MOTOR_FL | MOTOR_FL_REV))
    {
        msg.fl_set = true;
        motor_fl_speed = rel_to_abs(speed_fl, units);
    }
    if(which & (MOTOR_FR | MOTOR_FR_REV))
    {
        msg.fr_set = true;
        motor_fr_speed = rel_to_abs(speed_fr, units);
    }
    if(which & (MOTOR_BL | MOTOR_BL_REV))
    {
        msg.bl_set = true;
        motor_bl_speed = rel_to_abs(speed_bl, units);
    }
    if(which & (MOTOR_BR | MOTOR_BR_REV))
    {
        msg.br_set = true;
        motor_br_speed = rel_to_abs(speed_br, units);
    }

    /* Set all the speeds (the booleans in the set_motors message determine
     * which ones will be used) */
    msg.fl_speed = trim_speed("FL", (int) motor_fl_speed);
    msg.fr_speed = trim_speed("FR", (int) motor_fr_speed);
    msg.bl_speed = trim_speed("BL", (int) motor_bl_speed);
    msg.br_speed = trim_speed("BR", (int) motor_br_speed);

    /* Publishes message to set_motors topic */
    set_motors_pub.publish(msg);
    ros::spinOnce();
}

/**
 * Double-checks the which variable to make sure the front and back
 * bits are not both set for a single motor.
 *
 * @param which The which motor specfication to check.
 */
void MotorControl::check_which_ok(int which)
{
    ROS_ERROR_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)),
        "FL Motor set in both directions!");
    ROS_ERROR_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)),
        "FR Motor set in both directions!");
    ROS_ERROR_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)),
        "BL Motor set in both directions!");
    ROS_ERROR_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)),
        "BR Motor set in both directions!");
}

/**
 * Trims an absolute speed to stay within motor maximum range.
 * Uses ROS_WARN to warn the user if a speed has been trimmed.
 *
 * @param which_str A description of which motor, to be used in the warning
 *                  message (for example, "FL")
 * @param speed The requested speed to be set to
 * @return The requested speed, trimmed to stay within limits.
 */
int MotorControl::trim_speed(string which_str, int speed)
{
    int trimmed_speed = speed;
    if (speed > MAXSPEED_ABS)
    {
        trimmed_speed = (int) MAXSPEED_ABS;
        ROS_WARN("Motor (%s) speed trimmed to %d.", which_str.c_str(),
            (int) MAXSPEED_ABS);
    }
    else if (speed < -MAXSPEED_ABS)
    {
        trimmed_speed = (int) -MAXSPEED_ABS;
        ROS_WARN("Motor (%s) speed trimmed to %d.", which_str.c_str(),
            (int) -MAXSPEED_ABS);
    }

    return trimmed_speed;
}

/**
 * @brief Query the current speeds of the motors
 *
 * Sends a request to the query_motors service which will reply with the
 *  current speed of each motor.
 *
 * @TODO Change so we can get multiple motor speeds with one call
 *
 * @param which A bitmask that will specify which motor speed should be
 *  returned
 * @return The speed of the selected motor
 */
float MotorControl::query(int which)
{
    motors::query_motors srv;
    if(query_motors_client.call(srv))
    {
        switch(which)
        {
            case MOTOR_FL:
                return srv.response.fl_speed;
            case MOTOR_FR:
                return srv.response.fr_speed;
            case MOTOR_BL:
                return srv.response.bl_speed;
            case MOTOR_BR:
                return srv.response.br_speed;
            default:
                /// @todo: I hate this. Let's fix it soon.
                ROS_FATAL("Bad WHICH in motors_query.");
        }
    }
    else
    {
        ROS_FATAL("Failed to call service query_motors");
    }

    return 0;
}

/**
 * @brief Converts set speeds (of various units) to absolute speeds.
 *
 * @param speed The speed expressed in the desired units
 * @param units The units the desired speed is measured in
 * @return The absolute speed of the motor
 **/
float MotorControl::rel_to_abs(float rel_speed, int units)
{
    ROS_INFO("Input: %f of units #%d.", rel_speed, units);
    switch(units)
    {
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
        return rel_speed;
      case MOTOR_PERCENT:/* Convert from percentage */
        return rel_speed * MAXSPEED_ABS / 100.0;
      case MOTOR_MMS:/* Convert from mm/s */
        return rel_speed * MAXSPEED_ABS / (1000.0 * MAXSPEED_MPS);
      case MOTOR_CMS:/* Convert from cm/s */
        return rel_speed * MAXSPEED_ABS / (100.0 * MAXSPEED_MPS);
      default: /* The units aren't recognized */
        ROS_WARN("MotorControl::rel_to_abs used with default units.");
        return rel_speed;
    }
}

/**
 * @brief Convert absolute speeds to speeds of various units.
 *
 * @param speed The speed expressed in absolute units.
 * @param units The units the desired speed is measured in.
 * @return The relative speed of the motor in desired units.
 **/
float MotorControl::abs_to_rel(float abs_speed, int units)
{
    switch(units)
    {
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
        return abs_speed;
      case MOTOR_PERCENT:/* Convert from percentage */
        return abs_speed * 100.0 / MAXSPEED_ABS;
      case MOTOR_MMS:/* Convert from mm/s */
        return abs_speed * MAXSPEED_MPS * 1000.0 / MAXSPEED_ABS;
      case MOTOR_CMS:/* Convert from cm/s */
        return abs_speed * MAXSPEED_MPS * 100.0 / MAXSPEED_ABS;
      default: /* The units aren't recognized */
        ROS_WARN("MotorControl::rel_to_abs used with default units.");
        return abs_speed;
    }
}