# include<Arduino.h>
#include<fahrt.h>
#include<locomotive.h>

Fahrt::Fahrt(Locomotive locomotive, MotorDirection Direction, Gleis gleis, Gleisabschnitt gleisabschnitt): _locomotive(locomotive), _gleis(gleis), _gleisabschnitt(gleisabschnitt)
{
    _direction = Direction;
    _speed = locomotive.getDefaultSpeed();
    _bremsweg=0;
}

Fahrt::Fahrt(Locomotive locomotive, byte speed, MotorDirection Direction, Gleis gleis, Gleisabschnitt gleisabschnitt): _locomotive(locomotive), _gleis(gleis), _gleisabschnitt(gleisabschnitt)
{
    _direction = Direction;
    _speed = speed;
    _bremsweg=0;
}

Fahrt::Fahrt(Locomotive locomotive, byte speed, MotorDirection Direction, Gleis gleis, Gleisabschnitt gleisabschnitt, float bremsweg): _locomotive(locomotive), _gleis(gleis), _gleisabschnitt(gleisabschnitt)
{
    _direction = Direction;
    _speed = speed;
    _bremsweg=bremsweg;
}


void Fahrt::Vorbereiten()
{
    _gleis.Waehlen();
    _abschnittErkannt=false;
    Serial.print ("Auftrag: Speed=");
    Serial.print (_speed);
    Serial.print ("  Bremsweg=");
    Serial.print (_bremsweg);
    Serial.println();
}

void Fahrt::Start()
{
    _locomotive.setDirection(_direction);
    _locomotive.Accelerate(_speed);
}

void Fahrt::Loop()
{
    _locomotive.loop();
    if (_gleisabschnitt.IstBesetzt() && !_abschnittErkannt) 
    {

        delay(20);
        if (_gleisabschnitt.IstBesetzt() && !_abschnittErkannt) 
        {
            if (_bremsweg==0)
                _locomotive.Break();
            else    
                _locomotive.Accelerate(0, _bremsweg);
            _abschnittErkannt=true;
        }
    }
}

bool Fahrt::IstAngekommen()
{
    return (_locomotive.currentSpeed()==0);
}

float Fahrt::ChangeBremsweg(float DeltaBremsweg)
{
    _bremsweg += DeltaBremsweg;
    return(_bremsweg);
}

byte Fahrt::ChangeSpeed(byte DeltaSpeed)
{
    _speed += DeltaSpeed;
    return(_speed);
}