/***************************************************************************
 *                      ArmRobotSimulation.h							   *
 *                       -------------------	                           *
 * copyright         : (C) 2013 by Richard R. Carrillo and Niceto R. Luque *
 * email             : rcarrillo, nluque at ugr.es                         *
 ***************************************************************************/

/***************************************************************************
 *   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 3 of the License, or     *
 *   (at your option) any later version.                                   *
 ***************************************************************************/

 /*!
 * \file ArmRobotSimulation.h
 *
 * \author Richard R. Carrillo
 * \author Niceto R. Luque
 * \date 19 of September 2013
 *
 * This file declares the functions to simulate the robot's dynamics.
 */

#ifndef _ACCEL_H_
#define _ACCEL_H_

/*!
 * \file accel.h
 *
 * \author Niceto R. Luque
 * \author Richard R. Carrido
 * \date 28 June 2013
 *
 * This file declares all needed functions to calculate the inverse robot dynamics.
 */

#ifndef EXTERN_C
#  ifdef __cplusplus
#    define EXTERN_C extern "C"
#  else
#    define EXTERN_C extern
#  endif
#endif

#include "mex.h"
#include "RobotRecursiveInverseDynamics.h"

/***************************************************************************
 *                                                                         *
 *				DEFINES AND ENUM FOR MANAGING THE ROBOT					   *
 *																		   *		
 *                                                                         *
 ***************************************************************************/

/* Input Arguments */
#define	ROBOT_IN	prhs[0]
#define	A1_IN		prhs[1]
#define	A2_IN		prhs[2]
#define	A3_IN		prhs[3]
#define	A4_IN		prhs[4]
#define	A5_IN		prhs[5]

/* Output Arguments */
#define	TAU_OUT	plhs[0]

/* Some useful things */
#define	NUMROWS(x)	mxGetM(x)
#define	NUMCOLS(x)	mxGetN(x)
#define	NUMELS(x)	(mxGetN(x)*mxGetM(x))
#define	POINTER(x)	mxGetPr(x)

#define INTEGR_BUFFERSIZE 7

/*
 * enums for all the fields we want to pull out of the Matlab robot object
 */
enum {
	FLD_ALPHA,
	FLD_A,
	FLD_THETA,
	FLD_D,
	FLD_SIGMA,
	FLD_OFFSET,
	FLD_M,
	FLD_R,
	FLD_I,
	FLD_JM,
	FLD_G,
	FLD_B,
	FLD_TC,
	FLD_MAX
};
/*
 * struct for all the activity buffers we need to properly integrate acceleration velocity and position.
 */
struct integrator_buffers
  {
   double *velbuffer, *accbuffer;
   double *sumvel, *sumacc;
   double *qinit, *qvinit;
   int occupation;
  };

/* forward defines robot */
static void rot_mat(Link *l, double th, double d, DHType type);
static int mstruct_getfield_number(mxArray *m, char *field);
static int mstruct_getint(mxArray *m, int i, char *field);
static double mstruct_getreal(mxArray *m, int i, char *field);
static double * mstruct_getrealvect(mxArray *m, int i, char *field);
void error(char *s, ...);


/***************************************************************************
 *                                                                         *
 *				FUNTIONS FOR  INTEGRATING ACCELERATION					   *
 *				IN ORDER TO OBTAIN VELOCITY AND POSITION				   *		
 *                                                                         *
 ***************************************************************************/

/*!
 *\brief PURPOSE: Inverse Matrix for calculating the acceleration arguments and sends resultant string to main via and single vector. 
 *
 * Inverting two Matrices needed for obtaining acceleration value of the robot plan when a torque value is applied
 *\param sizeMatrix      size of M1 must be squared
 *\param  M1             PointerEntry Matrix to be multiplied
 *\param  INVM1          Pointer to the result Matrix
 *      
*/
void invermat(int sizeMatrix, double *a, double *ainv);


/* Matrix multiplication functions*/
/*!
 *\brief PURPOSE: Multiplying two Matrices needed for obtaining acceleration value of the robot plan when a torque value is applied
 *\param  M1             Entry Matrix to be multiplied
 *\param  M2             Entry Matrix to be multiplied
 *\param  Result         Pointer to the result Matrix
 *\param  oneRow         number of rows M1
 *\param  oneCol         number of cols M1
 *\param  twoRow         number of rows M2
 *\param  twoCol         number of cols M2
*
*/
void multiplyMatrices(double *M1, double *M2,double *Result,int oneRow, int oneCol, int twoRow, int twoCol);

