#include "stdafx.h"

#include "DllFunc.h"

#include <stdio.h>

FILE* POSwrite;

double pretime;

RecurDynV8R3_UserSubRoutineWizard7_API void __cdecl modal_force_ext

           (int id, double time, double upar[], int npar, int ifbody, int nodarr[], int nonde, int jflag, int iflag, double result[])

{

           using namespace rd_syscall;

           // Parameter Information

           //   id     : Modal force sequential identification. (Input)

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

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

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

           //   ifbody : RFLEX Body sequential ID. (Input)

           //   nodarr : Node ID array of input node set. (Input)

           //   nonde  : Number of node of node set. (Input)

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

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

           //   result : Returned nodal force vector. Acting point of the nodal force is each center of each node.

           //            Reference frame of each force vector must be the ground inertia marker. (Output, Size: nonde * 6)

           // User Statement

   int errflg =0 , ifinish = 0;

   double pos[12];

   if (iflag)

   {

      POSwrite=fopen("RFlexBodyPos_C.txt","w");

      fprintf(POSwrite,"RFlex Body Position \n");

   }

   if(!jflag && POSwrite != NULL && pretime !=time)

   {

      for (int i = 0; i<12 ;i++)

      {

         pos[i]=0.0;

      }

      get_rflex_pos(ifbody,pos,&errflg);

      fprintf(POSwrite,"TIME = %20.10e \n",time);

      fprintf(POSwrite,"POSITION \n");

      fprintf(POSwrite,"X = %20.10e\n",pos[0]);

      fprintf(POSwrite,"Y = %20.10e\n",pos[1]);

      fprintf(POSwrite,"Z = %20.10e\n",pos[2]);

      fprintf(POSwrite,"Orientation Matrix \n");

      fprintf(POSwrite,"%20.10e  %20.10e  %20.10e\n",pos[3],pos[6],pos[9]);

      fprintf(POSwrite,"%20.10e  %20.10e  %20.10e\n",pos[4],pos[7],pos[10]);

      fprintf(POSwrite,"%20.10e  %20.10e  %20.10e\n",pos[5],pos[8],pos[11]);

   }

   getfinishflag(&ifinish);

   if(ifinish)

   {

      fclose(POSwrite);

   }

   for(int i=0;i<6*nonde;i++)

   {

      result[i] = 0.0;

   }

   pretime = time;

}