Interface.h

// Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
// 
// Created by Jens Klimke on 2020-04-07.
//


#ifndef AGENT_MODEL_INTERFACE_H
#define AGENT_MODEL_INTERFACE_H


namespace agent_model {

    static const unsigned int NOT = 32; 
    static const unsigned int NOL = 32; 
    static const unsigned int NOS = 32; 
    static const unsigned int NOH = 32; 
    static const unsigned int NORP = 2; 
    static const unsigned int NOCP = 3; 
    static const unsigned int NOA = 32; 

    static const unsigned int NOTL = 32; 


    enum Accessibility { ACC_NOT_SET, ACC_ACCESSIBLE, ACC_NOT_ALLOWED, ACC_NOT_ACCESSIBLE };

    enum DrivingDirection { DD_FORWARDS, DD_BACKWARDS, DD_BOTH, DD_NONE, DD_NOT_SET };

    enum SignalType { SIGNAL_NOT_SET, SIGNAL_STOP, SIGNAL_TLS, SIGNAL_SPEED_LIMIT, SIGNAL_YIELD, SIGNAL_PRIORITY };
 //For now just colors.Todo: Add different icon color combinations
    enum TrafficLightColor { COLOR_RED, COLOR_YELLOW, COLOR_GREEN }; 
 //For now just colors.Todo: Add different icon color combinations
    enum TrafficLightIcon { ICON_OTHER, ICON_NONE, ICON_ARROW_STRAIGHT_AHEAD, ICON_ARROW_LEFT, ICON_ARROW_RIGHT }; 

    enum TargetPriority { TARGET_ON_PRIORITY_LANE, TARGET_ON_GIVE_WAY_LANE, TARGET_PRIORITY_NOT_SET };

    enum TargetPosition { TARGET_NOT_RELEVANT, TARGET_ON_PATH, TARGET_ON_INTERSECTION, TARGET_ON_OPPOSITE, TARGET_ON_RIGHT, TARGET_ON_LEFT };

    enum Maneuver { STRAIGHT, TURN_LEFT, TURN_RIGHT };



    struct Position {
        double x; 
        double y; 
        Position(): x(0.0), y(0.0) {}
        Position(double pX, double pY) : x(pX), y(pY) {}
        
        bool operator==(const Position& other)
        {
            return x == other.x && y == other.y;
        }
    };
    


    struct DynamicPosition {
        double x; 
        double y; 
        double dx; 
        double dy; 
    };

    struct Point {
        double distance; 
        double time; 
        double value; 
    };

    struct Dimensions {
        double width; 
        double length; 
    };

    struct VehicleState {
        double v; 
        double a; 
        double psi; 
        double dPsi; 
        double s; 
        double d; 
        double pedal; 
        double steering; 
        Maneuver maneuver; 
        double dsIntersection; 

    };

    struct Horizon {
        double ds[NOH]; 
        double x[NOH]; 
        double y[NOH]; 
        double psi[NOH]; 
        double kappa[NOH]; 
        double egoLaneWidth[NOH]; 
        double rightLaneOffset[NOH]; 
        double leftLaneOffset[NOH]; 
        double destinationPoint; 
    };

    struct Lane {
        int id; 
        double width; 
        double route; 
        double closed; 
        DrivingDirection dir; 
        Accessibility access; 
        int lane_change; 
                        // not defined (-1)
                        // not necessary (0) - already on target lane
                        // intended (1) - necessary, but also later possibility
                        // required (2) - last chance
    };

    struct ControlPath {
        double offset; 
        double factor; 
        DynamicPosition refPoints[NORP]; 
    };

    struct FollowTarget {
        double factor; 
        double lane; 
        double distance; 
        double velocity; 
    };

    struct Signal {
        unsigned int id; 
        double ds; 
        SignalType type; 
        double value; 
        TrafficLightColor color; 
        TrafficLightIcon icon; 
        bool subsignal;     
        bool sign_is_in_use;   
    };


