#VRML_SIM R2025a utf8
# Articulated Joint for the course PN3002 Robotica
# Updated 2025-08-28

PROTO Joint [
  field SFString joint        "1"
  field SFString description "R 0 0.6 0.4 0"
  field SFFloat  position    0.0
  field SFFloat  minPosition 0.0
  field SFFloat  maxPosition 0.0
  field MFNode   nextJoints  []
]
{
  %<
  let name = fields.joint.value;
  let description = fields.description.value.split(" ");
  let type = description[0];
  let theta = parseFloat(description[1]);
  let d = parseFloat(description[2]);
  let a = parseFloat(description[3]);
  let alpha = parseFloat(description[4]);
  let qInit = parseFloat(description[5]);
  let lengthA = Math.abs(a);
  let lengthD = Math.abs(d);
  >%

  %< if(type=="R"){ >%
  HingeJoint {
    jointParameters HingeJointParameters {
      position IS position
      axis 0 0 1
    }
    device [
      RotationalMotor {
        name "joint_%<=name>%"
        minPosition IS minPosition
        maxPosition IS maxPosition
      }
      PositionSensor {
        name "sensor_%<=name>%"
      }
    ]
  %<}else{>%
  SliderJoint {
    jointParameters JointParameters {
      position IS position
    }
    device [
      LinearMotor {
        name "joint_%<=name>%"
        minPosition IS minPosition
        maxPosition IS maxPosition
      }
      PositionSensor {
        name "sensor_%<=name>%"
      }
    ]
  %<}>%
    endPoint Solid {
      name "link_%<=name>%"
      children [
        DEF GEOMETRY%<=name>% Group {
          children [
            %< if(lengthA>0){ >%
            DEF A%<=name>% Pose {
              translation %<= a/2 >% 0 %<= d >%
              rotation 0 1 0 1.5708
              children [
                Shape {
                  appearance PBRAppearance {
                    roughness 1
                    metalness 0
                  }
                  geometry Cylinder {
                    height %<= lengthA >%
                    radius 0.02
                  }
                }
              ]
            }
            %<}>%
            DEF B%<=name>% Pose {
              translation 0 0 %<= d >%
              children [
                Shape {
                  appearance PBRAppearance {
                    roughness 1
                    metalness 0
                  }
                  geometry Sphere {
                    radius 0.02
                  }
                }
              ]
            }
            %< if(lengthD>0){ >%
            DEF D%<=name>% Pose {
              translation 0 0 %<= d/2 >%
              children [
                Shape {
                  appearance PBRAppearance {
                    roughness 1
                    metalness 0
                  }
                  geometry Cylinder {
                    height %<= lengthD >%
                    radius 0.02
                  }
                }
              ]
            }
            %<}>%
            DEF E%<=name>% Pose {
              children [
                Shape {
                  appearance PBRAppearance {
                    roughness 1
                    metalness 0
                  }
                  %< if(type=="R"){ >%
                  geometry Cylinder {
                    height 0.1
                    radius 0.05
                  }
                  %<}else{>%
                  geometry Box {
                    size 0.1 0.1 0.1
                  }
                  %<}>%
                }
              ]
            }
          ]
        }
        Pose {
          translation %<= a >% 0 %<= d >%
          rotation 0 0 1 %<= theta >%
          children [
            Pose {
              rotation 1 0 0 %<= alpha >%
              children IS nextJoints
            }
          ]
        }
      ]
      boundingObject USE GEOMETRY%<=name>%
      physics Physics {
      }
    }
  }
}