TrackJointFeeder.h

//  trax track library
//  AD 2022 
//
//  "Und liegt ein Koerper regungslos im Schnee,
//   schmeckt erst so richtig der Kaffee."
//
//                              Reinhard Fendrich
//
// Copyright (c) 2025 Trend Redaktions- und Verlagsgesellschaft mbH
// Copyright (c) 2019 Marc-Michael Horstmann
//
// Permission is hereby granted to any person obtaining a copy of this software 
// and associated source code (the "Software"), to use, view, and study the 
// Software for personal or internal business purposes, subject to the following 
// conditions:
//
// 1. Redistribution, modification, sublicensing, or commercial use of the 
// Software is NOT permitted without prior written consent from the copyright 
// holder.
//
// 2. The Software is provided "AS IS", without warranty of any kind, express 
// or implied.
//
// 3. All copies of the Software must retain this license notice.
//
// For further information, please contact: horstmann.marc@trendverlag.de
 
#pragma once
 
#include "TrackJoint.h"
#include "TrackJointLimits.h"
#include "Wheelset.h"
 
#include "trax/Location.h"
#include "trax/Jack.h"
 
namespace trax{
 
    struct TractionForceCharacteristic;

    class TrackJointFeeder{
    public:
        dclspc TrackJointFeeder( Real engine_meters_per_unit, Real engine_kilograms_per_unit ) noexcept;

        bool dclspc virtual Update( Time dt );
 
        bool dclspc Rail( std::shared_ptr<const Track> pTrack, const TrackLocation& tl, bool doPulse = true );
 
        bool dclspc Rail( const Location& location, bool doPulse = true );
 
        void dclspc DeRail( bool doPulse = true );
 
        bool dclspc IsRailed() const noexcept;
 
        bool dclspc IsMovableBodySleeping() const noexcept;
 
        void dclspc WakeUp() noexcept;
 
        void dclspc SetVelocity( Velocity velocity );
 
        Velocity dclspc GetVelocity() const noexcept;
 
        dclspc const Location& GetLocation() const noexcept;
 
        virtual void SetLimits( const TrackJointLimits& wfl ) = 0;
 
        virtual void SetMotorForceLimits( Force min, Force max ) noexcept = 0;
 
        virtual void SetMotorTarget( Velocity ) noexcept = 0;
 
        virtual Velocity GetMotorTarget() noexcept = 0;
 

        virtual spat::Frame<Length,One> GlobalAnchor() const = 0;

        virtual spat::Frame<Length,One> Anchor() const noexcept = 0;

        virtual void Anchor( const spat::Frame<Length,One>& frame ) = 0;
 
 
        // Sensor and Signal:

        dclspc std::shared_ptr<Event> AttachEvent( std::shared_ptr<Event> pEvent ) noexcept;
 

        dclspc std::shared_ptr<SignalTarget> AttachSignal( std::shared_ptr<SignalTarget> pSignalTarget ) noexcept;
 
 
        // Jacks 'N Plugs:

        dclspc Jack& JackOnRail() noexcept;

        dclspc Jack& JackOnDerail() noexcept;

        dclspc Jack& JackOnRollingStart() noexcept;

        dclspc Jack& JackOnRollingStop() noexcept;

        dclspc Jack& JackOnStartTilting() noexcept;

        dclspc Jack& JackOnStopTilting() noexcept;

        dclspc Jack& JackOnTrackTransition() noexcept;
 
 
        inline Real EngineMetersPerUnit() const noexcept{
            return m_EngineMetersPerUnit;
        }
        inline Real EngineKilogramsPerUnit() const noexcept{
            return m_EngineKilogramsPerUnit;
        }
    protected:
        virtual void TurnAnchor() const = 0;
        virtual void TrackBodyChange( std::shared_ptr<const Body> pToBody ) = 0;
        virtual void SetTimeStep( Time dt ) = 0;
        virtual void UpdateBodies() const = 0;
        virtual bool SetValues( const TrackData<Real>& trackData ) = 0;
        virtual bool DoIsMovableBodySleeping() const noexcept = 0;
        virtual void DoWakeUp() const = 0;
        virtual void DoRail() = 0;
        virtual void DoDerail() = 0;
 
        virtual std::pair<Length,bool> MoveBy( Length ds );
    private:
        Location m_Location;
        Real m_EngineMetersPerUnit;
        Real m_EngineKilogramsPerUnit;
        bool m_WakeupFlag;
 
        Velocity m_CurrentVelocity = 0_mIs;
        bool m_bRolling = false;

