package me.drton.jmavsim;

import me.drton.jmavlib.geo.GlobalPositionProjector;
import me.drton.jmavlib.processing.DelayLine;

import javax.vecmath.Matrix3d;
import javax.vecmath.Vector3d;

/**
 * User: ton Date: 27.11.13 Time: 19:06
 */
public class SimpleSensors implements Sensors {
    private DynamicObject object;
    private GlobalPositionProjector globalProjector = new GlobalPositionProjector();
    private DelayLine<GNSSReport> gpsDelayLine = new DelayLine<GNSSReport>();
    private long gpsStartTime = 0;
    private long gpsInterval = 200;
    private long gpsLast = 0;
    private GNSSReport gps = new GNSSReport();
    private boolean gpsUpdated = false;
    private double pressureAltOffset = 0.0;

    @Override
    public void setObject(DynamicObject object) {
        this.object = object;
        globalProjector.init(object.getWorld().getGlobalReference());
    }

    public Vector3d addZeroMeanNoise(Vector3d vIn, double var) {
        double x0;
        double b0, b1;

        do {
            b0 = Math.random();
            b1 = Math.random();
        } while (b0 <= Float.intBitsToFloat(0x1));

        x0 = java.lang.Math.sqrt(-2.0 * java.lang.Math.log(b0)) * java.lang.Math.cos(Math.PI * 2.0 * b1);

        double n = x0 * (var * var);

        if (java.lang.Double.isFinite(n)) {
            return new Vector3d(vIn.x + n, vIn.y + n, vIn.z + n);
        } else {
            return vIn;
        }
    }

    public void setGPSStartTime(long time) {
        gpsStartTime = time;
    }

    public void setGPSDelay(long delay) {
        gpsDelayLine.setDelay(delay);
    }

    public void setGPSInterval(long gpsInterval) {
        this.gpsInterval = gpsInterval;
    }

    public void setPressureAltOffset(double pressureAltOffset) {
        this.pressureAltOffset = pressureAltOffset;
    }

    @Override
    public Vector3d getAcc() {
        Vector3d accBody = new Vector3d(object.getAcceleration());
        accBody.sub(object.getWorld().getEnvironment().getG());
        Matrix3d rot = new Matrix3d(object.getRotation());
        rot.transpose();
        rot.transform(accBody);
        accBody = addZeroMeanNoise(accBody, 0.8);
        return accBody;
    }

    @Override
    public Vector3d getGyro() {
        return addZeroMeanNoise(object.getRotationRate(), 0.5);
    }

    @Override
    public Vector3d getMag() {
        Vector3d mag = new Vector3d(object.getWorld().getEnvironment().getMagField(object.getPosition()));
        Matrix3d rot = new Matrix3d(object.getRotation());
        rot.transpose();
        rot.transform(mag);
        return addZeroMeanNoise(mag, 0.3);
    }

    @Override
    public double getPressureAlt() {
        return -object.getPosition().z + pressureAltOffset;
    }

    @Override
    public GNSSReport getGNSS() {
        return gps;
    }

    @Override
    public boolean isGPSUpdated() {
        boolean res = gpsUpdated;
        gpsUpdated = false;
        return res;
    }

    @Override
    public void update(long t) {
        // GPS
        if (t > gpsStartTime && t > gpsLast + gpsInterval) {
            gpsLast = t;
            gpsUpdated = true;
            GNSSReport gpsCurrent = new GNSSReport();
            Vector3d pos = object.getPosition();
            gpsCurrent.position = globalProjector.reproject(new double[]{pos.x, pos.y, pos.z});
            gpsCurrent.eph = 1.0;
            gpsCurrent.epv = 1.0;
            gpsCurrent.velocity = new Vector3d(object.getVelocity());
            gpsCurrent.fix = 3;
            gpsCurrent.time = System.currentTimeMillis() * 1000;
            gps = gpsDelayLine.getOutput(t, gpsCurrent);
        }
    }
}

@@ -895,58 +895,40 @@
 lic 
-Vector3d addZeroMeanNoise(Vector3d vIn, 
+double randomNoise(
 double 
-var
+stdDev
 ) %7B%0A
@@ -1218,198 +1218,396 @@
     
-double n = x0 * (var * var
+if (java.lang.Double.isInfinite(x0) %7C%7C java.lang.Double.isNaN(x0)) %7B%0A            x0 = 0.0;%0A        %7D%0A%0A        return x0 * (stdDev * stdDev
 );%0A
-%0A
     
+%7D%0A%0A
     
-if (java.lang.Double.isFinite(n)) %7B%0A            return new Vector3d(vIn.x + n, vIn.y + n,
+public Vector3d addZeroMeanNoise(Vector3d vIn, double stdDev) %7B%0A%0A        return new Vector3d(vIn.x + randomNoise(stdDev),%0A                           
  vIn.
-z + n);%0A        %7D else %7B%0A            return vIn;%0A        %7D
+y + randomNoise(stdDev),%0A                            vIn.z + randomNoise(stdDev));
 %0A   

change the way noise is added