# The following traits are required to
# adjust the visuals of the the cylinder parts
# that come from the Assets.
trait UpsideDownCylinder:
    visual.local_transform.rotation: Math.Quat.angle_axis(Math.PI, {0,1,0})
    geometry.local_transform.rotation: visual.local_transform.rotation
trait CylinderPart90PosRotation:
    visual.local_transform.rotation: Math.Quat.angle_axis(Math.PI * 0.5, {0,1,0})
    geometry.local_transform.rotation: visual.local_transform.rotation
trait CylinderPart90NegRotation:
    visual.local_transform.rotation: Math.Quat.angle_axis(-Math.PI * 0.5, {0,1,0})
    geometry.local_transform.rotation: visual.local_transform.rotation

trait LiftCylinderVisual:
    body.visual is Visuals.Geometries.Cylinder:
        height: body.length
        radius: body.diameter * 0.5
        local_transform: body.cylinder.local_transform

Base is Vehicles.DumpTruck.ArticulatedWithLink:
    .doc:"""
    The VehicleAssmebly primarily maps the geometry of the obj-files in the
    Assets folder to the OpenPLX model.
    """
    with Vehicles.DumpTruck.Interfaces.SixWheels
    with Assets.Properties.Mass
    with Assets.Properties.Suspension

    max_angle:      45.0*(Math.PI/180)  # Max steering angle
    max_roll_angle: 10.0*(Math.PI/180)  # Max roll angle (front to rear frame)


    left_front_wheel becomes KomatsuHM400.Assets.Components.Wheel_L
    right_front_wheel becomes KomatsuHM400.Assets.Components.Wheel_R
    left_rear_wheel_1 becomes KomatsuHM400.Assets.Components.Wheel_L
    right_rear_wheel_1 becomes KomatsuHM400.Assets.Components.Wheel_R
    left_rear_wheel_2 becomes KomatsuHM400.Assets.Components.Wheel_L
    right_rear_wheel_2 becomes KomatsuHM400.Assets.Components.Wheel_R

    chassis becomes Vehicles.Chassis.Articulated.IntermediateLink:
        local_transform.position: {0,0,0}

        front_frame becomes MachineModeling.Components.Primitive.Base with Vehicles.Chassis.Articulated.Interfaces.DualJoint, Vehicles.Chassis.Articulated.Interfaces.DualSteeringActuators, Vehicles.DumpTruck.Interfaces.Chassis.AFrame:
            body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.f_frameBody:
                kinematics.local_transform.position: {0,0,0}
                velocity_vector_start is Physics3D.Interactions.MateConnector:
                    position: {0,0,0}
                velocity_vector_end is Physics3D.Interactions.MateConnector:
                    position: forward_vector
                hitch_c_2 is Physics3D.Interactions.MateConnector:
                    main_axis:  chassis.front_frame.body.hitch_c.main_axis
                    normal:     chassis.front_frame.body.hitch_c.normal
                    position:   chassis.front_frame.body.hitch_c.position + {0, 0, 0.7}

            upper_joint_connector:              chassis.front_frame.body.hitch_c
            lower_joint_connector:              chassis.front_frame.body.hitch_c_2
            left_steering_actuator_connector:   chassis.front_frame.body.l_steer_cyl_c
            right_steering_actuator_connector:  chassis.front_frame.body.r_steer_cyl_c
            center_a_frame_connector:           chassis.front_frame.body.f_axle_c
            left_a_frame_connector:             chassis.front_frame.body.fl_sus_cyl_c
            right_a_frame_connector:            chassis.front_frame.body.fr_sus_cyl_c

        velocity_vector_start_output is Physics3D.Signals.MateConnector.PositionOutput:
            source: chassis.front_frame.body.velocity_vector_start
        velocity_vector_end_output is Physics3D.Signals.MateConnector.PositionOutput:
            source: chassis.front_frame.body.velocity_vector_end

        link becomes MachineModeling.Components.Primitive.Base with Vehicles.Chassis.Articulated.Interfaces.DualJoint, Vehicles.Chassis.Articulated.Interfaces.SingleJoint, Vehicles.Chassis.Articulated.Interfaces.DualSteeringActuators:
            body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.hitchBody:
                kinematics.local_transform.position: {0,0,0}

                f_frame_c_2 is Physics3D.Interactions.MateConnector:
                    main_axis:  chassis.link.body.f_frame_c.main_axis
                    normal:     chassis.link.body.f_frame_c.normal
                    position:   chassis.link.body.f_frame_c.position + {0, 0, 0.7}

            upper_joint_connector:                 chassis.link.body.f_frame_c
            lower_joint_connector:                 chassis.link.body.f_frame_c_2
            left_steering_actuator_connector:      chassis.link.body.l_steer_pist_c
            right_steering_actuator_connector:     chassis.link.body.r_steer_pist_c
            joint_connector:                       chassis.link.body.b_frame_c

        rear_frame becomes MachineModeling.Components.Primitive.Base with Vehicles.Chassis.Articulated.Interfaces.SingleJoint, Vehicles.DumpTruck.Interfaces.Chassis.DualAFrame, Vehicles.DumpTruck.Interfaces.CargoBed.SingleJoint, Vehicles.DumpTruck.Interfaces.CargoBed.DualActuator:
            body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.b_frameBody:
                kinematics.local_transform.position: {0,0,0}

            joint_connector: chassis.rear_frame.body.hitch_c

            front_center_a_frame_connector: chassis.rear_frame.body.c_axle_c
            rear_center_a_frame_connector:  chassis.rear_frame.body.b_axle_c
            rear_left_a_frame_connector:    chassis.rear_frame.body.l_equalizer_c
            rear_right_a_frame_connector:   chassis.rear_frame.body.r_equalizer_c

            cargo_bed_connector:  chassis.rear_frame.body.body_c
            left_cargo_bed_actuator_connector:  chassis.rear_frame.body.l_lift_pist_c
            right_cargo_bed_actuator_connector: chassis.rear_frame.body.r_lift_pist_c

        frame_connection.link: chassis.link
        frame_connection.to_link.front_frame: chassis.front_frame
        frame_connection.to_link.rear_frame: chassis.link
        frame_connection.to_link.upper_from_connector: chassis.front_frame.upper_joint_connector
        frame_connection.to_link.lower_from_connector: chassis.front_frame.lower_joint_connector
        frame_connection.to_link.upper_to_connector: chassis.link.upper_joint_connector
        frame_connection.to_link.lower_to_connector: chassis.link.lower_joint_connector

        frame_connection.to_link.upper_pivot becomes MachineModeling.Connections.Pivot.Hinge:
           hinge.initial_angle: initial_angle
           hinge_range:
               connectors: chassis.frame_connection.to_link.upper_pivot.hinge.connectors
               start: max_angle
               end: -max_angle

        frame_connection.to_link.lower_pivot becomes MachineModeling.Connections.Pivot.Hinge:
            hinge.initial_angle: initial_angle
            hinge_range:
               connectors: chassis.frame_connection.to_link.lower_pivot.hinge.connectors
               start: max_angle
               end: -max_angle

        frame_connection.from_link:
            hinge.initial_angle: initial_roll_angle
            hinge_range:
                connectors: chassis.frame_connection.from_link.hinge.connectors
                start: max_roll_angle
                end: -max_roll_angle

    steering:
        left_steering_actuator becomes MachineModeling.Actuators.Linear.Cylinder:
            rod.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.l_steer_pistBody, CylinderPart90PosRotation
            barrel.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.l_steer_cylBody, CylinderPart90NegRotation
        right_steering_actuator becomes MachineModeling.Actuators.Linear.Cylinder:
            rod.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.r_steer_pistBody, CylinderPart90PosRotation
            barrel.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.r_steer_cylBody, CylinderPart90NegRotation

    front_axle_connection becomes Vehicles.DumpTruck.Connections.AFrame.Base with Vehicles.DumpTruck.Connections.AFrame.Modules.Suspension:
        chassis_frame: chassis.front_frame

        left_suspension.actuator.rod.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.fl_sus_pistBody
        left_suspension.actuator.barrel.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.fl_sus_cylBody, UpsideDownCylinder
        right_suspension.actuator.rod.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.fr_sus_pistBody
        right_suspension.actuator.barrel.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.fr_sus_cylBody, UpsideDownCylinder

        a_frame: front_wheel_axle
        left_axle_connector:    front_wheel_axle.left_wheel_connector
        right_axle_connector:   front_wheel_axle.right_wheel_connector

    rear_axle_connection becomes Vehicles.DumpTruck.Connections.AFrame.Dual:
        chassis_frame: chassis.rear_frame

        front_a_frame: rear_wheel_axle_1
        rear_a_frame: rear_wheel_axle_2
        left_equalizer_connection.hinge.initial_angle: 0
        right_equalizer_connection.hinge.initial_angle: 0
        left_equalizer becomes MachineModeling.Components.Primitive.Base:
            body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.l_equalizerBody
        right_equalizer becomes MachineModeling.Components.Primitive.Base:
            body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.r_equalizerBody

        left_suspension.actuator.rod.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.bl_sus_pistBody
        left_suspension.actuator.barrel.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.bl_sus_cylBody, UpsideDownCylinder
        right_suspension.actuator.rod.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.br_sus_pistBody
        right_suspension.actuator.barrel.body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.br_sus_cylBody, UpsideDownCylinder

        left_equalizer_pivot_connector:  rear_axle_connection.left_equalizer.body.b_frame_c
        left_equalizer_rear_connector:   rear_axle_connection.left_equalizer.body.bl_sus_cyl_c
        right_equalizer_pivot_connector: rear_axle_connection.right_equalizer.body.b_frame_c
        right_equalizer_rear_connector:  rear_axle_connection.right_equalizer.body.br_sus_cyl_c


    front_wheel_axle:
        body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.f_axleBody, Vehicles.DumpTruck.Interfaces.Chassis.AFrame:
            center_a_frame_connector: front_wheel_axle.body.f_frame_c
            left_a_frame_connector: front_wheel_axle.body.fl_sus_pist_c
            right_a_frame_connector: front_wheel_axle.body.fr_sus_pist_c
        center_a_frame_connector: front_wheel_axle.body.center_a_frame_connector
        left_a_frame_connector: front_wheel_axle.body.left_a_frame_connector
        right_a_frame_connector: front_wheel_axle.body.right_a_frame_connector
        left_wheel_connector: front_wheel_axle.body.fl_wheel_c
        right_wheel_connector: front_wheel_axle.body.fr_wheel_c


    rear_wheel_axle_1:
        body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.c_axleBody, Vehicles.DumpTruck.Interfaces.Chassis.AFrame:
            center_a_frame_connector: rear_wheel_axle_1.body.b_frame_c
        center_a_frame_connector: rear_wheel_axle_1.body.center_a_frame_connector
        left_wheel_connector: rear_wheel_axle_1.body.cl_wheel_c
        right_wheel_connector: rear_wheel_axle_1.body.cr_wheel_c


    rear_wheel_axle_2:
        body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.b_axleBody, Vehicles.DumpTruck.Interfaces.Chassis.AFrame:
            center_a_frame_connector: rear_wheel_axle_2.body.b_frame_c
            left_a_frame_connector: rear_wheel_axle_2.body.bl_sus_pist_c
            right_a_frame_connector: rear_wheel_axle_2.body.br_sus_pist_c
        center_a_frame_connector: rear_wheel_axle_2.body.center_a_frame_connector
        left_a_frame_connector: rear_wheel_axle_2.body.left_a_frame_connector
        right_a_frame_connector: rear_wheel_axle_2.body.right_a_frame_connector
        left_wheel_connector: rear_wheel_axle_2.body.bl_wheel_c
        right_wheel_connector: rear_wheel_axle_2.body.br_wheel_c


    cargo_bed becomes MachineModeling.Components.Primitive.Base with Vehicles.DumpTruck.Interfaces.CargoBed.SingleJoint, Vehicles.DumpTruck.Interfaces.CargoBed.DualActuator:
        body becomes Physics3D.Bodies.RigidBody with KomatsuHM400.Assets.RigidBodyDefinitions.HM400.bodyBody:
            kinematics.local_transform.position: {0,0,0}
        cargo_bed_connector:  cargo_bed.body.b_frame_c
        left_cargo_bed_actuator_connector:  cargo_bed.body.l_lift_cyl_c
        right_cargo_bed_actuator_connector: cargo_bed.body.r_lift_cyl_c

    cargo_bed_connection becomes Vehicles.DumpTruck.Connections.CargoBed.SingleJointDualActuator:
        chassis_frame: chassis.rear_frame
        cargo_bed: cargo_bed
        initial_angle: 0
        left_lift:
            actuator becomes MachineModeling.Actuators.Linear.Cylinder:
                barrel becomes MachineModeling.Actuators.Linear.CylinderPart with LiftCylinderVisual
                rod becomes MachineModeling.Actuators.Linear.CylinderPart with LiftCylinderVisual

            # Cylinder attachments as universal joints by allowing rotation around normal axis
            rod_connection.hinge.flexibility becomes Physics3D.Interactions.Flexibility.LinearElasticHingeFlexibility:
                around_normal.stiffness: 0
            barrel_connection.hinge.flexibility becomes Physics3D.Interactions.Flexibility.LinearElasticHingeFlexibility:
                around_normal.stiffness: 0

        right_lift:
            actuator becomes MachineModeling.Actuators.Linear.Cylinder:
                barrel becomes MachineModeling.Actuators.Linear.CylinderPart with LiftCylinderVisual
                rod becomes MachineModeling.Actuators.Linear.CylinderPart with LiftCylinderVisual

            # Cylinder attachments as universal joints by allowing rotation around normal axis
            rod_connection.hinge.flexibility becomes Physics3D.Interactions.Flexibility.LinearElasticHingeFlexibility:
                around_normal.stiffness: 0
            barrel_connection.hinge.flexibility becomes Physics3D.Interactions.Flexibility.LinearElasticHingeFlexibility:
                around_normal.stiffness: 0
