#ifndef _ANYMOCAP_AUTOMATIC_INITIAL_POSITION_OF_BODY_ANY_

#define _ANYMOCAP_AUTOMATIC_INITIAL_POSITION_OF_BODY_ANY_

#class_template AutoPelvisPos(

RASIS=RASI,

LASIS=LASI, 

BACK=RPSI,

DIRECT_PELVIS_POS=0,

C3D_OBJECT=Main.ModelSetup.C3DFileData,

TSTART = Main.Studies.InverseDynamicStudy.tStart,

MANNEQUIN_FOLDER = Main.HumanModel.Mannequin

) {

    #if DIRECT_PELVIS_POS == 0

      #var AnyVar tStart = TSTART;      

      // Calculate the index in the C3D data which corresponds to the tStart  variable.

      AnyFloat tStartC3D = C3D_OBJECT.Header.FirstFrameNo/C3D_OBJECT.Header.VideoFrameRate;

      AnyFloat tEndC3D = (C3D_OBJECT.Header.LastFrameNo+1)/C3D_OBJECT.Header.VideoFrameRate;

      AnyInt nC3d = C3D_OBJECT.Header.LastFrameNo-C3D_OBJECT.Header.FirstFrameNo+1;

      AnyFloat closest_idx = (nC3d)/(tEndC3D-tStartC3D)*(tStart -tStartC3D);

      AnyInt start_idx = round( max({0.0,  min({closest_idx, nC3d } )}));

      // construct the position vector and rotation matrix from the values in the C3D file.

      #var AnyVec3 Pelvis_RASIS = C3D_OBJECT.Points.Markers.RASIS.Pos[start_idx];

      #var AnyVec3 Pelvis_LASIS = C3D_OBJECT.Points.Markers.LASIS.Pos[start_idx];

      #var AnyVec3 Pelvis_Back = C3D_OBJECT.Points.Markers.BACK.Pos[start_idx];

      #var AnyVec3 PelvisPos = 0.5*(Pelvis_RASIS + Pelvis_LASIS) - 0.02*PelvisRotMat'[0] - 0.01*PelvisRotMat'[1];

      #var AnyMat33 PelvisRotMat = RotMat(Pelvis_LASIS, Pelvis_RASIS, Pelvis_Back)

                              * RotMat(-pi/2,x)

                              * RotMat(pi/2,y)*RotMat(11*pi/180,z);

    #else

      #var AnyVec3 PelvisPos;

      #var AnyMat33 PelvisRotMat;

    #endif

    AnyFolder PelvisRotations = 

    {

       AnyVar yRot1 = asin(-.PelvisRotMat[2][0]);

       AnyVar yRot2 = pi - yRot1;

       AnyVar zRot1 = atan2(.PelvisRotMat[1][0]/cos(yRot1), .PelvisRotMat[0][0]/cos(yRot1));

       AnyVar zRot2 = atan2(.PelvisRotMat[1][0]/cos(yRot2), .PelvisRotMat[0][0]/cos(yRot2));

       AnyVar xRot1 = atan2(.PelvisRotMat[2][1]/cos(yRot1),.PelvisRotMat[2][2]/cos(yRot1));

       AnyVar xRot2 = atan2(.PelvisRotMat[2][1]/cos(yRot2),.PelvisRotMat[2][2]/cos(yRot2));

       AnyVar abs1 = abs(zRot1)+abs(yRot1)+abs(zRot1);

       AnyVar abs2 = abs(zRot2)+abs(yRot2)+abs(zRot2);

       AnyVar zRot = iffun(gtfun(abs1, abs2), zRot2, zRot1);

       AnyVar yRot = iffun(gtfun(abs1, abs2), yRot2, yRot1);

       AnyVar xRot = iffun(gtfun(abs1, abs2), xRot2, xRot1);

       // Check that the auto pos can reconstruct the rotation matrix.

       AnyFloat err = abs(.PelvisRotMat-RotMat(zRot,z)*RotMat(yRot,y)*RotMat(xRot,x));

       AnyInt AutoPosAssert = expect(ltfun(err,1e-5), "AutoPos failed to calculate the correct values");

     };

    AnyFolder &InitPosRef= MANNEQUIN_FOLDER.Posture;    

    InitPosRef= 

    {

      PelvisPosX = DesignVar(.PelvisPos[0]);

      PelvisPosY = DesignVar(.PelvisPos[1]);

      PelvisPosZ = DesignVar(.PelvisPos[2]);

      PelvisRotX = DesignVar(.PelvisRotations.xRot*180/pi);

      PelvisRotY = DesignVar(.PelvisRotations.yRot*180/pi);

      PelvisRotZ = DesignVar(.PelvisRotations.zRot*180/pi);

    };

};