    struct Target {
        unsigned int id; 
        double ds; 
        Position xy; 
        double v; 
        double a; 
        double d; 
        double psi; 
        int lane; 
        Dimensions size; 
        double dsIntersection; 
        TargetPriority priority; 
        TargetPosition position; 
    };

    struct DecisionStopping {
        unsigned int id; 
        double position; 
        double standingTime; 
    };

    struct Decisions {
        int laneChangeInt; 
        int laneChangeDec; 
        Point lateral; 
        DecisionStopping signal; 
        DecisionStopping target; 
        DecisionStopping destination; 
        DecisionStopping lane; 
    };

    struct ConsciousVelocity {
        double local; 
        double prediction; 
    };

    struct ConsciousStop {
        double ds; 
        double dsMax; 
        bool standing; 
        bool priority; 
        bool give_way; 
    };

    struct ConsciousFollow {
        FollowTarget targets[2];
        bool standing; 
    };

    struct ConsciousLateral {
        ControlPath paths[NOCP]; 
    };

    struct Conscious {
        ConsciousVelocity velocity; 
        ConsciousStop stop; 
        ConsciousFollow follow; 
        ConsciousLateral lateral; 
    };

    struct Subconscious {
        double a; 
        double dPsi; 
        double kappa; 
        double pedal; 
        double steering; 
    };

    struct MemoryVehicle {
        double s; 
    };

    struct MemoryLateral {
        double time; 
        double startTime; 
        double distance; 
        double startDistance; 
        double offset; 
    };

    struct MemoryLaneChange {
        int switchLane; 
        int decision; 
        double startTime; 
    };

    struct ParameterVelocityControl {
        double thwMax; 
        double delta; 
        double deltaPred; 
        double a; 
        double b; 
        double vScale; 
        double ayMax; 
        double vComfort; 
    };

    struct ParameterFollowing {
        double timeHeadway; 
        double dsStopped; 
        double thwMax; 
    };

    struct ParameterVehicle {
        Dimensions size; 
        Position pos; 
    };

    struct ParameterSteering {
        double thw[NORP]; 
        double dsMin[NORP]; 
        double P[NORP]; 
        double D[NORP]; 
    };

    struct ParameterStopping {
        double dsGap; 
        double TMax; 
        double dsMax; 
        double T; 
        double tSign; 
        double vStopped; 
        double pedalDuringStanding; 
    };

    struct ParameterLaneChange {
        double bSafe; 
        double aThreshold; 
        double politenessFactor; 
        double time; 
    };

    struct Input {
        VehicleState vehicle; 
        Horizon horizon; 
        Signal signals[NOS]; 
        Lane lanes[NOL]; 
        Target targets[NOT]; 
    };

    struct State {
        double simulationTime; 
        Decisions decisions; 
        Conscious conscious; 
        Subconscious subconscious; 
        double aux[NOA]; 
    };

    struct Memory {
        MemoryVehicle vehicle; 
        double velocity; 
        MemoryLateral lateral; 
        MemoryLaneChange laneChange; 
    };

    struct Parameters {
        ParameterVehicle vehicle; 
        ParameterLaneChange laneChange; 
        ParameterStopping stop; 
        ParameterVelocityControl velocity; 
        ParameterFollowing follow; 
        ParameterSteering steering; 
    };


class Interface {

public:

    typedef agent_model::Input Input;
    typedef agent_model::State State;
    typedef agent_model::Memory Memory;
    typedef agent_model::Parameters Parameters;


protected:

    Input _input{}; 
    State _state{}; 
    Memory _memory{}; 
    Parameters _param{}; 

public:

    Interface() = default;

    virtual ~Interface() = default;


    Input *getInput() {
        return &_input;
    }

    const Input *getInput() const {
        return &_input;
    }


    State *getState() {
        return &_state;
    }

    const State *getState() const {
        return &_state;
    }


    Memory *getMemory() {
        return &_memory;
    }

    const Memory *getMemory() const {
        return &_memory;
    }


    Parameters *getParameters() {
        return &_param;
    }

    const Parameters *getParameters() const {
        return &_param;
    }



};

} // namespace

#endif // AGENT_MODEL_INTERFACE_H