import com.pi4j.io.gpio.*;
public class Drive
{
    public static final int STOP = 0;
    private int pwmMax = 1000;

    private GpioPinPwmOutput leftPwm;
    private GpioPinPwmOutput rightPwm;

    private GpioPinDigitalOutput leftDir;
    private GpioPinDigitalOutput rightDir;

    private int speedMin = 200; // with smaller values the engines are not spinning
    private int speedMax = pwmMax-speedMin;
    
    public int getSpeedMax(){
        return speedMax;
    }

    public Drive (Pin leftPwmPin, Pin leftDirPin, Pin rightPwmPin, Pin rightDirPin){
        GpioController gpio = GpioFactory.getInstance();
        
        leftPwm = gpio.provisionPwmOutputPin(leftPwmPin);
        rightPwm = gpio.provisionPwmOutputPin(rightPwmPin);

        com.pi4j.wiringpi.Gpio.pwmSetMode(com.pi4j.wiringpi.Gpio.PWM_MODE_MS);
        com.pi4j.wiringpi.Gpio.pwmSetRange(pwmMax);
        com.pi4j.wiringpi.Gpio.pwmSetClock(500);

        leftDir =  gpio.provisionDigitalOutputPin(leftDirPin, "Left Direction", PinState.LOW);
        rightDir =  gpio.provisionDigitalOutputPin(rightDirPin, "Right Direction", PinState.LOW);

    }
    public void setMinSpeed(int speed) throws Exception{
        if(speed <= STOP){
            throw new Exception("Min Speed to small!");
        }else if (speed > pwmMax){
            throw new Exception("Min Speed to big!");
        }

        speedMin = speed;
        speedMax = pwmMax-speedMin; 
    }

    public void turnLeft(int speed)throws Exception{
        int half = speed/2;// turn speed should be half of the speed 
        leftBackward(half);
        rightForward(half);

    }

    public void turnRight(int speed)throws Exception{
        int half = speed/2; // turn speed should be half of the speed
        leftForward(half);
        rightBackward(half);

    }

    public void backward(int speed)throws Exception{
        leftBackward(speed);
        rightBackward(speed);

    }

    public void forward(int speed)throws Exception{
        leftForward(speed);
        rightForward(speed);

    }

    public void leftForward(int speed)throws Exception{
        stopLeft();
        checkSpeed(speed);
        leftDir.low();
        leftPwm.setPwm(speed+speedMin);
    }

    public void rightForward(int speed)throws Exception{
        stopRight();
        checkSpeed(speed);
        rightDir.low();
        rightPwm.setPwm(speed+speedMin);
    }

    public void leftBackward(int speed)throws Exception{
        stopLeft();
        checkSpeed(speed);
        leftDir.high();
        leftPwm.setPwm(pwmMax- speed-speedMin);

    }

    public void rightBackward(int speed)throws Exception{
        stopRight();
        checkSpeed(speed);
        rightDir.high();
        rightPwm.setPwm(pwmMax-speed-speedMin);

    }

    public void stopLeft()throws Exception{
        leftPwm.setPwm(STOP);
        leftDir.low();
    }

    public void stopRight()throws Exception{
        rightPwm.setPwm(STOP);
        rightDir.low();
    }

    public void stop()throws Exception{
        stopLeft();
        stopRight();
    }

    public void release(){
        GpioController gpio = GpioFactory.getInstance();
        gpio.unprovisionPin(new GpioPin[]{leftPwm,rightPwm,leftDir,rightDir});
    }

    public boolean checkSpeed(int speed) throws Exception{
        if(speed> speedMax){
            throw new Exception("Speed value to high! " + speed +" >"+ speedMax);
        } else if(speed < STOP){
            throw new Exception("Speed value to low!");            
        }
        return true;
    }

}