package net.catchpole.B9.devices.thrusters;

import net.catchpole.B9.devices.esc.BlueESCData;
import net.catchpole.B9.devices.gps.SimulationGps;
import net.catchpole.B9.math.DistanceCalculator;
import net.catchpole.B9.math.HeadingCalculator;
import net.catchpole.B9.math.Normalise;
import net.catchpole.B9.spacial.Heading;

import java.util.Random;

public class SimulationThrusters implements Thrusters {
    private HeadingCalculator headingCalculator = new HeadingCalculator();
    private DistanceCalculator distanceCalculator = new DistanceCalculator();

    private final Random random = new Random();
    private final SimulationGps simulationGps;
    private final double kilometersPerSecond;
    private final int errorDegrees;

    private double left;
    private double right;

    public SimulationThrusters(SimulationGps simulationGps, double kilometersPerSecond, int errorDegrees) {
        this.simulationGps = simulationGps;
        this.kilometersPerSecond = kilometersPerSecond;
        this.errorDegrees = errorDegrees;
    }

    public void update(double left, double right) {
        this.left = left;
        this.right = right;
    }

    public void adviseNewHeading(Heading heading) {
        double error = errorDegrees == 0 ? 0 : random.nextInt(errorDegrees) - (errorDegrees/2);
        simulationGps.setHeading(new Heading(Normalise.degrees(heading.getDegrees() + error)));
    }

    public void oneSecondTick() {
        if (left != 0.0 && right != 0.0) {
            double kps = kilometersPerSecond * ((left + right) / 2.0d);
            simulationGps.setLocation(
                    headingCalculator.getLocation(simulationGps.getLocation(), simulationGps.getHeading(),
                            distanceCalculator.kilometersToDegrees(kps))
            );
        }
    }

    public BlueESCData getLeftData() {
        return getData(left);
    }

    public BlueESCData getRightData() {
        return getData(right);
    }

    private BlueESCData getData(double throttle) {
        return new BlueESCData((int)(100.0d * throttle), 12.1 - throttle, 22 + (throttle * 4), throttle * 10);
    }
}
@@ -729,16 +729,52 @@
 Degrees;
+%0A    private final double steerBias;
 %0A%0A    pr
@@ -923,16 +923,34 @@
 rDegrees
+, double steerBias
 ) %7B%0A    
@@ -1083,24 +1083,60 @@
 rorDegrees;%0A
+        this.steerBias = steerBias;%0A
     %7D%0A%0A    p
@@ -1236,16 +1236,44 @@
  right;%0A
+        adviseNewHeading();%0A
     %7D%0A%0A 
@@ -1272,29 +1272,30 @@
     %7D%0A%0A    p
-ublic
+rivate
  void advise
@@ -1309,26 +1309,212 @@
 ing(
-Heading heading) %7B
+) %7B%0A        double steer = left - right;%0A        double steerDegrees = steer * steerBias;%0A        Heading heading = new Heading( Normalise.degrees(simulationGps.getHeading().getDegrees() + steerDegrees));
 %0A   

convert adviseNewHeading() in SimulationThrusters to automatic calculation on update