/*
* 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.
*
*/

#include "fri_okc_types.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>

void
okc_coords_set_all_zero (coords_t* k)
{
  k->x = k->y = k->z = k->a = k->b = k->c = 0.0;
}

void
okc_axis_set_all_zero (lbr_axis_t* l)
{
  l->a1 = l->a2 = l->a3 = l->a4 = l->a5 = l->a6 = l->e1 = 0.0;
}

void
okc_copy_axis (const lbr_axis_t src, lbr_axis_t* dest)
{
  memcpy (dest,&src,sizeof (lbr_axis_t));
}

void
okc_copy_coords (const coords_t src, coords_t* dest)
{
  memcpy (dest,&src,sizeof (coords_t));
}

void 
okc_print_coords (coords_t k)
{
  printf ("x = %3.3f\ty = %3.3f\t z = %3.3f\n",k.x,k.y,k.z);
  printf ("a = %3.3f\tb = %3.3f\t c = %3.3f\n",k.a,k.b,k.c);
}


void
okc_print_axis (lbr_axis_t l)
{
  printf ("A1 = %3.3f\t A2 = %3.3f\t A3 = %3.3f\t A4 = %3.3f\n",l.a1, l.a2,
	  l.a3, l.a4);
  printf ("A5 = %3.3f\t A6 = %3.3f\t E1 = %3.3f\n",l.a5, l.a6, l.e1);
}

void
okc_print_lbr_mnj (fri_float_t* l)
{
  printf ("A1 = %f, A2 = %f, E1 = %f, A3 = %f\nA4 = %f, A5 = %f, A6 = %f\n",
	  l[0],l[1],l[2],l[3],l[4],l[5],l[6]);
}



okc_handle_t*
okc_malloc_init_okc_handle_t (void) 
{
  okc_handle_t* okch;
  int i;

  okch = malloc (sizeof (okc_handle_t));
  if (NULL == okch)
    {
      perror ("okc_malloc_init_okc_handle_t: malloc");
      return  (NULL);
    }

  okch->flags = 0;
  okch->listener = 0;
  okch->mode = -1;

  
  for (i=0; i < OKC_MAX_ROBOTS; i++)
    {
      okch->robot_map[i] = OKC_SLOT_UNASSIGNED;
      okch->robot_names[i] = NULL;
      okch->axis_set_abs_cb[i] = NULL;
      okch->cartpos_set_abs_cb[i] = NULL;
      okch->cartpos_axis_set_abs_cb[i] = NULL;
      okch->cartpos_axis_impedance_set_abs_cb[i] = NULL;
      okch->private_to_axis_set_abs_cb[i] = NULL;
      okch->private_to_cartpos_set_abs_cb[i] = NULL;
      okch->private_to_cartpos_axis_set_abs_cb[i] = NULL;
      okch->private_to_cartpos_axis_impedance_set_abs_cb[i] = NULL;
    }

  for (i=0; i < OKC_MAX_ROBOTS; i++)
    {
      if (0 != pthread_rwlock_init (&okch->pos_axis_in_lock[i],NULL))
	{
	  perror ("okc_malloc_init_okc_handle_t: pthread_rwlock_init");
	  free (okch);
	  return (NULL);
	}
      if (0 != pthread_rwlock_init (&okch->rist_lbra_out_lock[i],NULL))
	{
	  perror ("okc_malloc_init_okc_handle_t: pthread_rwlock_init");
	  free (okch);
	  return (NULL);
	}
      if (0 != pthread_rwlock_init (&okch->callback_lock[i],NULL))
	{
	  perror ("okc_malloc_init_okc_handle_t: pthread_rwlock_init");
	  free (okch);
	  return (NULL);
	}
    }

  if (0 != pthread_rwlock_init (okch->robot_map_names_lock, NULL))
    {
      perror ("okc_malloc_init_okc_handle_t: pthread_rwlock_init");
      free (okch);
      return (NULL);
    }
  
  okch->comm_thread = malloc (sizeof (pthread_t));

  if (NULL == okch->comm_thread)
    {
      perror ("okc_malloc_init_okc_handle_t: malloc");
      free (okch);
      return (NULL);
    }

  return okch;
}

okc_handle_t*
okc_destroy_okc_handle_t (okc_handle_t* okch)
{
  int i;

  for (i=0; i < OKC_MAX_ROBOTS; i++)
    {
      if (0 != pthread_rwlock_destroy (&okch->pos_axis_in_lock[i]))
	{
	  perror ("okc_destroy_okc_handle_t: pthread_rwlock_destroy");
	}
      if (0 != pthread_rwlock_destroy (&okch->rist_lbra_out_lock[i]))
	{
	  perror ("okc_destroy_okc_handle_t: pthread_rwlock_destroy");
	}
    }
  if (0 != pthread_rwlock_destroy (okch->robot_map_names_lock))
    {
      perror ("okc_destroy_okc_handle_t: pthread_rwlock_destroy");
    }

  free (okch->comm_thread);
  free (okch);
  
  return (NULL);
}

