/*
* This file is part of OpenKC
*
* Copyright(c) Matthias Schöpfer (mschoepf@techfak.uni-bielefeld.de)
* http://opensource.cit-ec.de/projects/openkc
*
* This file may be licensed under the terms of of the
* GNU Lesser General Public License Version 3 (the ``LGPL''),
* or (at your option) any later version.
*
* Software distributed under the License is distributed
* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
* express or implied. See the LGPL for the specific language
* governing rights and limitations.
*
* You should have received a copy of the LGPL along with this
* program. If not, go to http://www.gnu.org/licenses/lgpl.html
* or write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The development of this software was supported by the
* Excellence Cluster EXC 277 Cognitive Interaction Technology.
* The Excellence Cluster EXC 277 is a grant of the Deutsche
* Forschungsgemeinschaft (DFG) in the context of the German
* Excellence Initiative.
*
*/

#ifndef _FRI_OKC_TYPES_H_
#define _FRI_OKC_TYPES_H_

#include <pthread.h>
#include <fricomm.h>

#ifdef __cplusplus
extern "C" {
#endif

/** smallest such that 1.0+DBL_EPSILON != 1.0 */
#define OKC_DBL_EPSILON    2.2204460492503131e-016 

/** Change number of Robots that can be controlled with one Openkc-Instance*/
#define OKC_MAX_ROBOTS 2

/** This is what we always hope for */
#define OKC_OK 0


/** Inapropriate use of function */
#define OKC_INAPPROPIRATE -7
/** Cannot execute command, because robot is not in command mode. */
#define OKC_NOT_IN_COMMAND_MODE -6
/** Drives are not powered. Command failed therefore. */
#define OKC_DRIVES_POWER_OFF -5
/** This function is deprecated. Do not use, is only a stub. */
#define OKC_DEPRECATED -4
/** Inapropriate/invalid Parameter */
#define OKC_EINVAL -3
/** Robot is unavailable (not connected) */
#define OKC_UNAVAILABLE -2
/** Some Error occured */
#define OKC_ERROR -1

/** True and False*/
#define OKC_TRUE 1
#define OKC_FALSE 0


/** Internal use */
#define OKC_SLOT_UNASSIGNED -1
#define OKC_SLOT_IN_USE 1

/** OKC Modes */
#define OKC_MODE_SETTER_AXIS 0x11
#define OKC_MODE_SETTER_POS 0x12
#define OKC_MODE_SETTER 0x10
#define OKC_MODE_CALLBACK 0x100
#define OKC_MODE_CALLBACK_AXIS_ABS 0x101
#define OKC_MODE_CALLBACK_POS_ABS 0x102
#define OKC_MODE_CALLBACK_POS_AXIS_ABS 0x104
#define OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS 0x108
/** End the okcfri controller programm for good */
#define OKC_COMMAND_KRL_END 99

#define OKC_CMD_FLAGS_POSITION_CONTROL_MODE FRI_CMD_JNTPOS
#define OKC_CMD_FLAGS_AXIS_IMPEDANCE_MODE FRI_CMD_JNTPOS | FRI_CMD_JNTTRQ | FRI_CMD_JNTSTIFF | FRI_CMD_JNTDAMP 
#define OKC_CMD_FLAGS_CP_IMPEDANCE_MODE FRI_CMD_CARTPOS | FRI_CMD_CARTSTIFF | FRI_CMD_CARTDAMP 
#define OKC_CMD_FLAGS_CP_AXIS_IMPEDANCE_MODE FRI_CMD_CARTPOS |  FRI_CMD_CARTSTIFF | FRI_CMD_CARTDAMP | FRI_CMD_TCPFT

#define OKC_FRI_PORT 49938


/** Default Axis Stiffness Setting */
#define OKC_AXIS_STIFFNESS_DEFAULT 2000.0
/** Minimum Axis Stiffness. Set to some safe value. Modify here*/
#define OKC_AXIS_STIFFNESS_MIN 10.0
/** Maximum Axis Stiffness. Set to some safe value. Modify here*/
#define OKC_AXIS_STIFFNESS_MAX 2000.0

/** Default Axis Damping Setting */
#define OKC_AXIS_DAMPING_DEFAULT 0.7
/** Minimum Axis Damping. Set to some safe value. Modify here*/
#define OKC_AXIS_DAMPING_MIN 0.1
/** Maximum Axis Damping. Set to some safe value. Modify here*/
#define OKC_AXIS_DAMPING_MAX 1.0

/** Default Cartesian Stiffness Setting (wrt XYZ (Translation)) */
#define OKC_CART_XYZ_STIFFNESS_DEFAULT 2000.0
/** Minimum Cartesian Stiffness Setting (wrt XYZ). Modify here */
#define OKC_CART_XYZ_STIFFNESS_MAX 3000.0
/** Maximum Cartesian Stiffness Setting (wrt XYZ). Modify here */
#define OKC_CART_XYZ_STIFFNESS_MIN -1.0

/** Default Cartesian Stiffness Setting (wrt ABC (Rotation)) */
#define OKC_CART_ABC_STIFFNESS_DEFAULT 200.0
/** Minimum Cartesian Stiffness Setting (wrt ABC). Modify here */
#define OKC_CART_ABC_STIFFNESS_MAX 300.0
/** Maximum Cartesian Stiffness Setting (wrt ABC). Modify here */
#define OKC_CART_ABC_STIFFNESS_MIN 10.0

/** Default Cartesian Damping Setting (wrt XYZ (Translation)) */
#define OKC_CART_XYZ_DAMPING_DEFAULT 0.7
/** Minimum Cartesian Damping Setting (wrt XYZ). Modify here */
#define OKC_CART_XYZ_DAMPING_MAX 1.0
/** Maximum Cartesian Damping Setting (wrt XYZ). Modify here */
#define OKC_CART_XYZ_DAMPING_MIN 0.1

/** Default Cartesian Damping Setting (wrt ABC (Rotation)) */
#define OKC_CART_ABC_DAMPING_DEFAULT 0.7
/** Minimum Cartesian Damping Setting (wrt ABC). Modify here */
#define OKC_CART_ABC_DAMPING_MAX 1.0
/** Maximum Cartesian Damping Setting (wrt ABC). Modify here */
#define OKC_CART_ABC_DAMPING_MIN 0.1

#define OKC_FRI_START 1
#define OKC_FRI_STOP 2
#define OKC_RESET_STATUS 3
/** command flag to set axis stiffness & damping */
#define OKC_SET_CP_STIFFNESS_DAMPING 4
#define OKC_SET_AXIS_STIFFNESS_DAMPING 5
#define OKC_SWITCH_CP_CONTROL 6
#define OKC_SWITCH_AXIS_CONTROL 7
#define OKC_SWITCH_GRAVCOMP 8
#define OKC_SWITCH_POSITION 9
#define OKC_MOVE_START_POSITION 10
#define OKC_RESET_COUNTER 11

#define OKC_ACK_FRI_START 100
#define OKC_ACK_FRI_STOP 101
#define OKC_ACK_SET_CP_STIFFNESS_DAMPING 102
#define OKC_ACK_SET_AXIS_STIFFNESS_DAMPING 103
#define OKC_ACK_SWITCH_CP_CONTROL 104
#define OKC_ACK_SWITCH_AXIS_CONTROL 105
#define OKC_ACK_SWITCH_GRAVCOMP 106
#define OKC_ACK_SWITCH_POSITION 107
#define OKC_ACK_MOVE_START_POSITION 108
#define OKC_ACK_RESET_COUNTER 111

#define OKC_COMMAND_PENDING 0x1000
#define OKC_COMMAND_MASK 0x0fff

#define OKC_ADD_AXIS_TORQUE_DEFAULT 0.0
#define OKC_ADD_TCP_FT_DEFAULT 0.0;

#define OKC_TICKS_REISSUE 50

typedef struct Coords
{
  fri_float_t x;
  fri_float_t y;
  fri_float_t z;
  fri_float_t a;
  fri_float_t b;
  fri_float_t c;
} coords_t;

typedef struct Lbr_Axis
{
  fri_float_t a1;
  fri_float_t a2;
  fri_float_t e1;
  fri_float_t a3;
  fri_float_t a4;
  fri_float_t a5;
  fri_float_t a6;
} lbr_axis_t;

typedef struct Transform_t
{
  fri_float_t rxx;
  fri_float_t rxy;
  fri_float_t rxz;
  fri_float_t tx;
  fri_float_t ryx;
  fri_float_t ryy;
  fri_float_t ryz;
  fri_float_t ty;
  fri_float_t rzx;
  fri_float_t rzy;
  fri_float_t rzz;
  fri_float_t tz;
} transform_t;


typedef int (*okc_callback_axis_t) (void*, const fri_float_t*, fri_float_t*);
typedef int (*okc_callback_cartpos_t) (void*, const fri_float_t*, fri_float_t*);
typedef int (*okc_callback_cartpos_axis_t) (void*, const fri_float_t*, 
					     const fri_float_t*, fri_float_t*,
					     fri_float_t*);
typedef int (*okc_callback_cartpos_axis_impedance_t) (void*, const fri_float_t*,
						      const fri_float_t*, 
						      fri_float_t*, 
						      fri_float_t*,
						      fri_float_t*, 
						      fri_float_t*);

						     



typedef struct Okc_Handle_T
{
  pthread_t* comm_thread;

  int flags;
  int listener;
  int mode;
  pthread_rwlock_t callback_lock[OKC_MAX_ROBOTS];
  okc_callback_axis_t axis_set_abs_cb[OKC_MAX_ROBOTS];
  okc_callback_cartpos_t cartpos_set_abs_cb[OKC_MAX_ROBOTS];
  okc_callback_cartpos_axis_t cartpos_axis_set_abs_cb[OKC_MAX_ROBOTS];
  okc_callback_cartpos_axis_impedance_t cartpos_axis_impedance_set_abs_cb[OKC_MAX_ROBOTS];
  void* private_to_axis_set_abs_cb[OKC_MAX_ROBOTS];
  void* private_to_cartpos_set_abs_cb[OKC_MAX_ROBOTS];
  void* private_to_cartpos_axis_set_abs_cb[OKC_MAX_ROBOTS];
  void* private_to_cartpos_axis_impedance_set_abs_cb[OKC_MAX_ROBOTS];
  pthread_rwlock_t robot_map_names_lock[OKC_MAX_ROBOTS];
  int robot_map[OKC_MAX_ROBOTS];
  unsigned long robot_ip_addr[OKC_MAX_ROBOTS];
  char* robot_names[OKC_MAX_ROBOTS];
  pthread_rwlock_t pos_axis_in_lock[OKC_MAX_ROBOTS];
  fri_float_t matrix_pos_correction[OKC_MAX_ROBOTS][FRI_CART_FRM_DIM];
  coords_t pos_correction[OKC_MAX_ROBOTS];
  lbr_axis_t axis_correction[OKC_MAX_ROBOTS];
  fri_float_t pos_abs_set[OKC_MAX_ROBOTS][FRI_CART_FRM_DIM];
  fri_float_t axis_abs_set[OKC_MAX_ROBOTS][LBR_MNJ];
  fri_float_t axis_stiffness[OKC_MAX_ROBOTS][LBR_MNJ];
  fri_float_t axis_damping[OKC_MAX_ROBOTS][LBR_MNJ];
  fri_float_t cart_stiffness[OKC_MAX_ROBOTS][FRI_CART_VEC];
  fri_float_t cart_damping[OKC_MAX_ROBOTS][FRI_CART_VEC];
  fri_float_t add_axis_torque[OKC_MAX_ROBOTS][LBR_MNJ];
  fri_float_t add_tcp_ft[OKC_MAX_ROBOTS][FRI_CART_VEC];
  int command[OKC_MAX_ROBOTS];
  unsigned int command_seq_count[OKC_MAX_ROBOTS];
  unsigned int command_ticks_count[OKC_MAX_ROBOTS];
  unsigned int seq_count[OKC_MAX_ROBOTS];
  int command_flags[OKC_MAX_ROBOTS];
  tFriKrlData krl_to_robot[OKC_MAX_ROBOTS];
  pthread_rwlock_t rist_lbra_out_lock[OKC_MAX_ROBOTS];
  tFriKrlData krl_from_robot[OKC_MAX_ROBOTS];
  long int update_ticks[OKC_MAX_ROBOTS];
  tFriHeader message_header[OKC_MAX_ROBOTS];
  tFriRobotState state[OKC_MAX_ROBOTS];
  tFriRobotData robot_data[OKC_MAX_ROBOTS];
  tFriIntfState connection_stats[OKC_MAX_ROBOTS];
} okc_handle_t;

#ifdef __cplusplus
}
#endif

#endif