/*! 
*
*\brief PURPOSE: Integration methods to calcule q and qd
* In order to compute the integral value of the given acceleration and velocity to solve the direct robot dinamic a buffer of 7 elemnts per joint is used to integrate
* the acceleration and velocity. Solving the direct dynamic
*\param		accbuffer     activity acc buffer
*\param		velbuffer     activity velocity buffer
*\param		occupation	  buffer occupation   
*\param		sumacc        accumulated integral
*\param		sumvel        accumulated integral      
*\param	    qddoutput_1	  qdd to be integrated
*\param	    qdoutput	  accumulated integral
*\param     qoutput       accumulated integral
*\param	    qvinit        initial conditions velocity
*\param		qinit         initial conditions possition
*\param	    njoints		  number of robot joints
*\param 	stepsize	  integration step		
*        
*/
void integrationprocedure(double *accbuffer, double *velbuffer,int *occupation,double *sumacc, double *sumvel, double *qddoutput_1 ,double *qdoutput,double *qoutput,double *qvinit, double *qinit,int njoints,double stepsize);

/*!
*\brief Allocates the memory used in the integrative process.
* It allocates the memory of several buffers: sumvel,sumacc,accbuffer,velbuffer,qinit and qvinit.
* For instance, in order to compute the integral value of the given acceleration and velocity to calculate the direct robot 
* dynamics a buffer of 5 elemnts per joint is needed
*\param integr_buffers Pointer to a integrator_buffers structure. This struct will contain the buffer pointers.
*\param njoints Number of robot joints.
*\return The occurred error (0 if the function succeeded).
*
*/
EXTERN_C int allocate_integration_buffers(struct integrator_buffers *integr_buffers, int njoints);

/*!
*
*\brief Frees the memory allocated by allocate_integration_buffers() and used in the integrative process
*
*\param integr_buffers Pointer to a integrator_buffers structure.
*
*/
EXTERN_C void free_integration_buffers(struct integrator_buffers *integr_buffers);

/*!
*
*\brief Initializes the buffers used in the integrative process.
*
*\param entry Pointer to an array containing the initial position, velocity and acceleration per joint (entry values to be accumulated)
*\param integr_buffers Pointer to a integrator_buffers structure to be initialized. This struct must contain allocated buffers.
*\param njoints Number of robot joints.
*
*/
EXTERN_C void initialize_integration_buffers(double *entry, struct integrator_buffers *integr_buffers, int njoints);

/*!
*\brief PURPOSE: approximate the value of a definite integral using boole's rule
*       
*\param   f           buffer containing function points
*\param   h           h(upper limit of integration step-lower limit of integration)        
*\return  y			  approximate value of definite integral
*/
double boolerule ( double *f, double h);

/*!
*\brief PURPOSE: approximate the value of a definite integral using  simsomp's 3/8 rule
*       
*\param   f           buffer containing function points
*\param   h           h(upper limit of integration step-lower limit of integration)        
*\return  y			  approximate value of definite integral
*/
double simp3_8 ( double *f, double h );

/*!
*\brief PURPOSE: approximate the value of a definite integral using  simsompīs rule
*       
*\param   f           buffer containing function points
*\param   h           h(upper limit of integration step-lower limit of integration)        
*\return  y			  approximate value of definite integral
*/
double simp ( double *f, double h );

/*!
*\brief PURPOSE: approximate the value of a definite integral using the composite trapezoidal rule
*       
*\param   f           buffer containing function points
*\param   h           h(upper limit of integration step-lower limit of integration)        
*\return  y			  approximate value of definite integral
*/
double trap ( double *f, double h );

/*!
*\brief PURPOSE: approximate the value of a definite integral using a fifth order rule
*       
*\param   f           buffer containing function points
*\param   h           h(upper limit of integration step-lower limit of integration)        
*\return  y			  approximate value of definite integral
*/
double fifth ( double *f, double h );

/*!
*\brief PURPOSE: approximate the value of a definite integral using a six order rule
*       
*\param   f           buffer containing function points
*\param   h           h(upper limit of integration step-lower limit of integration)        
*\return  y			  approximate value of definite integral
*/
double sixth ( double *f, double h );


/***************************************************************************
 *                                                                         *
 *				FUNTIONS FOR THE DIREC & INVERSE DYNAMIC   				   *
 *																		   *		
 *                                                                         *
 ***************************************************************************/

/*
*\brief PURPOSE: The plhs[] and prhs[] parameters are vectors that contain pointers to each left-hand side (output) variable and each right-hand side (input) variable, respectively. 
*Accordingly, plhs[0] contains a pointer to the first left-hand side argument, plhs[1] contains a pointer 
*to the second left-hand side argument, and so on. 
*Likewise, prhs[0] contains a pointer to the first right-hand side argument, prhs[1] points to the second, and so on. 
*
*
*\param		nlhs		MATLAB sets nlhs with the number of expected mxArrays.
*\param		plhs		MATLAB sets plhs to a pointer to an array of NULL pointers.
*\param		nrhs		MATLAB sets nrhs to the number of input mxArrays.
*\param		prhs		MATLAB sets prhs to a pointer to an array of input mxArrays. 
*				
*/
void frnecFunction(int	nlhs, mxArray **plhs, int nrhs, const mxArray **prhs);