#class_template AutomaticFullBodyInitialPosition (

MANNEQUIN_FOLDER = Main.HumanModel.Mannequin

) {

#var AnyVec3 RASIS = {-0.13,1,0};

#var AnyVec3 LASIS = {0.13,1,0};

#var AnyVec3 SACRUM = {0,1,-0.14} ;

 #var AnyVec3 PelvisPos = DesignVar((1/3)*( RASIS + LASIS + SACRUM ));

 #var AnyMat33 PelvisRotMat = RotMat(SACRUM , 

                                    0.5*(LASIS +RASIS ),

                                    LASIS )*RotMat(pi/2,x);  

#var AnyVec3 RKNEE = RightHip +{0,-0.5,0}*PelvisRotMat';

#var AnyVec3 LKNEE= LeftHip +{0,-0.5,0} *PelvisRotMat';

#var AnyVec3 RANKLE= RKNEE + {0,-0.5,0}*RightThighRotation';

#var AnyVec3 LANKLE= LKNEE + {0,-0.5,0}*LeftThighRotation';

#var AnyVec3 RTOE= RANKLE + {0.21,-0.05,0}*RightShankRotation';

#var AnyVec3 LTOE= LANKLE + {0.21,-0.05,0}*LeftShankRotation';

#var AnyVec3 RSHOULDER= PelvisPos +{0,0.6,0.15}*PelvisRotMat';