        inline Length ds() const
        // ds = (A.P(t+dt) - wF.P(s,t+dt)) * wF.T(s,t)
        {
            const spat::Position<Length> AP{ GlobalAnchor().P };
            spat::VectorBundle<Length,One> wF;
            m_Location.Transition( wF );
 
            return (AP - wF.P) * wF.T;
        }
 
        std::shared_ptr<Event> m_pEvent;
        std::shared_ptr<SignalTarget> m_pSignalTarget;
 
        Jack_Imp m_JackOnRail           { "JackOnRail" };
        Jack_Imp m_JackOnDerail         { "JackOnDerail" };
        Jack_Imp m_JackOnRollingStart   { "JackOnRollingStart" };
        Jack_Imp m_JackOnRollingStop    { "JackOnRollingStop" };
        Jack_Imp m_JackOnStartTilting   { "JackOnStartTilting" };
        Jack_Imp m_JackOnStopTilting    { "JackOnStopTilting" };
        Jack_Imp m_JackOnTrackTransition{ "JackOnTrackTransition" };
    };
 

    class TrackJointFeederMotorModel : public TrackJointFeeder{
    public:
        TrackJointFeederMotorModel( Real engine_meters_per_unit, Real engine_kilograms_per_unit );
 
        bool dclspc Update( Time dt ) override;

        void dclspc TargetVelocity( Velocity vTarget ) noexcept;
 

        Velocity dclspc TargetVelocity() const noexcept;
 
    //  EndType dclspc TargetDirection() const noexcept;

        Power dclspc MaximumPowerOutput() const noexcept;
 

        Power dclspc CurrentPowerOutput() const noexcept;
 

        Velocity dclspc MaximumVelocity() const noexcept;
 

        void dclspc Thrust( One byfactor ) noexcept;
 

        One dclspc Thrust() const noexcept;
 

        void dclspc ThrustAbsolute( Force thrust ) noexcept;
 

        Force dclspc ThrustAbsolute() const noexcept;
 

        Force dclspc MaxThrust() const noexcept;
 
        Force dclspc MaxThrust( Velocity velocity ) const noexcept;
 

        void dclspc Brake( One byfactor ) noexcept;
 

        One dclspc Brake() const noexcept;
 

        void dclspc BrakeAbsolute( Force brake ) noexcept;
 

        Force dclspc BrakeAbsolute() const noexcept;
 

        Force dclspc MaxBrake() const noexcept;
 

        void dclspc SetTractionForceCharacteristic( std::shared_ptr<const struct TractionForceCharacteristic> pCharacteristic );
 

        std::shared_ptr<const TractionForceCharacteristic> dclspc GetTractionForceCharacteristic() const noexcept;
 

        int dclspc Attach( const Wheelset& ws );
 

        void dclspc DetachAllWheelsets() noexcept;
 

        int dclspc CntWheelsets() const;
 

        dclspc const Wheelset& GetWheelset( int idx ) const;
 

        dclspc Jack& JackOnMotorActive() noexcept;

        dclspc Jack& JackOnMotorInactive() noexcept;

        dclspc Jack& JackOnBrakeActive() noexcept;

        dclspc Jack& JackOnBrakeInactive() noexcept;
 
    protected:
        std::pair<Length,bool> MoveBy( Length ds ) override;
    private:
        std::shared_ptr<const TractionForceCharacteristic> m_pTFC;
        std::vector<Wheelset> m_Wheelsets;
 
        One m_ThrustFactor;
        One m_BrakingFactor;
        Velocity m_TargetVelocity;
 
        Force MaxPossibleWheelThrust() const noexcept;
        Force MaxWheelThrust() const noexcept;
        Force MaxWheelBraking() const noexcept;
        Force MaxWheelFriction() const noexcept;
        Real Characteristic( Velocity velocity ) const noexcept;
 
        void UpdateMotorValues() noexcept;
 
        bool CalculateMotorAction() noexcept;
        bool CalculateBrakeAction() noexcept;
        bool m_bMotorAction;
        bool m_bBrakeAction;
 
        Jack_Imp m_JackOnMotorActive{ "JackOnMotorActive" };
        Jack_Imp m_JackOnMotorInactive{ "JackOnMotorInactive" };
        Jack_Imp m_JackOnBrakeActive{ "JackOnBrakeActive" };
        Jack_Imp m_JackOnBrakeInactive{ "JackOnBrakeInactive" };
    };
 

    template<typename Valtype,class Base>
    class TrackJointFeeder_Imp : public Base{
    public:
        TrackJointFeeder_Imp() = delete;
        TrackJointFeeder_Imp( TrackJoint<Valtype>& rTrackJoint, Valtype engine_meters_per_unit, Valtype engine_kilograms_per_unit ) noexcept;
        ~TrackJointFeeder_Imp();
 