int
okc_is_coords_zero (coords_t k)
{
  if (fabs (k.x) > OKC_DBL_EPSILON)
    return 0;
  if (fabs (k.y) > OKC_DBL_EPSILON)
    return 0;
  if (fabs (k.z) > OKC_DBL_EPSILON)
    return 0;
  if (fabs (k.a) > OKC_DBL_EPSILON)
    return 0;
  if (fabs (k.b) > OKC_DBL_EPSILON)
    return 0;
  if (fabs (k.c) > OKC_DBL_EPSILON)
    return 0;
  return 1;
}

int
okc_is_axis_zero (lbr_axis_t l)
{
  if (fabsf (l.a1) > OKC_DBL_EPSILON)
    return 0;
  if (fabsf (l.e1) > OKC_DBL_EPSILON)
    return 0;
  if (fabsf (l.a2) > OKC_DBL_EPSILON)
    return 0;
  if (fabsf (l.a3) > OKC_DBL_EPSILON)
    return 0;
  if (fabsf (l.a4) > OKC_DBL_EPSILON)
    return 0;
  if (fabsf (l.a5) > OKC_DBL_EPSILON)
    return 0;
  if (fabsf (l.a6) > OKC_DBL_EPSILON)
    return 0;
  return 1;
}

void
okc_subtract_coords (coords_t* difference, coords_t minuend, coords_t subtrahend)
{
  difference->x = minuend.x - subtrahend.x;
  difference->y = minuend.y - subtrahend.y;
  difference->z = minuend.z - subtrahend.z;
  difference->a = minuend.a - subtrahend.a;
  difference->b = minuend.b - subtrahend.b;
  difference->c = minuend.c - subtrahend.c;
}

void
okc_subtract_axis (lbr_axis_t* difference, lbr_axis_t minuend, 
		   lbr_axis_t subtrahend)
{
  difference->a1 = minuend.a1 - subtrahend.a1;
  difference->a2 = minuend.a2 - subtrahend.a2;
  difference->e1 = minuend.e1 - subtrahend.e1;
  difference->a3 = minuend.a3 - subtrahend.a3;
  difference->a4 = minuend.a4 - subtrahend.a4;
  difference->a5 = minuend.a5 - subtrahend.a5;
  difference->a6 = minuend.a6 - subtrahend.a6;
}


static void
_okc_print_head (tFriHeader h)
{
  printf ("\tPackets Send: %u Packets Received: %u\n",h.sendSeqCount,
	  h.reflSeqCount);
}

static
const char* 
_okc_quality_to_str (fri_uint16_t q)
{
  switch (q)
    {
    case FRI_QUALITY_UNACCEPTABLE: return "unacceptable";
    case FRI_QUALITY_BAD: return "bad";
    case FRI_QUALITY_OK: return "ok";
    case FRI_QUALITY_PERFECT: return "perfect";
    default: 
      return "unknown value";
    }
  return (NULL);
}

static void
_okc_print_state (tFriIntfState s)
{
  printf ("\tAvg. Answer Rate: %.2f, Latency %.5f (± %.5f)\n",
	  s.stat.answerRate, s.stat.latency, s.stat.jitter);
  printf ("\tMissRate: %.4f, Missed Packet Counter: %u\n",
	  s.stat.missRate, s.stat.missCounter);
  printf ("\tRobot Timestamp: %f\n", s.timestamp);
  printf ("\tState = %s, Quality = %s\n",(s.state == FRI_STATE_MON)?"monitor":
	  (s.state == FRI_STATE_CMD)?"command":"unknown mode",
	  _okc_quality_to_str (s.quality));
  printf ("\tDesired Sample Time: %f\n", s.desiredMsrSampleTime);
  printf ("\tDesired Command Sample Time: %f\n",s.desiredCmdSampleTime);
  printf ("\tSafty Limits: %f\n",s.safetyLimits);
}

static const char*
_okc_control_to_str (fri_uint16_t c)
{
  switch (c)
    {
    case FRI_CTRL_POSITION: return "position control";
    case FRI_CTRL_CART_IMP: return "cartesian impedance control";
    case FRI_CTRL_JNT_IMP: return "joint impedance control";
    case FRI_CTRL_OTHER: return "'other'";
    }
  return "unknown";
}

