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

Locomotive::Locomotive(float accelerate, float decelerate, byte minSpeedStart, byte minSpeedStop, IMotor& motor): _motor(motor)
{
    _currentSpeed=0;
    _currentAcc=0;
    _accelerate=accelerate;
    _decelerate=decelerate;
    _minSpeedStart=minSpeedStart;
    _minSpeedStop=minSpeedStop;

};

void Locomotive::Accelerate(byte Speed)
{
/*      Serial.print("Locomotive.Accelerate: ");
        Serial.print(_currentSpeed);
        Serial.print(" ");
        Serial.print(Speed);
        Serial.print(" ");
*/ 
    Serial.print("Accelerate: currentspeed=");
    Serial.print((_currentSpeed));
    Serial.print(" targetspeed=");
    Serial.print(Speed);

    _targetSpeed = Speed;
    _millisStart = millis()-_delay;
    if (_targetSpeed>_currentSpeed)
        _currentAcc = _accelerate;
    else
        _currentAcc = - _decelerate;

    Serial.print(" _currentAcc=");
    Serial.print(_currentAcc);
    Serial.println();

};


void Locomotive::Accelerate(byte Speed, float bremsweg)
{
    Serial.print("Accelerate: currentspeed=");
    Serial.print((_currentSpeed));
    Serial.print(" targetspeed=");
    Serial.print(Speed);
    Serial.print(" bremsweg=");
    Serial.print((bremsweg));

    _targetSpeed = Speed;
    _currentSpeed = (_currentSpeed<_minSpeedStart)? _minSpeedStart : _currentSpeed;
    _millisStart = millis()-_delay;

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

    Serial.print(" targetspeed2=");
    Serial.print(_targetSpeed2);
    Serial.print(" _currentAcc=");
    Serial.print(_currentAcc);
    Serial.println();
};

void Locomotive::setSpeed(float 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 = _minSpeedStop;

        if ((newspeed>_targetSpeed) && (_currentAcc>0))
        {
            newspeed = _targetSpeed;
            _currentAcc =0;
        }
        else if ((newspeed<_minSpeedStart) && (_currentAcc>0)) newspeed = _minSpeedStart;
        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.
    }

};

float Locomotive::currentSpeed()
{
    return (_currentSpeed);
};