#ifndef _ANYMOCAP_ROTATION_AXIS_ANGLES_ANY_

#define _ANYMOCAP_ROTATION_AXIS_ANGLES_ANY_

/*

---

group: Utilities

topic: Angles

descr: |

  Class template to calculate Bryant (euler) angles for the z-y-x rotation

  sequence given a a rotation matrix.

---

To use these drivers import the file:

#include "<AMMR_TOOLS>/AnyMoCap/RotationAxisAngles.any"

See the individual classes for more information.

*/

//

//  Calculates the Bryant (euler) angles for the z-y-x rotation sequence given a 

//  rotation matrix

//

// :::{rubric} Example

// :::

// Here is an example of how to use the class.

//

// ```{code-block} AnyScriptDoc

//

//  RotAxisAnglesZYX Angles(ROT = .SomeSeg.Axes0) = {};

//

//  // Results are in radians

//  AnyVar zRot = Angles.zRot;

//  AnyVar yRot = Angles.yRot;

//  AnyVar xRot = Angles.xRot;

//

// ```

//

//  The class can also calculate the relative 

//  rotation angles between two segments. 

//

// ```{code-block} AnyScriptDoc

//

// RotAxisAnglesZYX Angles(ROT = .SomeSeg.Axes0) = {

//   ROT_rel = .SomeOtherSeg.Axes0;

// };

//

// ```

//

#class_template RotAxisAnglesZYX (ROT){

  // Arguments

  // -----------

  //  RotAxisAnglesZYX#ROT

  //   The rotation matrix to calculate the angles from

    // RotAxisAnglesZYX

    ///   A relative rotation matrix so the angles are calculated

    ///   between these two rotation matrices.

    #var AnyMat33 ROT_rel = eye(3);

    AnyFolder Calculations = {

      AnyMat33 Rot = .ROT_rel'*.ROT;

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

      AnyVar yRot2 = pi - yRot1;

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

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

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

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

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

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

    };

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

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

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

}; 

/* 

  Class to bind two segments together with drivers based on their load time positions. The

  driver constraint type can be controlled by the `_CTYPE` arg

  The class adds a node on Seg1 with position and velocity set to 0. This helps certain

  situations where gimbal lock might occur. 

  The class can also add add the forces between the segments using strong recruited

  actuators. (Recruited actuators are used to avoid statically indeterminate systems,

  which could easily occur with normal reaction forces.)

    DriverBasedOnLoadPos driver1(Seg1=..Mass1,Seg2=..Mass2, NodeName = Node1) ={};

*/ 

#class_template DriverBasedOnLoadPos(AnySeg &Seg1,AnySeg &Seg2, NodeName, RECRUITMENT_CONSTRAINTS = 1, _CTYPE = Soft){

  RotAxisAnglesZYX  RotPos (ROT=.Seg2.Axes0)= {ROT_rel=.Seg1.Axes0;};

  Seg1= {

    //        AnyDrawSeg drw2 ={};

    AnyRefNode NodeName ={

      sRel=(..Seg2.r0-.r0)*.Axes0;

      ARel= RotMat(..RotPos.zRot,z)*RotMat(..RotPos.yRot,y)*RotMat(..RotPos.xRot,x);

    };

  };

  AnyKinEqSimpleDriver driver ={

    AnyKinLinear lin ={

      AnyRefFrame &ref1=..Seg1.NodeName;

      AnyRefFrame &ref2=..Seg2;

    };

    AnyKinRotational rot ={

      AnyRefFrame &ref1=..Seg1.NodeName;

      AnyRefFrame &ref2=..Seg2;

      Type=RotVector;

    };

    DriverPos={0,0,0,0,0,0};

    DriverVel={0,0,0,0,0,0};

    Reaction.Type={Off,Off,Off,Off,Off,Off};

    CType={_CTYPE,_CTYPE,_CTYPE,_CTYPE,_CTYPE,_CTYPE};

  };

  #if RECRUITMENT_CONSTRAINTS

  AnyRecruitedActuator RecruitedReactions_1 = {

    AnyKinMeasure &lin=.driver.lin;

    AnyKinMeasure &rot=.driver.rot;

    Type = NonNegative;

    Strength = {2000,2000,2000,2000,2000,2000};

    SET_DEFAULT_ACTUATOR_VOLUME;

  };

  AnyRecruitedActuator RecruitedReactions_2 = {

    AnyKinMeasure &lin=.driver.lin;

    AnyKinMeasure &rot=.driver.rot;

    Type = NonPositive;

    Strength = {2000,2000,2000,2000,2000,2000};

    SET_DEFAULT_ACTUATOR_VOLUME;

  };

  #endif

};

#endif