static void
_okc_print_robotstate (tFriRobotState r)
{
  printf ("\tpower (undocumented bitfield) = 0x%X\n",r.power);
  printf ("\tcontrol strategy = %s\n",_okc_control_to_str (r.control));
  printf ("\terror (undocumented) = %u\n",r.error);
  printf ("\twarning (undocumented) = %u\n",r.warning);
  printf ("\tjoint temperatures = (%2.2f,%2.2f,%2.2f,%2.2f,%2.2f,%2.2f,%2.2f)\n",
	  r.temperature[0],r.temperature[1],r.temperature[2],r.temperature[3],
	  r.temperature[4],r.temperature[5],r.temperature[6]);
}

static void
_okc_print_float_array (fri_float_t* f, int dim)
{
  assert (dim > 0);
  int j;
  printf ("(%2.2f", f[0]);
  for (j = 1;j < dim;j++)
    {
      printf (", %2.2f", f[j]);
    }
  printf(")\n");
}

static void
_okc_print_robot_data (tFriRobotData r)
{
  int i,j;
  printf ("Measured Joint Position (rad) = ");
  _okc_print_float_array (r.msrJntPos,LBR_MNJ);
  printf ("Measured Cart Position (m) = ");
  _okc_print_float_array (r.msrCartPos, FRI_CART_FRM_DIM);
  printf ("Commanded Joint Position (rad) = ");
  _okc_print_float_array (r.cmdJntPos,LBR_MNJ);
  printf ("Commanded Cart Position (m) = ");
  _okc_print_float_array (r.cmdCartPos, FRI_CART_FRM_DIM);
  printf ("Commanded Joint Offset (rad) = ");
  _okc_print_float_array (r.cmdJntPosFriOffset,LBR_MNJ);
  printf ("Measured Joint Torques (Nm) = ");
  _okc_print_float_array (r.msrJntTrq, LBR_MNJ);
  printf ("Estimated External Joint Torques (Nm) = ");
  _okc_print_float_array (r.estExtJntTrq, LBR_MNJ);
  printf ("Estimated Force/Torque TCP (N/NM) = ");
  _okc_print_float_array (r.estExtTcpFT,FRI_CART_FRM_DIM);
  
  printf ("\tJacobian:\n");
  for (i = 0; i < FRI_CART_VEC; i++)
    {
      printf ("\t\t");
      for (j = 0; j < LBR_MNJ; j++) 
	{
	  printf ("%2.2f ",r.jacobian[i*LBR_MNJ+j]);
	}
      printf ("\n");
    }
  printf ("\tMass Matrix:\n");
  for (i = 0; i < LBR_MNJ; i++)
    {
      printf ("\t\t");
      for (j = 0; j < LBR_MNJ; j++) 
	{
	  printf ("%2.2f ",r.massMatrix[i*LBR_MNJ+j]);
	}
      printf ("\n");
    }

  printf ("\tGravity = ");
  _okc_print_float_array (r.gravity, LBR_MNJ);
}

static void
_okc_print_krl_data (tFriKrlData k)
{
  int i;
  
  printf ("\tKRL DATA:\tfloat\tint\tbool\n");
  for (i = 0;i < FRI_USER_SIZE; i++)
    {
      printf ("\t\t%2.3f\t%d\t%d\n",
	      k.realData[i],k.intData[i],
	      (k.boolData&(1<<i))?1:0);
    }
}
	  
  
void
okc_print_robot_informations (okc_handle_t* okc, int robot_id)
{
  tFriHeader head;
  tFriKrlData krl;
  tFriIntfState con_state;
  tFriRobotState rstate;
  tFriRobotData rdata;

  if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id]))
    {
      perror ("okc_print_robot_informations: pthread_rwlock_rdlock");
      exit (EXIT_FAILURE);
    }

  rstate = okc->state[robot_id]; 
  rdata = okc->robot_data[robot_id];
  krl = okc->krl_from_robot[robot_id];
  con_state = okc->connection_stats[robot_id];
  head = okc->message_header[robot_id];

  if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id]))
    {
      perror ("okc_print_robot_informations: pthread_rwlock_unlock");
      exit (EXIT_FAILURE);
    }

  printf ("Robot Informations for Robot %d:\n",robot_id);
  _okc_print_head (head);
  _okc_print_state (con_state);
  _okc_print_robotstate (rstate);
  _okc_print_robot_data (rdata);
  _okc_print_krl_data (krl);

}

static int
_okc_check_range (fri_float_t d, fri_float_t min, fri_float_t max)
{
  if ((d < min) || (d > max))
    return 1;
  return 0;
}
  

int
okc_check_axis_stiffness_value_range (lbr_axis_t stiff)
{
  if (1 == 
      _okc_check_range (stiff.a1,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.a2,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.e1,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.a3,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.a4,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.a5,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.a6,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX))
    return (OKC_EINVAL);
  return (OKC_OK);
}