/*!
*\brief Calculates the robot's inverse dynamics
* This FrnecFuntion Computes inverse dynamics via recursive Newton-Euler formulation
* 
* 	TAU = RNE(ROBOT, Q, QD, QDD)
* 	TAU = RNE(ROBOT, [Q QD QDD])
* 
* 	Returns the joint torque required to achieve the specified joint position,
* 	velocity and acceleration state.
* 
* 	Gravity vector is an attribute of the robot object but this may be 
* 	overriden by providing a gravity acceleration	vector [gx gy gz].
* 
* 	TAU = RNE(ROBOT, Q, QD, QDD, GRAV)
* 	TAU = RNE(ROBOT, [Q QD QDD], GRAV)
* 
* 	An external force/moment acting on the end of the manipulator may also be
* 	specified by a 6-element vector [Fx Fy Fz Mx My Mz].
* 
* 	TAU = RNE(ROBOT, Q, QD, QDD, GRAV, FEXT)
* 	TAU = RNE(ROBOT, [Q QD QDD], GRAV, FEXT)
* 
*	where	Q, QD and QDD are row vectors of the manipulator state; pos, vel, and accel.
* 
*	The torque computed also contains a contribution due to armature
* 	inertia.
*
*	
*\param			 robot				robot variable to be used
*\param			 tray  				position velocity and acceleration values per link from the trajectory generator	
*\param 		 ExternalForce 		external applied Force per link
*\param 		 Gravity			gravity acceleration vector
*\param     	 TorqueOutput		Obtained torque per link
*
*/
EXTERN_C void compute_robot_inv_dynamics(mxArray *robot,double *tray,const double *ExternalForce,const double *Gravity,double *TorqueOutput);

/*!
*\brief PURPOSE:calculate robot's direct dynamics
*        
* Returns a vector of joint accelerations that result from applying the 
* actuator TORQUE to the manipulator ROBOT in state Q and QD.
*
* Uses the method 1 of Walker and Orin to compute the forward dynamics.
* The accelerations of the coordinates are obtained first 
* with the method of Walker-Orin and, later,it is joining to obtain speed and position.  
*
* This form is useful for simulation of manipulator dynamics, in
* conjunction with a numerical integration function.
*
* Walker and Orin is a numerical method used to obtain the acceleration of the
* articular coordinates from the torque vector.For it, Newton-Euler's
* algorithm uses when articular aceleration is zero
* B= 0+H(q,q')+C(q); tau=D(q)q''+B; q''=inv(D(q))[tau-B]
*
*
*    
*\param			  robot						robot variable to be used
*\param			  robot_initial_state		position velocity and acceleration  values per link previously obtained	 
*\param			  external_torque			external applied torque per link
*\param			  robot_resultant_state		Actual position velocity and acceleration  values per link obtained
*\param			  integr_buffers			Activity buffer
*\param			  Gravity					Gravity vector
*\param			  Stepsize					integration step size
*
*/
EXTERN_C void compute_robot_dir_dynamics(mxArray *robot, double *robot_initial_state, double *external_torque, double *robot_resultant_state, struct integrator_buffers *integr_buffers, const double *external_force, const double *gravity, double stepsize);


/***************************************************************************
 *                                                                         *
 *				FUNTIONS FOR CREATING THE TRAJECTORY					   *
 *				AND LOADING THE ROBOT   								   *
 *																		   *		
 *                                                                         *
 ***************************************************************************/

/*Trajectory generator*/

/*!
*\brief PURPOSE: This is for generating an eight like trajectory in cartesian space by means of using sinusoidal curves in joint space
*
*
*
*     
*\param			tsimul	simulation time
*\param         n_joint	number of links the robot has       
*\param     	q          position per link at t= tsimul
*\param 		qd			velocity per link at t=tsimul
*\param 		qdd		acceleration per link at t=tsimul
*
*/
EXTERN_C void trajectory(double *q, double *qd, double *qdd, double tsimul,int n_joints);

/*!
*\brief Loads the robot object from Matlab file
*  in order to dynamically create all the variables needed
*
*\param		  robotfile	   File name from where the robot object is loaded
*\param		  robotname    Robot's variable name in the file
*\param       robot        Pointer to a robot variable pointer which will be set to the loaded robot
*\param		  size         Pointer to the variable where the number of joints will be stored
*\return      occurred error (=0 if no error)
*
*/
EXTERN_C int load_robot(const char *robotfile, const char *robotname, int *size, mxArray **robot);

/*!
*\brief Frees the robot memory array
*
*\param   robot        Robot variable pointer in which the robot variable is allocated
*
*/
EXTERN_C void free_robot(mxArray *robot);


/***************************************************************************
 *                                                                         *
 *				FUNTIONS FOR TESTING DIREC & INVERSE DYNAMIC   			   *
 *																		   *		
 *                                                                         *
 ***************************************************************************/

/*!
*\brief  PURPOSE: This function is for debugging purposes. It simulates the robot during NUMSTEPS*STEPSIZE and creates tra.txt log file
*		  This log file can be plotted with the tra.m Matlab script
*
*/
EXTERN_C void test_simulated_robot_dynamics(void);

#endif /*_ACCEL_H_*/
