package com.netfluke.sergey.accelintegrator;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.support.v7.app.AppCompatActivity;
import android.os.Bundle;
import android.util.Log;
import android.widget.TextView;

/*
 *   This code fails spectacularly. When at rest on a table, the phone
 *     still believes it is traveling, with increasing speed. See
 *     David Sachs, ""Sensor Fusion on Android Devices", at ~ 23:20 for why:
 *       https://www.youtube.com/watch?v=C7JQ7Rpwn2k
 *
 *   Note: you can add a calibration phase to detect the accelerometer biases (offsets)
 *     and then subtract them from the readings, as per
 *     https://developer.android.com/guide/topics/sensors/sensors_motion.html#sensors-motion-linear
 *     See how much it helps.
 */


public class MainActivity extends AppCompatActivity implements SensorEventListener {

    private SensorManager mSensorManager;
    private Sensor mAccel;

    float x, y, z; // integrated distances
    float vx, vy, vz; // integrated speeds
    float vx0, vy0, vz0; // speeds at a previous turn dT

    private static final float NS2S = 1.0f / 1000000000.0f; // nanoseconds to seconds conversion factor
    private float timestamp = 0;

    // low pass filtering
    float[] accl_lp = new float[3];

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);

        mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
        mAccel = mSensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION);

        x = y = z = 0f;
        vx = vy = vz = 0f;
        vx0 = vy0 = vz0 = 0f;
    }

    @Override
    protected void onResume() {
        super.onResume();
        mSensorManager.registerListener(this, mAccel, SensorManager.SENSOR_DELAY_GAME);
    }

    @Override
    protected void onPause() {
        super.onPause();
        mSensorManager.unregisterListener(this, mAccel);
    }

    @Override
    public void onSensorChanged(SensorEvent event) {

        float[] accl = new float[3];
        if (event.sensor == mAccel) {
            System.arraycopy(event.values, 0, accl, 0, event.values.length);
            Log.d("ACCL", "x: " + accl[0] + " y: " + accl[1] + " z: " + accl[2]);

            // Rounded-off values. Try rounding to a different number of decimal digits
            float xr = Math.round(accl[0]*100)/100;
            float yr = Math.round(accl[1]*100)/100;
            float zr = Math.round(accl[2]*100)/100;
            Log.d("ROUNDED", "x: " + xr + " y: " + yr + " z: " + zr);


            // Apply a low-pass filter to smooth the sensor jitter.
            //   This doesn't help the double integration drift problem,
            //    because it's caused by low-frequency noise.
            //
            // final float alpha = 0.8f;
            // accl_lp[0] = alpha * accl_lp[0] + (1 - alpha) * accl[0];
            // accl_lp[1] = alpha * accl_lp[1] + (1 - alpha) * accl[1];
            // accl_lp[2] = alpha * accl_lp[2] + (1 - alpha) * accl[2];

            // Unfiltered. Comment this out & uncomment the above to apply low-pass filtering
            accl_lp[0] = accl[0];
            accl_lp[1] = accl[1];
            accl_lp[2] = accl[2];

            if( timestamp != 0) {
                final float dT = (event.timestamp - timestamp) * NS2S;

                // integrate once, for speed
                vx += accl_lp[0] * dT;
                vy += accl_lp[1] * dT;
                vz += accl_lp[2] * dT;

                // integrate twice, for distance
                x += (vx+vx0)/2f * dT;
                y += (vy+vy0)/2f * dT;
                z += (vz+vz0)/2f * dT;

                // save previous values of speed
                vx0 = vx;
                vy0 = vy;
                vz0 = vz;
            }
            timestamp = event.timestamp;

            TextView t = findViewById(R.id.txt);
            t.setText( "x: " + x + "\ny: " + y + "\nz: " + z);


        } else
            Log.d("ACCL", "Not an acceleration sensor");
    }


    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy) {
        // TODO Auto-generated method stub
    }

}