/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003  
 *     Nate Koenig & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/* Desc: Model for Pioneer2 Gripper
 *
 * This model reproduce Pionner2 gripper's main behaviors according to 
 * Player's gripper interface. 
 *
 * Each model that needs to be gripped must contain the following tags in
 * its model description (in the .world file) : <canBeGrip></canBeGrip> and
 * the value  should be either 'yes' or 'no'. If tag is not present, it will
 * be considered to be not available for gripper. This have been implemented
 * to avoid considering every object as grippable, which enhances simulation
 * performances when a lot of objects can be present in simulated world.
 *
 * For now, gripping is emulated which means that when grasping, ODE's
 * physical friction is not used to lift an object. Gripped object is just
 * repositionned to lift's height. Using ODE's friction model have been
 * tried but with no good results. Many bugs and glitches were observed
 * leading to an unusable model :-(
 * 
 * Author: Carle Cote 
 *            (Laborius - Universite de Sherbrooke - Sherbrooke, Quebec, Canada)
 * Date: 23 february 2004
 * 
 * TODOs: 
 *  - Change grips emulated friction for ODE's physical friction model.
 *  - Avoid using rays proximity sensor's geoms to draw rays
 * 
 * WARNINGS:
 *  - pionner2DX (and probably AT) mass modelisation can lead to problem 
 *    lifting heavy objects (can create weird behaviors). Recommended object 
 *    mass should be around 0.1.
 * 
 * CVS: $Id: Pioneer2Gripper.hh,v 1.6 2004/11/10 06:46:11 inspectorg Exp $
 * 
 * Modification description :
 * 
 * Author: Carle Cote 
 *           (Laborius - Universite de Sherbrooke - Sherbrooke, Quebec, Canada)
 * Date: 21 march 2004 
 * Desc.: Patch the shaking bug. This should be a temporary patch until using 
 *        ODE's physic model for friction
 *
 * Author: Carle Cote 
 *           (Laborius - Universite de Sherbrooke - Sherbrooke, Quebec, Canada)
 * Date: 26 may 2004 
 * Desc: Fix model with refactored core
 *          - Change collision detection mechanism
 *          - Change pressure pads to a no collision geoms with their own spaceID
 *          - Move pressure pads a little bit inside the grips to help detect gripped state  
 */

#ifndef GAZEBO_P2GRIPPER_HH
#define GAZEBO_P2GRIPPER_HH

#include "Model.hh"

// Forward declarations
class Body;
class Geom;
class RayGeom;
class GeomData;
class SliderJoint;
class RayProximity;

typedef struct gz_gripper gz_gripper_t;

class Pioneer2Gripper : public Model
{
  // Constructor, destructor
  public: Pioneer2Gripper( World *world );
  public: virtual ~Pioneer2Gripper();

  // Load the model
  public: virtual int Load( WorldFile *file, WorldFileNode *node );

  // Initialize the model
  public: virtual int Init( WorldFile *file, WorldFileNode *node );

  // Finalize the model
  public: virtual int Fini();

  // Update the model state
  public: virtual void Update( double step );

  // Grips-intersection callback
  private: static void UpdateGripCallback( void *data, dGeomID o1, dGeomID o2 );

  // Load ODE stuff
  private: int OdeLoad( WorldFile *file, WorldFileNode *node );

  // Initialize ODE
  private: int OdeInit( WorldFile *file, WorldFileNode *node );

  // Finalize ODE
  private: int OdeFini();

  // Initialize the external interface
  private: int IfaceInit();

  // Finalize the external interface
  private: int IfaceFini();

  // Get commands from the external interface
  private: void IfaceGetCmd();

  // Update the data in the external interface
  private: void IfacePutData();  
  
  // Grip command functions
  public: void OpenGrip();
  public: void CloseGrip();
  public: void StopGrip();
  
  // Lift command functions
  public: void StopLift();
  public: void LiftUp();
  public: void LiftDown();
  
