var five = require("johnny-five");
var temporal = require("temporal");

var opts = {};
opts.port = process.argv[2] || "";

var board = new five.Board(opts);

board.on("ready", function() {

    console.log("Robot będzie poruszał się do przodu, dopóki nie wykryje przeszkody.");
    console.log("Wtedy cofnie się, zmieni kierunek i będzie dalej poruszał się do przodu.");

    var left_wheel  = new five.Servo({ pin:  9, type: 'continuous' });
    var right_wheel = new five.Servo({ pin: 8, type: 'continuous'  });
    var ping = new five.Ping({pin: 7}); //<1>
    var state = "DRIVING";
    forward();

    ping.on("change", function() { //<2>
        if (this.cm < 15 && state == "DRIVING") {
            state = "AVOID_COLLISION";
            temporal.queue([ //<3>
                {
                    // ostateczne zatrzymanie
                    delay: 10,
                    task: stop,
                },
                {
                    // cofnięcie
                    delay: 3000,
                    task: backward,
                },
                {
                    // zatrzymanie
                    delay: 1500,
                    task: stop,
                },
                {
                    // obrót
                    delay: 3000,
                    task: spin, 
                },
                {
                    // zatrzymanie
                    delay: 1000,
                    task: stop,
                },
                {
                    // do przodu
                    delay: 3000,
                    task: forward, //<4>
                },
            ])
        }
    });

    // pomocnicze funkcje sterujące
    function forward() {
        console.log("do przodu");
        state = "DRIVING";
        left_wheel.cw();
        right_wheel.ccw();
    }

    function backward() {
        console.log("do tyłu");
        left_wheel.ccw();
        right_wheel.cw();
    }

    function stop() {
        console.log("stop");
        left_wheel.to(92);
        right_wheel.to(94);
    }

    function spin() {
        console.log("obrót");
        left_wheel.cw();
        right_wheel.cw();
    }
});