#var AnyVec3 LSHOULDER= PelvisPos +{0,0.6,-0.15}*PelvisRotMat';

    AnyFolder PelvisRotations = 

    {

       AnyVar yRot1 = asin(-.PelvisRotMat[2][0]);

       AnyVar yRot2 = pi - yRot1;

       AnyVar zRot1 = atan2(.PelvisRotMat[1][0]/cos(yRot1), .PelvisRotMat[0][0]/cos(yRot1));

       AnyVar zRot2 = atan2(.PelvisRotMat[1][0]/cos(yRot2), .PelvisRotMat[0][0]/cos(yRot2));

       AnyVar xRot1 = atan2(.PelvisRotMat[2][1]/cos(yRot1),.PelvisRotMat[2][2]/cos(yRot1));

       AnyVar xRot2 = atan2(.PelvisRotMat[2][1]/cos(yRot2),.PelvisRotMat[2][2]/cos(yRot2));

       AnyVar abs1 = abs(zRot1)+abs(yRot1)+abs(zRot1);

       AnyVar abs2 = abs(zRot2)+abs(yRot2)+abs(zRot2);

       AnyVar zRot = iffun(gtfun(abs1, abs2), zRot2, zRot1);

       AnyVar yRot = iffun(gtfun(abs1, abs2), yRot2, yRot1);

       AnyVar xRot = iffun(gtfun(abs1, abs2), xRot2, xRot1);

       // Check that the auto pos can reconstruct the rotation matrix.

       AnyFloat err = abs(.PelvisRotMat-RotMat(zRot,z)*RotMat(yRot,y)*RotMat(xRot,x));

       AnyInt AutoPosAssert = expect(ltfun(err,1e-5), "AutoPos failed to calculate the correct values");

     };

      AnyVar PelvisPosX = PelvisPos[0];

      AnyVar PelvisPosY = PelvisPos[1];

      AnyVar PelvisPosZ = PelvisPos[2];

      // Inital Pelvis rotation 

      AnyVar PelvisRotX = DesignVar(PelvisRotations.xRot*180/pi);

      AnyVar PelvisRotY = DesignVar(PelvisRotations.yRot*180/pi);

      AnyVar PelvisRotZ = DesignVar(PelvisRotations.zRot*180/pi);

   AnyVec3 RightHip = PelvisPos + {0.08, -0.15, 0.12}*PelvisRotMat';

   AnyVec3 LeftHip = PelvisPos + {0.08, -0.15, -0.12}*PelvisRotMat';

   AnyMat33 RightThighRotation = RotMat( RKNEE, 

                                         RightHip , 

                                         RKNEE+{0,0,1}*PelvisRotMat')

                                        *RotMat(-pi/2,y)*RotMat(-pi/2,x);                             

   AnyMat33 LeftThighRotation = RotMat( LKNEE,

                                        LeftHip , 

                                        RKNEE+ {0,0,1}*PelvisRotMat')

                                        *RotMat(-pi/2,y)*RotMat(-pi/2,x);                             

   AnyMat33 RightShankRotation = RotMat(RANKLE, 

                                       RKNEE,

                                       RANKLE + {0,0,1}*RightThighRotation')

                                        *RotMat(-pi/2,y)*RotMat(-pi/2,x);                             

   AnyMat33 LeftShankRotation = RotMat( LANKLE, 

                                       LKNEE,

                                       LANKLE + {0,0,1}*LeftThighRotation')

                                        *RotMat(-pi/2,y)*RotMat(-pi/2,x);                             

   AnyMat33 RightFootRotation = RotMat(RANKLE, 

                                       RTOE,

                                       RKNEE);

   AnyMat33 LeftFootRotation = RotMat( LANKLE, 

                                       LTOE,

                                       LKNEE );

   // Calculation of Hip Angles

   AnyFolder RightHipRotations = {

     AnyMat33 RotRel = .PelvisRotMat'*.RightThighRotation ;

     AnyVar xRot1 = asin(RotRel[2][1]);

     AnyVar xRot2 = pi-xRot1;

     AnyVar yRot1 = atan2(-RotRel[2][0]/cos(xRot1 ),RotRel[2][2]/cos(xRot1));

     AnyVar yRot2 = atan2(-RotRel[2][0]/cos(xRot2 ),RotRel[2][2]/cos(xRot2));

     AnyVar zRot1 = atan2(-RotRel[0][1]/cos(yRot1),RotRel[1][1]/cos(yRot1));

     AnyVar zRot2 = atan2(-RotRel[0][1]/cos(yRot2),RotRel[1][1]/cos(yRot2));

     AnyVar abs1 = abs(zRot1)+abs(yRot1)+abs(zRot1);

     AnyVar abs2 = abs(zRot2)+abs(yRot2)+abs(zRot2);

     AnyVar zRot = iffun(gtfun(abs1,abs2), zRot2,zRot1);

     AnyVar yRot = iffun(gtfun(abs1,abs2), yRot2,yRot1);

     AnyVar xRot = iffun(gtfun(abs1,abs2), xRot2,xRot1) ;

   };

   AnyFolder LeftHipRotations = {

     AnyMat33 RotRel = .PelvisRotMat'*.LeftThighRotation ;

     AnyVar xRot1 = asin(RotRel[2][1]);

     AnyVar xRot2 = pi-xRot1;

     AnyVar yRot1 = atan2(-RotRel[2][0]/cos(xRot1 ),RotRel[2][2]/cos(xRot1));

     AnyVar yRot2 = atan2(-RotRel[2][0]/cos(xRot2 ),RotRel[2][2]/cos(xRot2));

     AnyVar zRot1 = atan2(-RotRel[0][1]/cos(yRot1),RotRel[1][1]/cos(yRot1));

     AnyVar zRot2 = atan2(-RotRel[0][1]/cos(yRot2),RotRel[1][1]/cos(yRot2));

     AnyVar abs1 = abs(zRot1)+abs(yRot1)+abs(zRot1);

     AnyVar abs2 = abs(zRot2)+abs(yRot2)+abs(zRot2);

     AnyVar zRot = iffun(gtfun(abs1,abs2), zRot2,zRot1);

     AnyVar yRot = iffun(gtfun(abs1,abs2), yRot2,yRot1);

     AnyVar xRot = iffun(gtfun(abs1,abs2), xRot2,xRot1) ;

   };

   AnyFolder RightKneeRotations = {

     AnyMat33 RotRel = .RightThighRotation' *.RightShankRotation ;

     AnyVar xRot1 = asin(RotRel[2][1]);

     AnyVar xRot2 = pi-xRot1;

     AnyVar yRot1 = atan2(-RotRel[2][0]/cos(xRot1 ),RotRel[2][2]/cos(xRot1));

     AnyVar yRot2 = atan2(-RotRel[2][0]/cos(xRot2 ),RotRel[2][2]/cos(xRot2));

     AnyVar zRot1 = atan2(-RotRel[0][1]/cos(yRot1),RotRel[1][1]/cos(yRot1));

     AnyVar zRot2 = atan2(-RotRel[0][1]/cos(yRot2),RotRel[1][1]/cos(yRot2));

     AnyVar abs1 = abs(zRot1)+abs(yRot1)+abs(zRot1);

     AnyVar abs2 = abs(zRot2)+abs(yRot2)+abs(zRot2);

     AnyVar zRot = iffun(gtfun(abs1,abs2), zRot2,zRot1);

     AnyVar yRot = iffun(gtfun(abs1,abs2), yRot2,yRot1);

     AnyVar xRot = iffun(gtfun(abs1,abs2), xRot2,xRot1) ;

   };

   AnyFolder LeftKneeRotations = {

     AnyMat33 RotRel = .LeftThighRotation' *.LeftShankRotation ;

     AnyVar xRot1 = asin(RotRel[2][1]);

     AnyVar xRot2 = pi-xRot1;

     AnyVar yRot1 = atan2(-RotRel[2][0]/cos(xRot1 ),RotRel[2][2]/cos(xRot1));

     AnyVar yRot2 = atan2(-RotRel[2][0]/cos(xRot2 ),RotRel[2][2]/cos(xRot2));

     AnyVar zRot1 = atan2(-RotRel[0][1]/cos(yRot1),RotRel[1][1]/cos(yRot1));

     AnyVar zRot2 = atan2(-RotRel[0][1]/cos(yRot2),RotRel[1][1]/cos(yRot2));

     AnyVar abs1 = abs(zRot1)+abs(yRot1)+abs(zRot1);

     AnyVar abs2 = abs(zRot2)+abs(yRot2)+abs(zRot2);

     AnyVar zRot = iffun(gtfun(abs1,abs2), zRot2,zRot1);

     AnyVar yRot = iffun(gtfun(abs1,abs2), yRot2,yRot1);

     AnyVar xRot = iffun(gtfun(abs1,abs2), xRot2,xRot1) ;

   };

   AnyFolder RightAnkleRotations = {

     AnyMat33 RotRel = .RightShankRotation' *.RightFootRotation ;

     AnyVar xRot1 = asin(RotRel[2][1]);

     AnyVar xRot2 = pi-xRot1;

     AnyVar yRot1 = atan2(-RotRel[2][0]/cos(xRot1 ),RotRel[2][2]/cos(xRot1));

     AnyVar yRot2 = atan2(-RotRel[2][0]/cos(xRot2 ),RotRel[2][2]/cos(xRot2));

     AnyVar zRot1 = atan2(-RotRel[0][1]/cos(yRot1),RotRel[1][1]/cos(yRot1));

     AnyVar zRot2 = atan2(-RotRel[0][1]/cos(yRot2),RotRel[1][1]/cos(yRot2));

     AnyVar abs1 = abs(zRot1)+abs(yRot1)+abs(zRot1);

     AnyVar abs2 = abs(zRot2)+abs(yRot2)+abs(zRot2);

     AnyVar zRot = iffun(gtfun(abs1,abs2), zRot2,zRot1);

     AnyVar yRot = iffun(gtfun(abs1,abs2), yRot2,yRot1);

     AnyVar xRot = iffun(gtfun(abs1,abs2), xRot2,xRot1) ;

   };

   AnyFolder LeftAnkleRotations = {

     AnyMat33 RotRel = .LeftShankRotation' *.LeftFootRotation ;

     AnyVar xRot1 = asin(RotRel[2][1]);

     AnyVar xRot2 = pi-xRot1;

     AnyVar yRot1 = atan2(-RotRel[2][0]/cos(xRot1 ),RotRel[2][2]/cos(xRot1));

     AnyVar yRot2 = atan2(-RotRel[2][0]/cos(xRot2 ),RotRel[2][2]/cos(xRot2));

     AnyVar zRot1 = atan2(-RotRel[0][1]/cos(yRot1),RotRel[1][1]/cos(yRot1));

     AnyVar zRot2 = atan2(-RotRel[0][1]/cos(yRot2),RotRel[1][1]/cos(yRot2));

     AnyVar abs1 = abs(zRot1)+abs(yRot1)+abs(zRot1);

     AnyVar abs2 = abs(zRot2)+abs(yRot2)+abs(zRot2);

     AnyVar zRot = iffun(gtfun(abs1,abs2), zRot2,zRot1);

     AnyVar yRot = iffun(gtfun(abs1,abs2), yRot2,yRot1);

     AnyVar xRot = iffun(gtfun(abs1,abs2), xRot2,xRot1) ;

   };   

   #var AnyVar PelvisThoraxExtension = 0;

   #var AnyVar PelvisThoraxLateralBending = 0;

   #var AnyVar PelvisThoraxRotation = 0;

   #var AnyVar NeckExtension = 0;

   #var AnyVar NeckLateralBending = 0;

   #var AnyVar NeckRotation = 0;

AnyFolder Right = {

  #var AnyVar HipFlexion = .RightHipRotations.zRot*180/pi;

  #var AnyVar HipAbduction = -.RightHipRotations.xRot*180/pi;

  #var AnyVar HipExternalRotation = 0;

  #var AnyVar KneeFlexion = -.RightKneeRotations.zRot*180/pi;

  #var AnyVar AnklePlantarFlexion = -.RightAnkleRotations.zRot*180/pi;

  #var AnyVar SubTalarEversion = 0;

  #var AnyVar GlenohumeralFlexion = 0;

  #var AnyVar GlenohumeralAbduction = 1;

  #var AnyVar GlenohumeralExternalRotation = 0;

  #var AnyVar ElbowFlexion = 0;

  #var AnyVar ElbowPronation = 0;

  #var AnyVar WristFlexion = 0;

  #var AnyVar WristAbduction = 0;

};

AnyFolder Left = {

  #var AnyVar HipFlexion = .LeftHipRotations.zRot*180/pi;

  #var AnyVar HipAbduction = .LeftHipRotations.xRot*180/pi;

  #var AnyVar HipExternalRotation = 0;

  #var AnyVar KneeFlexion = -.LeftKneeRotations.zRot*180/pi;

  #var AnyVar AnklePlantarFlexion = -.LeftAnkleRotations.zRot*180/pi;

  #var AnyVar SubTalarEversion = 0;

  #var AnyVar GlenohumeralFlexion = 0;

  #var AnyVar GlenohumeralAbduction = 1;

  #var AnyVar GlenohumeralExternalRotation = 0;

  #var AnyVar ElbowFlexion = 0;

  #var AnyVar ElbowPronation = 0;

  #var AnyVar WristFlexion = 0;

  #var AnyVar WristAbduction = 0;

};

AnyFolder &InitPosRef= MANNEQUIN_FOLDER.Posture;    

InitPosRef= {

PelvisPosX = .PelvisPosX;

PelvisPosY = .PelvisPosY;

PelvisPosZ = .PelvisPosZ;

// Inital Pelvis rotation 

PelvisRotX = .PelvisRotX;

PelvisRotY = .PelvisRotY;

PelvisRotZ = .PelvisRotZ;

PelvisThoraxExtension = .PelvisThoraxExtension;

PelvisThoraxLateralBending = .PelvisThoraxLateralBending ;

PelvisThoraxRotation = .PelvisThoraxRotation ;

NeckExtension = .NeckExtension ;

NeckLateralBending = .NeckLateralBending ;

NeckRotation = .NeckRotation ;

Right = {

  HipFlexion = ..Right.HipFlexion ;

  HipAbduction = ..Right.HipAbduction ;

  HipExternalRotation = ..Right.HipExternalRotation ;

  KneeFlexion = ..Right.KneeFlexion ;

  AnklePlantarFlexion = ..Right.AnklePlantarFlexion ;

  SubTalarEversion = ..Right.SubTalarEversion ;

  GlenohumeralFlexion = ..Right.GlenohumeralFlexion ;

  GlenohumeralAbduction = ..Right.GlenohumeralAbduction ;

  GlenohumeralExternalRotation = ..Right.GlenohumeralExternalRotation ;

  ElbowFlexion = ..Right.ElbowFlexion ;

  ElbowPronation = ..Right.ElbowPronation ;

  WristFlexion = ..Right.WristFlexion ;

  WristAbduction = ..Right.WristAbduction ;

};

Left = {

  HipFlexion = ..Left.HipFlexion ;

  HipAbduction = ..Left.HipAbduction ;

  HipExternalRotation = ..Left.HipExternalRotation ;

  KneeFlexion = ..Left.KneeFlexion ;

  AnklePlantarFlexion = ..Left.AnklePlantarFlexion ;

  SubTalarEversion = ..Left.SubTalarEversion ;

  GlenohumeralFlexion = ..Left.GlenohumeralFlexion ;

  GlenohumeralAbduction = ..Left.GlenohumeralAbduction ;

  GlenohumeralExternalRotation = ..Left.GlenohumeralExternalRotation ;

  ElbowFlexion = ..Left.ElbowFlexion ;

  ElbowPronation = ..Left.ElbowPronation ;

  WristFlexion = ..Left.WristFlexion ;

  WristAbduction = ..Left.WristAbduction ;

};

};// InitPosRef

//#var AnyVar ThighLengthIni = 0.5*(vnorm(RightHip-RKNEE)+vnorm(LeftHip-LKNEE));

//#var AnyVar ShankLengthIni = 0.5*(vnorm(RKNEE-RANKLE)+vnorm(LKNEE-LANKLE));

//

//

//AnyFolder &IniSegmentLength = Main.ModelSetup.SubjectSpecificData.Anthropometrics = {

//ThighLengthIni = .ThighLengthIni;

//ShankLengthIni = .ShankLengthIni;

//

//};// IniSegmentLength

//

};

#endif