#include "stdafx.h"

#include "DllFunc.h"

#include "stdio.h"

#include <math.h>

void asp(double s[], double A[], double sp[])

{

    s[0] = A[0]*sp[0]+A[3]*sp[1]+A[6]*sp[2];

    s[1] = A[1]*sp[0]+A[4]*sp[1]+A[7]*sp[2];

    s[2] = A[2]*sp[0]+A[5]*sp[1]+A[8]*sp[2];

}

NodalForce_API void __cdecl nodal_force

           (int id, double time, double upar[], int npar, double pos[12], double vel[6], int jflag, int iflag, double result[6])

{

           using namespace rd_syscall;

           // Parameter Information

           //   id     : Nodal Force sequential identification. (Input)

           //   time   : Simulation time of RD/Solver. (Input)

           //   upar   : Parameters defined by user. (Input)

           //   npar   : Number of user parameters. (Input)

           //   pos    : Nodal position w.r.t. Inertia REF. (Input)

           //   vel    : Nodal velocity w.r.t. Inertia REF. (Input)

           //   jflag  : When RD/Solver evaluates a Jacobian, the flag is true. (Input)

           //   iflag  : When RD/Solver initializes arraies, the flag is true. (Input)

           //   result : Returned nodal force vector. (Input, Size : 6)

           // User Statement

           int ActionMarker, BaseMarker;

           int ifTXActive,ifTYActive,ifTZActive;

           int ifRXActive,ifRYActive,ifRZActive;

           int nfid,ifbody,nodeseq,nodeid,errflg;

           int mkid[3];

           double djip[3],vjip[3];

           double Phijip[3],wjip[3],fp[6];

           double A[9], ea313[3];

           double TK, TC, RK, RC;

           char msg[256];

           double PI;

    PI = acos(-1.0);

           if(npar<6) {

                     errmes(1000,"Number of the input parameter must be 6",id,"NodalForceUSUB");

                     return;

           }

           ActionMarker = (int)upar[0];

           BaseMarker = (int)upar[1];

           TK = upar[2];

           TC = upar[3];

           RK = upar[4];

           RC = upar[5];

           if (npar < 12) {

                     ifTXActive = 1;

                     ifTYActive = 1;

                     ifTZActive = 1;

                     ifRXActive = 1;

                     ifRYActive = 1;

                     ifRZActive = 0;

           }

           else {

                     ifTXActive = (int)upar[6];

                     ifTYActive = (int)upar[7];

                     ifTZActive = (int)upar[8];

                     ifRXActive = (int)upar[9];

                     ifRYActive = (int)upar[10];

                     ifRZActive = (int)upar[11];

           }

           nfid = 0;

           ifbody = 0;

           nodeseq = 0;

           nodeid = 0;

           errflg = 0;

           get_nforce_addarg(&nfid,&ifbody,&nodeseq,&nodeid,&errflg);

           if(iflag)

           {

                     sprintf(msg, "Nodal Force Seq = %d", nfid);

                     printmsg(msg, strlen(msg));

                     sprintf(msg, "FFlex Body Seq = %d", ifbody);

                     printmsg(msg,strlen(msg));

                     sprintf(msg, "Current Node Seq = %d", nodeseq);

                     printmsg(msg, strlen(msg));

                     sprintf(msg, "Current Node ID = %d", nodeid);

                     printmsg(msg,strlen(msg));

           }

           mkid[0] = ActionMarker;

           mkid[1] = BaseMarker;

           mkid[2] = ActionMarker;

           sysary("TDISP", mkid, 3, djip, 3, &errflg);

           sysary("TVEL", mkid, 3, vjip, 3, &errflg);

           sysary("RVEL", mkid, 3, wjip, 3, &errflg);

           mkid[0] = BaseMarker;

           mkid[1] = ActionMarker;

           sysfnc("ROLL", mkid, 2, &Phijip[0], &errflg);

           sysfnc("PITCH", mkid, 2, &Phijip[1], &errflg);

           sysfnc("YAW", mkid, 2, &Phijip[2], &errflg);

           Phijip[0]=Phijip[0]/180.0*PI;

           Phijip[1]=Phijip[1]/180.0*PI;

           Phijip[2]=Phijip[2]/180.0*PI;

           mkid[0] = ActionMarker;

           sysary("RDISP", mkid, 1, ea313, 3, &errflg);

           orientationmatrix(313, ea313, A, &errflg);

           for(int i = 0; i < 3; i++){

                     fp[i] = -djip[i]*TK-vjip[i]*TC;

                     fp[3+i] = Phijip[i]*RK-wjip[i]*RC;

    }

    if(ifTXActive == 0) fp[0] = 0.0;

    if(ifTYActive == 0) fp[1] = 0.0;

    if(ifTZActive == 0) fp[2] = 0.0;

    if(ifRXActive == 0) fp[3] = 0.0;

    if(ifRYActive == 0) fp[4] = 0.0;

    if(ifRZActive == 0) fp[5] = 0.0;

    asp(result, A, fp);

    asp(&result[3], A, &fp[3]);

}