VelocityHorizon.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-02.
// Contributors:
//
// VelocityHorizon.h


#ifndef SIMDRIVER_VELOCITYHORIZON_H
#define SIMDRIVER_VELOCITYHORIZON_H

#include <cmath>
#include <deque>
#include <algorithm>
#include "model_collection.h"


namespace agent_model {


    class VelocityHorizon {

    protected:

        struct PredictionPoint {
            size_t i;     
            double s;     
            double ds;    
            double vRule; 
            double vCont; 
            double sCont; 
        };

        double _offset;
        double _vMax;

        std::deque <PredictionPoint> _elements{};



    public:


        void init(double offset, unsigned int noOfElements) {

            // set offset
            _offset = std::floor(offset);

            // reset elements
            if (_elements.size() > 0) _elements.clear();

            // create points
            for (size_t i = 0; i < noOfElements; ++i)
                _elements.emplace_back(newPoint(i));

        }


        void update(double s) {

            size_t i0 = 0; // first element with positive distance

            for (auto &e : _elements) {

                // recalculate distance
                e.ds = e.s - s;

                // check distance and increment if <= 0
                if(e.ds <= 0.0)
                    i0++;
            }

            // get reference index of the last element
            size_t ib = _elements.back().i;

            // remove old element
            for (size_t i = 0; i + 1 < i0; ++i) {

                // new index
                size_t i1 = ib + i + 1;

                // remove from front, add to the back
                _elements.pop_front();
                _elements.emplace_back(newPoint(i1));

            }

        }


        unsigned int getIndexBefore(double s) {

            double s0 = _elements.at(0).s;

            if(s <= s0)
                return 0.0;
            if(s >= _elements.back().s)
                return _elements.size() - 1;

            return (unsigned int) std::floor(s - s0);

        }


        unsigned int getIndexAfter(double s) {

            double s0 = _elements.at(0).s;

            if(s <= s0)
                return 0.0;
            if(s >= _elements.back().s)
                return _elements.size() - 1;

            return (unsigned int) std::ceil(s - s0);

        }


        void setMaxVelocity(double v) {

            _vMax = v;

        }


        void resetSpeedRule() {

            for(auto &e : _elements)
                e.vRule = INFINITY;

        }


        void updateSpeedRuleInInterval(double s0, double s1, double v) {

            auto i0 = getIndexBefore(s0);
            auto i1 = getIndexAfter(s1);

            for (unsigned int i = i0; i <= i1; ++i) {

                // get element
                auto &e = _elements.at(i);

                // set speed if speed is smaller and point in interval
                if(e.vRule > v)
                    e.vRule = v;

            }

        }


        void updateContinuousPoint(double s, double v) {

            // get index before position
            auto i = getIndexAfter(s);
            auto &e = _elements.at(i);

            if(s > e.sCont) {

                e.sCont = s;
                e.vCont = v;

            }

        }



        double mean(double s0, double s1, double delta = 1.0) {

            // instantiate
            double v = 0.0;
            double vMin = INFINITY;
            double j = 0;

            // get indexes
            auto i0 = getIndexBefore(s0);
            auto i1 = getIndexAfter(s1);

            for (unsigned int i = i0; i <= i1; ++i) {

                // get speed and s
                auto v0 = (std::min)(vMin, getSpeedAt(i));
                auto s = _elements.at(i).s;

                // sum up with scaled factor
                auto f = agent_model::scale(s, s1, s0, delta);
                v += f * v0;

                // set minimum for future
                if(v0 < vMin)
                    vMin = v0;

                // divisor
                j += f;

            }

            return v / (double) j;

        }



    protected:


        double getSpeedAt(unsigned int i) {

            // get speed at index
            auto &e = _elements.at(i);
            return (std::min)((std::min)(e.vCont, e.vRule), _vMax);

        }


        PredictionPoint newPoint(size_t i) {

            double s = _offset + (double) i;
            return PredictionPoint{i, s, INFINITY, INFINITY, INFINITY, s - 1.0};

        }


    };


}


#endif //SIMDRIVER_VELOCITYHORIZON_H