//---------------------------
// UCL-CEREM-MBS
//
// @version MBsysLab_s 1.7.a
//
// Creation : 2006
// Last update : 01/10/2008
//
//
//---------------------------

#include "math.h"
#include "mbs_tyre.h"
#include "mbs_data.h"
#include "user_model.h"
#include "user_all_id.h"
#include "mbs_data.h"
#include "mbs_project_interface.h"

double* user_ExtForces(double PxF[4], double RxF[4][4],
                       double VxF[4], double OMxF[4],
                       double AxF[4], double OMPxF[4],
                       MbsData *mbs_data, double tsim,int ixF)
{
    double Fx=0.0, Fy=0.0, Fz=0.0;
    double Mx=0.0, My=0.0, Mz=0.0;
    double dxF[4] ={0.0, 0.0, 0.0, 0.0};


    double *SWr = mbs_data->SWr[ixF];

    // default application point of the force: anchor point to which it is attached
    int idpt = 0;
    idpt = mbs_data->xfidpt[ixF];
    dxF[1] = mbs_data->dpt[1][idpt];
    dxF[2] = mbs_data->dpt[2][idpt];
    dxF[3] = mbs_data->dpt[3][idpt];

/* Begin of user declaration */

	UserModel *um = mbs_data->user_model;
	double K = um->Pneu.K;
	double D = um->Pneu.D;
	//double sinus = RxF[2][3];
	double R0 = um->Pneu.R0;
	double rzp, anglisp, angcamb, glissp, Vct[4], Vws[4], Rtsol[4][4];
	double M[4];
	rzp = 0;
	anglisp = 0;
	angcamb = 0;
	glissp = 0;
	double PressionIntPneu = 9e5; // Fiche technique pneu il doit etre gonflé à 9 bar
	int is_left_tire = 1; // 1 si c'est un pneu gauche et 0 si c'est un pneu droite

/* End of user declaration */

    switch(ixF){

/* Begin of user code */


	double F[4] = { 3.0,Fx,Fy,Fz };

	case Force_contactD1_id:
		is_left_tire = 0;
		mbs_kine_wheel(PxF, RxF, VxF, OMxF, tsim, Force_contactD1_id, &rzp, &anglisp, &angcamb, &glissp, Vct, Vws, Rtsol, dxF);
		if (PxF[3] < R0) {
			Fz = K * (R0 - PxF[3]) - D * VxF[3];
		}
		F[3] = Fz;
		mbs_pacejka_6_1_contact(F, M, anglisp, angcamb, glissp, VxF[1], PressionIntPneu, is_left_tire);
		Fx = F[1];
		Fy = F[2];
		Fz = F[3];

		break;

	case Force_contactD2_id:
		is_left_tire = 0;
		mbs_kine_wheel(PxF, RxF, VxF, OMxF, tsim, Force_contactD2_id, &rzp, &anglisp, &angcamb, &glissp, Vct, Vws, Rtsol, dxF);
		if (PxF[3] < R0) {
			Fz = K * (R0 - PxF[3]) - D * VxF[3];
		}
		F[3] = Fz;
		mbs_pacejka_6_1_contact(F, M, anglisp, angcamb, glissp, VxF[1], PressionIntPneu, is_left_tire);
		Fx = F[1];
		Fy = F[2];
		Fz = F[3];
		break;

	case Force_contactD3_id:
		is_left_tire = 0;
		mbs_kine_wheel(PxF, RxF, VxF, OMxF, tsim, Force_contactD3_id, &rzp, &anglisp, &angcamb, &glissp, Vct, Vws, Rtsol, dxF);
		if (PxF[3] < R0) {
			Fz = K * (R0 - PxF[3]) - D * VxF[3];
		}
		F[3] = Fz;
		mbs_pacejka_6_1_contact(F, M, anglisp, angcamb, glissp, VxF[1], PressionIntPneu, is_left_tire);
		Fx = F[1];
		Fy = F[2];
		Fz = F[3];

		break;

	case Force_contactG1_id:
		is_left_tire = 1;
		mbs_kine_wheel(PxF, RxF, VxF, OMxF, tsim, Force_contactG1_id, &rzp, &anglisp, &angcamb, &glissp, Vct, Vws, Rtsol, dxF);
		if (PxF[3] < R0) {
			Fz = K * (R0 - PxF[3]) - D * VxF[3];
		}
		F[3] = Fz;
		mbs_pacejka_6_1_contact(F, M, anglisp, angcamb, glissp, VxF[1], PressionIntPneu, is_left_tire);
		Fx = F[1];
		Fy = F[2];
		Fz = F[3];

		break;

	case Force_contactG2_id:
		is_left_tire = 1;
		mbs_kine_wheel(PxF, RxF, VxF, OMxF, tsim, Force_contactG2_id, &rzp, &anglisp, &angcamb, &glissp, Vct, Vws, Rtsol, dxF);
		if (PxF[3] < R0) {
			Fz = K * (R0 - PxF[3]) - D * VxF[3];
		}
		F[3] = Fz;
		mbs_pacejka_6_1_contact(F, M, anglisp, angcamb, glissp, VxF[1], PressionIntPneu, is_left_tire);
		Fx = F[1];
		Fy = F[2];
		Fz = F[3];

		break;

	case Force_contactG3_id:
		is_left_tire = 1;
		mbs_kine_wheel(PxF, RxF, VxF, OMxF, tsim, Force_contactG3_id, &rzp, &anglisp, &angcamb, &glissp, Vct, Vws, Rtsol, dxF);
		if (PxF[3] < R0) {
			Fz = K * (R0 - PxF[3]) - D * VxF[3];
		}
		F[3] = Fz;
		mbs_pacejka_6_1_contact(F, M, anglisp, angcamb, glissp, VxF[1], PressionIntPneu, is_left_tire);
		Fx = F[1];
		Fy = F[2];
		Fz = F[3];

		break;



    /*  case 2:


        break;
    */

/* End of user code */

    }

    SWr[1]=Fx;
    SWr[2]=Fy;
    SWr[3]=Fz;
    SWr[4]=Mx;
    SWr[5]=My;
    SWr[6]=Mz;
    SWr[7]=dxF[1];
    SWr[8]=dxF[2];
    SWr[9]=dxF[3];

    return SWr;
}