  // adds model in the list of grippable models 
  public: void AddGrippableModel( Model *model);

  // Gripper's mode
  public: enum GripperMode{ GRIPPER_STOP_MODE = 0, GRIPPER_OPEN_MODE, GRIPPER_CLOSE_MODE };
  // Lift's mode
  public: enum LiftMode{ LIFT_STOP_MODE = 0, LIFT_UP_MODE, LIFT_DOWN_MODE };
  
  // Gripper's parameters
  private: dReal gripMaxSpeed;
  private: dReal liftMaxSpeed;
  
  private: dReal fixedbarLength;
  private: dReal fixedbarWidth;
  private: dReal fixedbarHeight;
  private: dReal fixedbarMass;

  private: dReal basebarLength;
  private: dReal basebarWidth;
  private: dReal basebarHeight;
  private: dReal basebarZPos; // Use to set the gripper fully down position
  private: dReal basebarMass;

  private: dReal gripLength;
  private: dReal gripWidth;
  private: dReal gripHeight;
  private: dReal gripMass;
  private: dReal gripsOpenWidth;
  private: dReal gripsCloseWidth;
  private: dReal gripMinPosLimit;
  private: dReal gripMaxPosLimit; 
  private: dReal gripMaxForce;

  private: dReal liftHeight;
  private: dReal liftMinPosLimit;
  private: dReal liftMaxPosLimit;
  private: dReal liftMaxForce;
  
  private: dReal ffTolerance; // used instead of FudgeFactor param to counter ODE bug with joints limit
  private: dReal simFrictionTolerance; // used to avoid gripped model contact with grips for emulating friction.
                                       // If not used, ODE's friction model generates unwanted forces.
                                       
  // These boolean variables indicate the state of the gripper
  //(same nomenclature found on Player side)
  private: bool lift_limit_reach;   //Unused
  private: bool grip_limit_reach;   //Unused
  private: bool outer_beam_obstruct;
  private: bool inner_beam_obstruct;
  private: bool left_paddle_open;   //Unused
  private: bool right_paddle_open;  //Unused
  private: bool paddles_opened;
  private: bool paddles_closed;
  private: bool paddles_moving;
  private: bool paddles_error;      //Unused
  private: bool lift_up;
  private: bool lift_down;
  private: bool lift_moving;
  private: bool lift_error;         //Unused

  // data and state
  private: unsigned char state;
  private: unsigned char beams;

  private: GripperMode gripperMode;
  private: LiftMode liftMode;

  private: bool rightGripperHaveContact;
  private: bool leftGripperHaveContact;
  private: bool isGripping;
  private: Model *grippedModel;
  private: GzVector gripInitialPos;
  private: GzVector grippedModelInitialPos;

  // ODE objects
  private: Body *body;
  private: Body *baseBarBody;
  private: Body *leftGripperBody;
  private: Body *rightGripperBody;
  
  private: Geom *fixedBarGeom;
  private: Geom *baseBarGeom;
  private: Geom *leftGripGeom;
  private: Geom *rightGripGeom;
  private: Geom *leftPressureSensorGeom;
  private: Geom *rightPressureSensorGeom;

  private: RayProximity *innerBreakbeam;
  private: RayProximity *outerBreakbeam;

  private: SliderJoint *leftGripperJoint;
  private: SliderJoint *rightGripperJoint;
  private: SliderJoint *baseJoint;
  
  private: dJointGroupID contactGroupID;
  private: bool contactFound;
  
  private: dSpaceID leftGripSpaceId;
  private: dSpaceID rightGripSpaceId;
  private: dJointID fjoint;
  
  // External interface
  private: gz_gripper_t *gripper_iface;

  // World models that can be gripped
  private: Model **grippableModels;
  private: int grippableModelsMaxCount;
  private: int grippableModelsCount;

  private: double gripped_local[3];
  private: double gripped_world[3];

  private: void set_inner_beam (double height);
  private: void set_outer_beam (double height);
  private: double gripper_init_height;
};

#endif