import com.pi4j.io.gpio.*;
import com.pi4j.util.CommandArgumentParser;
import com.pi4j.util.Console;

public class MotorPwmTest
{
    public static void main (String args[]){
        GpioController gpio = GpioFactory.getInstance();

        GpioPinPwmOutput pwm1 = gpio.provisionPwmOutputPin(RaspiPin.GPIO_01);
        // argument array to search in

        //GpioPinPwmOutput pwm2 = gpio.provisionPwmOutputPin(RaspiPin.GPIO_26);
        final GpioPinDigitalOutput pin2 = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_26, "MyLED", PinState.LOW);

        GpioPinPwmOutput pwm3 = gpio.provisionPwmOutputPin(RaspiPin.GPIO_23);
        final GpioPinDigitalOutput pin4 = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_24, "MyLED", PinState.LOW);

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

        // play 
        int j = 0;
        while (j < 10){
            // forward
            pin2.low();
            pin4.low();

            for(int i = 0; i < 1000; i++){
                pwm1.setPwm(i);
                pwm3.setPwm(i);
                sleep(10);
            }
            sleep(2000);

            // stop
            pwm1.setPwm(0);
            pwm3.setPwm(0);
            pin2.low();
            pin4.low();
            sleep(2000);

            // backward
            pin2.high();
            pin4.high();
            for(int i = 0; i < 1000; i++){
                pwm1.setPwm(1000-i);
                pwm3.setPwm(1000-i);
                sleep(10);
            }

            // stop
            pwm1.setPwm(0);
            pwm3.setPwm(0);
            pin2.low();
            pin4.low();
            sleep(2000);

            // turn left
            pin2.low();
            pin4.high();
            for(int i = 0; i < 1000; i++){
                pwm1.setPwm(i);
                pwm3.setPwm(1000-i);
                sleep(10);
            }
            
            // stop
            pwm1.setPwm(0);
            pwm3.setPwm(0);
            pin2.low();
            pin4.low();
            sleep(2000);
            
            // turn right
            pin2.high();
            pin4.low();
            for(int i = 0; i < 1000; i++){
                pwm1.setPwm(1000-i);
                pwm3.setPwm(i);
                sleep(10);
            }
            sleep(2000);           

            // stop
            pwm1.setPwm(0);
            pin2.low();
            pwm3.setPwm(0);
            pin4.low();
            j++;
            sleep(2000);
        }
        gpio.shutdown();
        gpio.unprovisionPin(new GpioPin[]{pwm1,pin2,pwm3,pin4});
    }

    public static void sleep(long s){
        try{
            Thread.sleep(s);
        } catch (Exception e) {
            // do nothing shit happends
        }
    }
}