        void SetLimits( const TrackJointLimits& wfl ) override;
 
        void SetMotorForceLimits( Force min, Force max ) noexcept override;
 
        void SetMotorTarget( Velocity ) noexcept override;
 
        Velocity GetMotorTarget() noexcept override;
 
        spat::Frame<Length,One> GlobalAnchor() const override;
 
        spat::Frame<Length,One> Anchor() const noexcept override;
 
        void Anchor( const spat::Frame<Length,One>& frame ) noexcept override;
    protected:
        void TurnAnchor() const override;
        void TrackBodyChange( std::shared_ptr<const Body> pToBody ) override;
        void SetTimeStep( Time dt ) override;
        void UpdateBodies() const override;
        bool SetValues( const TrackData<Real>& trackData ) override;
        bool DoIsMovableBodySleeping() const noexcept override;
        void DoWakeUp() const override;
        void DoRail() override;
        void DoDerail() override;
    private:
        TrackJoint<Valtype>& m_TrackJoint;
    };

template<typename Valtype,class Base>
inline TrackJointFeeder_Imp<Valtype,Base>::TrackJointFeeder_Imp( 
    TrackJoint<Valtype>& rTrackJoint, 
    Valtype engine_meters_per_unit, 
    Valtype engine_kilograms_per_unit ) noexcept
    :   Base        { engine_meters_per_unit, engine_kilograms_per_unit },
        m_TrackJoint{ rTrackJoint }
{
}
 
template<typename Valtype, class Base>
inline trax::TrackJointFeeder_Imp<Valtype, Base>::~TrackJointFeeder_Imp()
{
    m_TrackJoint.release(); //trackjoints deletes on release.
}
 
template<typename Valtype, class Base>
inline void TrackJointFeeder_Imp<Valtype, Base>::SetLimits( const TrackJointLimits& wfl )
{
    m_TrackJoint.NormalForceLimits( 
        _N(wfl.NormalForceLimitMin) / Base::EngineKilogramsPerUnit() / Base::EngineMetersPerUnit(), 
        _N(wfl.NormalForceLimitMax) / Base::EngineKilogramsPerUnit() / Base::EngineMetersPerUnit() );
    m_TrackJoint.BinormalForceLimits(
        _N(wfl.BinormalForceLimitMin) / Base::EngineKilogramsPerUnit() / Base::EngineMetersPerUnit(),
        _N(wfl.BinormalForceLimitMax) / Base::EngineKilogramsPerUnit() / Base::EngineMetersPerUnit() );
    m_TrackJoint.NormalTorqueLimit( 
        _Nm(wfl.NormalTorqueLimit) / Base::EngineKilogramsPerUnit() / Base::EngineMetersPerUnit() / Base::EngineMetersPerUnit() );
    m_TrackJoint.TorqueLimit( 
        _Nm(wfl.TorqueLimit) / Base::EngineKilogramsPerUnit() / Base::EngineMetersPerUnit() / Base::EngineMetersPerUnit() );
    m_TrackJoint.ThresholdP( _m(wfl.ThresholdPosition) / Base::EngineMetersPerUnit() );
    m_TrackJoint.ThresholdT( wfl.ThresholdTangent );
    m_TrackJoint.ThresholdN( wfl.ThresholdNormal );
    m_TrackJoint.ThresholdB( wfl.ThresholdBinormal );
    m_TrackJoint.ErrorReductionParameter( wfl.ErrorReduction );
}
 
template<typename Valtype,class Base>
inline void TrackJointFeeder_Imp<Valtype,Base>::SetMotorForceLimits( Force min, Force max ) noexcept{
    if( Base::GetLocation().Orient() == Orientation::Value::anti ){
        const Force temp = min;
        min = -max;
        max = -temp;
    }
 
    m_TrackJoint.MotorForceLimits( 
        _N(min) / Base::EngineKilogramsPerUnit() / Base::EngineMetersPerUnit(), 
        _N(max) / Base::EngineKilogramsPerUnit() / Base::EngineMetersPerUnit() );
}
 
template<typename Valtype,class Base>
inline void TrackJointFeeder_Imp<Valtype,Base>::SetMotorTarget( Velocity vTarget ) noexcept{
    m_TrackJoint.MotorTarget( (Base::GetLocation().Orient() == Orientation::Value::para ? +1.0f : -1.0f) * _mIs(vTarget) / Base::EngineMetersPerUnit() );
}
 
template<typename Valtype,class Base>
inline Velocity TrackJointFeeder_Imp<Valtype,Base>::GetMotorTarget() noexcept
{
    return _mIs( (Base::GetLocation().Orient() == Orientation::Value::para ? +1.0f : -1.0f) * Base::EngineMetersPerUnit() * m_TrackJoint.MotorTarget() );
}
 
template<typename Valtype,class Base>
spat::Frame<Length,One> TrackJointFeeder_Imp<Valtype,Base>::GlobalAnchor() const
{
    return { 
        {   _m(Base::EngineMetersPerUnit() * m_TrackJoint.GlobalAnchor().P.x), 
            _m(Base::EngineMetersPerUnit() * m_TrackJoint.GlobalAnchor().P.y), 
            _m(Base::EngineMetersPerUnit() * m_TrackJoint.GlobalAnchor().P.z) },
            spat::spatial_cast<One>(m_TrackJoint.GlobalAnchor().T),
            spat::spatial_cast<One>(m_TrackJoint.GlobalAnchor().N),
            spat::spatial_cast<One>(m_TrackJoint.GlobalAnchor().B) 
    };
}
 
template<typename Valtype, class Base>
inline spat::Frame<Length,One> TrackJointFeeder_Imp<Valtype, Base>::Anchor() const noexcept
{
    return { 
        {   _m(Base::EngineMetersPerUnit() * m_TrackJoint.Anchor().P.x), 
            _m(Base::EngineMetersPerUnit() * m_TrackJoint.Anchor().P.y), 
            _m(Base::EngineMetersPerUnit() * m_TrackJoint.Anchor().P.z) },
            spat::spatial_cast<One>(m_TrackJoint.Anchor().T),
            spat::spatial_cast<One>(m_TrackJoint.Anchor().N),
            spat::spatial_cast<One>(m_TrackJoint.Anchor().B) 
    };
}
 
template<typename Valtype, class Base>
inline void TrackJointFeeder_Imp<Valtype, Base>::Anchor( const spat::Frame<Length, One>& frame ) noexcept
{
    m_TrackJoint.Anchor( { 
        {   _m(frame.P.x) / Base::EngineMetersPerUnit(), 
            _m(frame.P.y) / Base::EngineMetersPerUnit(), 
            _m(frame.P.z) / Base::EngineMetersPerUnit() },
            spat::spatial_cast<Valtype>(frame.T),
            spat::spatial_cast<Valtype>(frame.N),
            spat::spatial_cast<Valtype>(frame.B) 
    } );
}
 
template<typename Valtype,class Base>
void TrackJointFeeder_Imp<Valtype,Base>::TurnAnchor() const
{
    spat::Frame<Valtype> A = m_TrackJoint.Anchor();
    m_TrackJoint.Anchor( { A.P, -A.T, -A.N, A.B } );
    m_TrackJoint.MotorTarget( -m_TrackJoint.MotorTarget() );
    m_TrackJoint.MotorForceLimits( -m_TrackJoint.MotorForceMax(), -m_TrackJoint.MotorForceMin() );
}
 
template<typename Valtype,class Base>
inline void TrackJointFeeder_Imp<Valtype,Base>::TrackBodyChange( std::shared_ptr<const Body> pToBody ){
    m_TrackJoint.UpdateTrackBody( pToBody );
}
 
template<typename Valtype,class Base>
void TrackJointFeeder_Imp<Valtype,Base>::SetTimeStep( Time dt ){
    m_TrackJoint.SetTimeStep( dt );
}
 
template<typename Valtype,class Base>
inline void TrackJointFeeder_Imp<Valtype,Base>::UpdateBodies() const{
    m_TrackJoint.UpdateBodies();
}
 
template<typename Valtype,class Base>
bool TrackJointFeeder_Imp<Valtype,Base>::SetValues( const TrackData<Real>& trackData ){
    return m_TrackJoint.SetValues( trackData );
}
 
template<typename Valtype,class Base>
inline bool TrackJointFeeder_Imp<Valtype,Base>::DoIsMovableBodySleeping() const noexcept{
    return m_TrackJoint.IsMovableBodySleeping();
}
 
template<typename Valtype,class Base>
void TrackJointFeeder_Imp<Valtype,Base>::DoWakeUp() const{
    return m_TrackJoint.WakeUp();
}
 
template<typename Valtype, class Base>
inline void TrackJointFeeder_Imp<Valtype, Base>::DoRail()
{
    m_TrackJoint.SetDerailed( false );
}
 
template<typename Valtype,class Base>
inline void TrackJointFeeder_Imp<Valtype,Base>::DoDerail()
{
    m_TrackJoint.SetDerailed( true );
}
}