#include<locomotive.h>
#include<motor.h>
#include<math.h>

Locomotive::Locomotive(int accelerate, int decelerate, byte minSpeed, byte maxSpeed, IMotor& motor): _motor(motor)
{
    _currentSpeed=0;
    _currentAcc=0;
    _accelerate=accelerate;
    _decelerate=decelerate;
    _minSpeed=minSpeed;
    _maxSpeed=maxSpeed;

};

void Locomotive::Accelerate()
{
    Accelerate(_maxSpeed);
};

void Locomotive::Accelerate(byte Speed)
{
/*      Serial.print("Locomotive.Accelerate: ");
        Serial.print(_currentSpeed);
        Serial.print(" ");
        Serial.print(Speed);
        Serial.print(" ");
*/ 
    _targetSpeed = Speed;
    _millisStart = millis()-_delay;
    if (_targetSpeed>_currentSpeed)
        _currentAcc = _accelerate;
    else
        _currentAcc = - _decelerate;

};


void Locomotive::Accelerate(byte Speed, float bremsweg)
{
    _targetSpeed = Speed;
    _currentSpeed = (_currentSpeed<_minSpeed)? _minSpeed : _currentSpeed;
    _millisStart = millis()-_delay;

    byte _targetSpeed2 = ((_targetSpeed < _currentSpeed)&&(_targetSpeed == 0)) ? _minSpeed : _targetSpeed;
    int deltaspeed = (_targetSpeed2 - _currentSpeed);
    
    _currentAcc = copysign(deltaspeed * deltaspeed / (100.0 * bremsweg), deltaspeed);
    if (_currentAcc == 0)
        _currentAcc = copysign(1, deltaspeed);

};


void Locomotive::Break()
{
    Accelerate(0);
};

void Locomotive::setSpeed(byte Speed)
{
    _currentSpeed = Speed;
    _motor.setSpeed( (int) (_currentSpeed+0.5));
};

void Locomotive::setDirection(MotorDirection dir)
{
    Serial.print("Lokomotive Richtung: ");
    Serial.println(dir);
    _motor.setDirection(dir);
};

void Locomotive::loop()
{
    unsigned long currentMillis;
    currentMillis = millis();
    if (currentMillis - _millisStart >= _delay)  //test whether the period has elapsed
    {
        float newspeed;
        newspeed = _currentSpeed + _currentAcc;
 /*       Serial.print("Locomotive.Loop: ");
        Serial.print(_currentSpeed);
        Serial.print(" ");
        Serial.print(_currentAcc);
        Serial.print(" ");
        Serial.println(newspeed);
*/

        int breakspeed = _minSpeed * 80 / 100;

        if ((newspeed>_targetSpeed) && (_currentAcc>0))
        {
            newspeed = _targetSpeed;
            _currentAcc =0;
        }
        else if ((newspeed<_minSpeed) && (_currentAcc>0)) newspeed = _minSpeed;
        else if ((newspeed<breakspeed) && (_currentAcc<0))
        {
            newspeed = 0;
            _currentAcc = 0;
        };

        setSpeed(newspeed);

        _millisStart = currentMillis;  //IMPORTANT to save the start time of the current LED state.
    }

};

byte Locomotive::currentSpeed()
{
    return ((byte) (_currentSpeed+0.5));
};

byte Locomotive::getDefaultSpeed()
{
    return _maxSpeed;
};