package org.lasarobotics.vision.test.android;

import android.content.Context;
import android.content.res.Configuration;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.util.Log;
import android.view.WindowManager;

import org.lasarobotics.vision.test.util.ScreenOrientation;

import java.util.Arrays;

/**
 * Contains methods for reading Android native sensors, other than the camera
 */
public final class Sensors implements SensorEventListener {

    private static boolean activated = false;
    private final SensorManager mSensorManager;
    private final Sensor mAccelerometer;
    private final Sensor mRotationVector;
    private final Sensor mMagneticField;

    static float[] gravity = new float[3];
    static float[] linear_acceleration = new float[3];
    static float[] rotation_vector = new float[3];
    static float[] geomagnetic = new float[3];

    private static final float PITCH_TOLERANCE = 20.0f;
    private static final float PITCH_TOLERANCE_HIGH = 45.0f;
    private static final float ROLL_MINIMUM = 10.0f;
    private static final int READ_SPEED = SensorManager.SENSOR_DELAY_NORMAL;

    public Sensors()
    {
        mSensorManager = (SensorManager)Util.getContext().getSystemService(Context.SENSOR_SERVICE);
        mAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        mRotationVector = mSensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
        mMagneticField = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        activated = false;
        resume();
    }

    public void resume()
    {
        if (activated)
            return;
        mSensorManager.registerListener(this, mAccelerometer, READ_SPEED);
        mSensorManager.registerListener(this, mRotationVector, READ_SPEED);
        mSensorManager.registerListener(this, mMagneticField, READ_SPEED);
        activated = true;
    }
    public void stop()
    {
        if (!activated)
            return;
        activated = false;
        mSensorManager.unregisterListener(this);
    };

    private ScreenOrientation screenOrientation = null;

    public boolean hasOrientation()
    {
        return screenOrientation != null;
    }
    public ScreenOrientation getScreenOrientation()
    {
        return screenOrientation != null ? screenOrientation : ScreenOrientation.LANDSCAPE;
    }
    public ScreenOrientation getActivityScreenOrientation()
    {
        WindowManager windowManager = (WindowManager)Util.getContext().getSystemService(Context.WINDOW_SERVICE);
        int rotation = windowManager.getDefaultDisplay().getRotation();
        return ScreenOrientation.getFromSurface(rotation);
    }
    public double getScreenOrientationCompensation()
    {
        return getScreenOrientation().getAngle() - getActivityScreenOrientation().getAngle() + ScreenOrientation.PORTRAIT.getAngle();
    }

    private void updateScreenOrientation()
    {
        float[] R = new float[9];
        float[] I = new float[9];
        SensorManager.getRotationMatrix(R, I, gravity, geomagnetic);
        float[] orientation = new float[3];
        SensorManager.getOrientation(R, orientation);

        //device rotation angle = pitch (first value) [clockwise from horizontal]

        double pitch = orientation[1] / 2 / Math.PI * 360.0;
        double roll = orientation[2] / 2 / Math.PI * 360.0;
        double azimuth = orientation[0] / 2 / Math.PI * 360.0;

        Log.w("Rotation", pitch + ", " + roll + ", " + azimuth);

        //If the phone is too close to the ground, don't update
        if (Math.abs(roll) <= ROLL_MINIMUM)
            return;

        ScreenOrientation current = screenOrientation;

        if (Math.abs(pitch) <= PITCH_TOLERANCE)
            if (roll > 0.0f)
                current = ScreenOrientation.LANDSCAPE_WEST;
            else
                current = ScreenOrientation.LANDSCAPE;
        else if (Math.abs(pitch) >= PITCH_TOLERANCE_HIGH)
            if (pitch > 0.0f)
                current = ScreenOrientation.PORTRAIT;
            else
                current = ScreenOrientation.PORTRAIT_REVERSE;

       screenOrientation = current;
    }

    public void onSensorChanged(SensorEvent event)
    {
        switch(event.sensor.getType())
        {
            case Sensor.TYPE_ACCELEROMETER:
                // alpha is calculated as t / (t + dT)
                // with t, the low-pass filter's time-constant
                // and dT, the event delivery rate
                final float alpha = 0.8f;

                gravity[0] = alpha * gravity[0] + (1 - alpha) * event.values[0];
                gravity[1] = alpha * gravity[1] + (1 - alpha) * event.values[1];
                gravity[2] = alpha * gravity[2] + (1 - alpha) * event.values[2];

                linear_acceleration[0] = event.values[0] - gravity[0];
                linear_acceleration[1] = event.values[1] - gravity[1];
                linear_acceleration[2] = event.values[2] - gravity[2];
                updateScreenOrientation();
                break;
            case Sensor.TYPE_ROTATION_VECTOR:
                rotation_vector = event.values;
                updateScreenOrientation();
                break;
            case Sensor.TYPE_MAGNETIC_FIELD:
                geomagnetic = event.values;
                updateScreenOrientation();
                break;
        }
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int i) {

    }
}

@@ -701,50 +701,8 @@
 er;%0A
-    private final Sensor mRotationVector;%0A
     
@@ -738,16 +738,16 @@
 cField;%0A
+
 %0A    sta
@@ -841,59 +841,8 @@
 3%5D;%0A
-    static float%5B%5D rotation_vector = new float%5B3%5D;%0A
     
@@ -1349,96 +1349,8 @@
 R);%0A
-        mRotationVector = mSensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);%0A
     
@@ -1549,32 +1549,32 @@
         return;%0A
+
         mSensorM
@@ -1636,84 +1636,8 @@
 D);%0A
-        mSensorManager.registerListener(this, mRotationVector, READ_SPEED);%0A
     
@@ -4882,168 +4882,8 @@
 ak;%0A
-            case Sensor.TYPE_ROTATION_VECTOR:%0A                rotation_vector = event.values;%0A                updateScreenOrientation();%0A                break;%0A
     

Remove unneeded sensor