var five = require("johnny-five"),
    board = new five.Board(),
    PID = require('pid-controller');

board.on("ready", function() {
  console.log('ready');
  var sampleRate = 10;

  /////////////////////////////////////////
  // CONFIGURE SERVOS AND PING SENSORS
  var head = new five.Servo(6);

  var wheels = {
    left: new five.Servo({ pin: 10, type: 'continuous' }),
    right: new five.Servo({ pin: 9, type: 'continuous' })
  };

  var walls = {
    left: new five.Ping(13),
    front: new five.Ping(11),
    right: new five.Ping(12)
  };
  
  var ctrl = new PID(0, 0, 1, 10, 0, PID.DIRECT);

  /////////////////////////////////////////
  // ACTIONS

  var actions = {

    forward: function(center) { 
      var speed = {};

      if(center < 0) {
        console.log('<<<<');
        speed = { left: 0.035, right: 0.045 };
      } else if (center > 0) {
        console.log('>>>>');
        speed = { left: 0.045, right: 0.035 };
      } else {
        console.log('||||');
        speed = { left: 0.04, right: 0.04 }
      }

      wheels.left.cw(speed.left);
      wheels.right.ccw(speed.right);
    },

    stop: function() {
      wheels.left.center();
      wheels.right.center();
      head.to(60);
    }
  }

  ///////////////////////
  // READINGS

  function getSetpoint() {
    return 4; // inches
  }

  function readInput() {
    return walls.left.inches;
  }

  //////////////////////
  // GO!

  function init() {
    console.log('init')
    ctrl.setMode(PID.AUTOMATIC);
    ctrl.setSampleTime(sampleRate);

    actions.stop();
  }

  function loop() {
    ctrl.setPoint(getSetpoint());
    ctrl.setInput(readInput());
    ctrl.compute();

    actions.forward(ctrl.myOutput - 1);
  }

  board.repl.inject({
    head: head,
    wheels: wheels,
    walls: walls,
    ctrl: ctrl,
    actions: actions
  });

  init();
  setInterval(loop, sampleRate);

});