int
okc_check_axis_damping_value_range (lbr_axis_t damp)
{
  if (1 == 
      _okc_check_range (damp.a1,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.a2,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.e1,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.a3,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.a4,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.a5,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.a6,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX))
    return (OKC_EINVAL);
  return (OKC_OK);
}

int
okc_check_cp_stiffness_value_range (coords_t stiff)
{
  if (1 == 
      _okc_check_range (stiff.x,OKC_CART_XYZ_STIFFNESS_MIN,
			OKC_CART_XYZ_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.y,OKC_CART_XYZ_STIFFNESS_MIN,
			OKC_CART_XYZ_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.z,OKC_CART_XYZ_STIFFNESS_MIN,
			OKC_CART_XYZ_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.a,OKC_CART_ABC_STIFFNESS_MIN,
			OKC_CART_ABC_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.b,OKC_CART_ABC_STIFFNESS_MIN,
			OKC_CART_ABC_STIFFNESS_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (stiff.c,OKC_CART_ABC_STIFFNESS_MIN,
			OKC_CART_ABC_STIFFNESS_MAX))
    return (OKC_EINVAL);

  return (OKC_OK);
}


int
okc_check_cp_damping_value_range (coords_t damp)
{
  if (1 == 
      _okc_check_range (damp.x,OKC_CART_XYZ_DAMPING_MIN,
			OKC_CART_XYZ_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.y,OKC_CART_XYZ_DAMPING_MIN,
			OKC_CART_XYZ_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.z,OKC_CART_XYZ_DAMPING_MIN,
			OKC_CART_XYZ_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.a,OKC_CART_ABC_DAMPING_MIN,
			OKC_CART_ABC_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.b,OKC_CART_ABC_DAMPING_MIN,
			OKC_CART_ABC_DAMPING_MAX))
    return (OKC_EINVAL);
  if (1 == 
      _okc_check_range (damp.c,OKC_CART_ABC_DAMPING_MIN,
			OKC_CART_ABC_DAMPING_MAX))
    return (OKC_EINVAL);

  return (OKC_OK);
}

int
okc_sleep_cycletime (okc_handle_t* okc, int robot_id)
{
  struct timespec time_val;
  struct timespec time_left;

  time_val.tv_sec = 0;
  
  if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id]))
    {
      perror ("okc_sleep_cycletime: pthread_rwlock_rdlock");
      return (OKC_ERROR);
    }

  time_val.tv_nsec = round (okc->connection_stats[0].desiredMsrSampleTime*
			    1000000000.0);
  
  if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id]))
    {
      perror ("okc_sleep_cycletime: pthread_rwlock_unlock");
      return (OKC_ERROR);
    }

  
  while (0 != nanosleep (&time_val,&time_left))
    {
      time_val = time_left;
    }
  return (OKC_OK);
}

void
okc_print_cart_frm_dim (fri_float_t* vec)
{
  printf ("%f\t%f\t%f\t\t%f\n",vec[0],vec[1],vec[2],vec[3]);
  printf ("%f\t%f\t%f\t\t%f\n",vec[4],vec[5],vec[6],vec[7]);
  printf ("%f\t%f\t%f\t\t%f\n",vec[8],vec[9],vec[10],vec[11]);
}

void
okc_print_transform (transform_t t)
{
  printf ("%f\t%f\t%f\t\t%f\n",t.rxx, t.rxy, t.rxz, t.tx);
  printf ("%f\t%f\t%f\t\t%f\n",t.ryx, t.ryy, t.ryz, t.ty);
  printf ("%f\t%f\t%f\t\t%f\n",t.rzx, t.rzy, t.rzz, t.tz);
}

void
okc_cp_cart_frm_dim (const fri_float_t* source, fri_float_t* target)
{
  memcpy (target,source,sizeof (fri_float_t)*FRI_CART_FRM_DIM);
}

void
okc_cp_lbr_mnj (const fri_float_t* source, fri_float_t* target)
{
  memcpy (target,source,sizeof (fri_float_t)*LBR_MNJ);
}

void
okc_cp_cart_vec (const fri_float_t* source, fri_float_t* target)
{
  memcpy (target,source,sizeof (fri_float_t)*FRI_CART_VEC);
}

void
okc_set_to_cp_stiff_damp_default (fri_float_t* cp_stiff,
				  fri_float_t* cp_damp)
{
  int i;
  for (i = 0; i < FRI_CART_VEC; i++)
    {
      if (i < 3)
	{
	  cp_stiff[i] = OKC_CART_XYZ_STIFFNESS_DEFAULT;
	  cp_damp[i] = OKC_CART_XYZ_DAMPING_DEFAULT;
	}
      else
	{
	  cp_stiff[i] = OKC_CART_ABC_STIFFNESS_DEFAULT;
	  cp_damp[i] = OKC_CART_ABC_DAMPING_DEFAULT;
	}
    }
}