package no.itera.lego;

import java.lang.reflect.Field;

import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.motor.EV3MediumRegulatedMotor;
import lejos.hardware.motor.UnregulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.EV3IRSensor;
import lejos.robotics.Color;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;

public class Helpers {

  private static final int DISTANCE_DEGREES_FACTOR = 36;
  private static final double ROTATE_DEGREES_FACTOR = 11.3;

  private RegulatedMotor motorRight;
  private RegulatedMotor motorLeft;
  private RegulatedMotor motorCannon;
  private EV3IRSensor irSensor;
  private EV3ColorSensor colorSensor;

  private SampleProvider rangeSampler;
  private float[] lastRange;

  private String[] colors;

  private enum Direction{
    FORWARD, BACKWARD;
  }

  public Helpers() {
    motorRight = new EV3LargeRegulatedMotor(MotorPort.A);
    motorLeft = new EV3LargeRegulatedMotor(MotorPort.B);
    motorCannon = instantiateMotorCannon();
    irSensor = new EV3IRSensor(SensorPort.S1);
    colorSensor = new EV3ColorSensor(SensorPort.S4);

    rangeSampler = irSensor.getDistanceMode();
    lastRange = new float[rangeSampler.sampleSize()];

    colors = getColors();
  }

  public RegulatedMotor getMotorRight() {
    return motorRight;
  }

  public RegulatedMotor getMotorLeft() {
    return motorLeft;
  }

  public RegulatedMotor getMotorCannon() {
    return motorCannon;
  }

  public EV3IRSensor getIrSensor() {
    return irSensor;
  }

  public EV3ColorSensor getColorSensor() {
    return colorSensor;
  }

  public float getDistance() {
    rangeSampler.fetchSample(lastRange, 0);
    return lastRange[0];
  }

  public String getColorName() {
    int color = colorSensor.getColorID();
    if (color < 0 || color >= colors.length) {
      return "NO_COLOR";
    }
    return colors[color];
  }

  private String[] getColors() {
    Field[] names = Color.class.getFields();
    String[] list = new String[names.length];
    for (int i = 0; i < list.length; i++) {
      list[i] = names[i].getName();
    }
    return list;
  }

  public void forward(int cm){
    drive(cm, Direction.FORWARD);
  }

  public void backward(int cm){
    drive(cm, Direction.BACKWARD);
  }

  private void drive(int cm, Direction direction){
    if(direction == Direction.BACKWARD){
      cm *= -1;
    }
    motorLeft.rotate(cm * DISTANCE_DEGREES_FACTOR, true);
    motorRight.rotate(cm * DISTANCE_DEGREES_FACTOR);
  }

  public void turnLeft(int degrees) {
    motorLeft.stop(true);
    motorRight.rotate((int)(ROTATE_DEGREES_FACTOR * degrees));
  }

  public void turnRight(int degrees) {
    motorRight.stop(true);
    motorLeft.rotate((int)(ROTATE_DEGREES_FACTOR * degrees));
  }

  private EV3MediumRegulatedMotor instantiateMotorCannon() {
    calibrateMotorCannon();
    EV3MediumRegulatedMotor motor = new EV3MediumRegulatedMotor(MotorPort.C);
    motor.rotate(-360);
    return motor;
  }

  private void calibrateMotorCannon() {
    UnregulatedMotor motor = new UnregulatedMotor(MotorPort.C);
    motor.setPower(50);

    int maxIterations = 40;
    int lastTachoCount = 0;
    while (maxIterations-- > 0) {
      Delay.msDelay(100);
      int newTachoCount = motor.getTachoCount();
      if (newTachoCount == lastTachoCount) {
        break;
      }
      lastTachoCount = newTachoCount;
    }

    motor.close();
  }
}

@@ -991,24 +991,101 @@
 Helpers() %7B%0A
+    this(false);%0A  %7D%0A%0A  public Helpers(boolean skipMotorCannonCalibration) %7B%0A
     motorRig
@@ -1224,24 +1224,50 @@
 MotorCannon(
+skipMotorCannonCalibration
 );%0A    irSen
@@ -3047,28 +3047,103 @@
 MotorCannon(
-) %7B%0A
+boolean skipMotorCannonCalibration) %7B%0A    if (!skipMotorCannonCalibration) %7B%0A  
     calibrat
@@ -3154,24 +3154,30 @@
 orCannon();%0A
+    %7D%0A
     EV3Mediu
@@ -3234,32 +3234,73 @@
 r(MotorPort.C);%0A
+    if (!skipMotorCannonCalibration) %7B%0A  
     motor.rotate
@@ -3303,24 +3303,30 @@
 tate(-360);%0A
+    %7D%0A
     return m

Make calibration of cannon motor optional