#include "IMotor.h"
#include <L298n.h>
#include "Arduino.h"


    L298n::L298n(int pinPwm, int pin1, int pin2) 
        {
            _pinPwm = pinPwm;
            _pin1 = pin1;
            _pin2 = pin2;
            _currentDir = motorForward;
            
            pinMode(_pinPwm,OUTPUT);
            pinMode(_pin1,OUTPUT);
            pinMode(_pin2,OUTPUT);
            digitalWrite(_pin1, LOW);
            digitalWrite(_pin2, LOW);

        }

    void L298n::setSpeed(int speed) 
        {
/*            Serial.print("Regler Speed:" );
            Serial.print(speed);
                    Serial.print(" on ");
                    Serial.println(_pinPwm); */
            analogWrite(_pinPwm, speed);
            setDirection(_currentDir);

        }

    void L298n::setDirection(MotorDirection dir) 
        {
            _currentDir=dir;
            if (_currentDir == motorBackward)
            {
/*                    Serial.print("Regler backward");
                    Serial.print(_pin1);
                    Serial.print("-");
                    Serial.println(_pin2); */
                digitalWrite(_pin2, LOW);
                digitalWrite(_pin1, HIGH);
            }
            else
            {
/*                    Serial.print("Regler forward");
                    Serial.print(_pin1);
                    Serial.print("-");
                    Serial.println(_pin2); */
                digitalWrite(_pin1, LOW);
                digitalWrite(_pin2, HIGH);
            }
        }

    void L298n::Stop() 
        {
            digitalWrite(_pin1, LOW);
            digitalWrite(_pin2, LOW);
            setSpeed (0);
        }