Xamarin C# SensorType.RotationVector, Gravity, Accelerometer

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using Android.Content;
using Android.Hardware;
using KoalaPhone.Screens;
using Microsoft.Xna.Framework;

namespace KoalaPhone.AndroidHelpers.Listeners
{
    /// <summary>
    /// Speciální listener pro čtení orientace z akcelerometru / kompasu v případě VR
    /// - vrací úhel naklopení hlavy / koeficient
    /// </summary>
    public class VROrientListener
    {
        SensorManager sensorManager;
        Sensor gravitySensor, rotationSensor, accSensor, magneticSensor; // rotace = vyhlazená
        ISensorEventListener listener; // Gravitace (osa Y)
        bool enabled;

        public Vector2 CursorPosCoef = Vector2.Zero; // Výsledná pozice VR kurzoru (-1 až 1 v ose X,Y)
        public bool IsInitialized = false;

        public float LastAzimuth; // Parametry pro kalibraci (čte si je třída MultiListenerImpl)
        public float AzimBaseAngle;
        public Stopwatch SwPrecise; // POčítadlo, kolik uběhlo od posledního bouchnutí do telefonu

        public void InitializeSensorListener()
        {
            // Připraví objekt pro čtení z akcelerometru, napojí event (volaný, když se změní orientace)
            sensorManager = (SensorManager)Game.Activity.GetSystemService(Context.SensorService);
            gravitySensor = sensorManager.GetDefaultSensor(SensorType.Gravity);
            rotationSensor = sensorManager.GetDefaultSensor(SensorType.RotationVector);
            accSensor = sensorManager.GetDefaultSensor(SensorType.Accelerometer); // Pro detekci "bouchnutí"
            magneticSensor = sensorManager.GetDefaultSensor(SensorType.MagneticField); // Cardboard tlačítko

            if (gravitySensor != null || rotationSensor != null)
                listener = new MultiListenerImpl();

            IsInitialized = true;
        }

        public void CalibrateToCenter()
        {
            // Zkalibruje hodnoty, že tohle je střed obrazu
            // - voláno na začátku + po stisku na displej
            AzimBaseAngle = LastAzimuth;
            //TODO kalibrovat i Y souřadnici?
        }

        public void StartSensorReading()
        {
            if (gravitySensor == null)
                return;

            try
            {
                if (!enabled)
                {
                    // Spustí čtení z akcelerometru, gravitace a magnetometru (pokud už není spuštěné)
                    if (gravitySensor != null)
                        sensorManager.RegisterListener(listener, gravitySensor, SensorDelay.Game);
                    if (rotationSensor != null)
                        sensorManager.RegisterListener(listener, rotationSensor, SensorDelay.Game);
                    if (accSensor != null)
                        sensorManager.RegisterListener(listener, accSensor, SensorDelay.Game);
                    if (magneticSensor != null)
                        sensorManager.RegisterListener(listener, magneticSensor, SensorDelay.Fastest);
                    enabled = true;

                    SwPrecise = new Stopwatch();
                    SwPrecise.Start();
                }
            }
            catch (Exception ex)
            {
                ExceptionLogger.LogException(ex, false, true);
            }
        }

        public void StopReading()
        {
            if (gravitySensor == null)
                return;

            try
            {
                if (enabled)
                {
                    // Vypne čtení z akcelerometru (voláno, když odešel z obrazovky atd.)
                    sensorManager.UnregisterListener(listener);
                    enabled = false;

                    SwPrecise.Stop();
                }
            }
            catch (Exception ex)
            {
                ExceptionLogger.LogException(ex, false, true);
            }
        }

        public class MultiListenerImpl : Java.Lang.Object, ISensorEventListener
        {
            // Poslední hodnoty načtené z akcelerometru / magnetického kompasu
            float[] rotationVals = new float[3];
            float[] rotationMatrix = new float[16];
            float[] orientation = new float[3];
            float[] accVals = new float[3]; // Detekce bouchnutí do telefonu
            bool calibrated = false;

            float[] magneticVals = new float[3]; // Cardboard tlačítko
            List<float[]> magData = new List<float[]>();
            const int MAGNETIC_COUNT = 40;
            float[] offsets = new float[20];

            const float THRESHOLD = 8f; // Od kdy se to bere jako klepnutí na telefon
            const float WAIT_TAP_MS = 250f; // Kolik čeká mezi jednotlivými kliknutími
            long lastElapsedTime = 0;

            public void OnAccuracyChanged(Sensor sensor, SensorStatus accuracy)
            {
            }

            public void OnSensorChanged(SensorEvent e)
            {
                Vector3 val = new Vector3(e.Values[0], e.Values[1], e.Values[2]);

                if (e.Sensor.Type == SensorType.Gravity)
                {
                    // Nastavím gravitaci (pohyb nahoru / dolů = osa Y)
                    // - sensitivity: od 1 do 100 (1 je nejmenší, tj. nejmenší pohyb hlavy)
                    //   výchozí: 50% (nastavení citlivosti má vliv i na osu X)
                    float gravity = val.Z / (10f - ((Engine.VRSensitivity * 0.8f) / 10f));
                    gravity -= 0.3f; //TODO tohle je to kalibrační nastavení na začátku, pro osu Y
                    gravity = Math.Min(1f, gravity);
                    gravity = Math.Max(-1f, gravity);

                    // Rovnou nastavím osu Y (je to všechno, co pro to potřebujem vědět)
                    Engine.Game.VRListener.CursorPosCoef.Y = gravity;
                    Engine.Game.VRSampler.SampleValue();
                }

                if (e.Sensor.Type == SensorType.Accelerometer)
                {
                    // Detekce velkého klepnutí (finálně potvrdí výběr položky)
                    if ((accVals[0] == 0 && accVals[1] == 0 && accVals[2] == 0))
                        CopyValues(e.Values, accVals);
                    else
                    {
                        // Pokud už zná předchozí hodnotu, porovná ji
                        float force = Math.Abs(e.Values[0] + e.Values[1] + e.Values[2] - accVals[0] - accVals[1] - accVals[2]);
                        CopyValues(e.Values, accVals);

                        if (force > THRESHOLD && calibrated) //TODO doladit, aby to šlo vypínat (+ citlivost?)
                        {
                            // Ochrání si, aby bylo možné odklepnout jen 1x za 250 ms (nebralo to jako 2 dotyky)
                            long elapsedTime = Engine.Game.VRListener.SwPrecise.ElapsedMilliseconds;
                            if (elapsedTime > WAIT_TAP_MS)
                            {
                                // Odklepne tlačítko
                                Engine.Game.ConfirmButtonFromVR();
                                Engine.Game.VRListener.SwPrecise.Reset();
                                Engine.Game.VRListener.SwPrecise.Start();
                            }
                        }
                    }
                }

                if (e.Sensor.Type == SensorType.MagneticField)
                {
                    // Cardboard, detekce stisku tlačítka
                    CopyValues(e.Values, magneticVals);

                    if (!(magneticVals[0] == 0 && magneticVals[1] == 0 && magneticVals[2] == 0))
                    {
                        if (magData.Count > MAGNETIC_COUNT)
                            magData.RemoveAt(0);
                        magData.Add(magneticVals);

                        if (magData.Count == MAGNETIC_COUNT)
                            EvaluateCardboardButton();
                    }
                }

                if (e.Sensor.Type == SensorType.RotationVector)
                {
                    // Pohyb doprava / doleva (= azimut, kam se dívá do stran)
                    CopyValues(e.Values, rotationVals);

                    SensorManager.GetRotationMatrixFromVector(rotationMatrix, rotationVals);
                    SensorManager.GetOrientation(rotationMatrix, orientation);

                    // Azimut = úhel doprava / doleva, kam se díváme
                    float azimuth = RadianToDegree(orientation[0]); //smoothedAzimuth); //
                    if (azimuth < 0) azimuth += 360;
                    if (azimuth > 360) azimuth -= 360;
                    Engine.Game.VRListener.LastAzimuth = azimuth; // Podle tohohle se pak kalibruje azimut

                    if (!calibrated)
                        if (!(rotationVals[0] == 0 && rotationVals[1] == 0 && rotationVals[2] == 0))
                        {
                            // Načetli jsme první reálné hodnoty, zkalibrujeme pohled
                            Engine.Game.VRListener.CalibrateToCenter();
                            calibrated = true; //TODO zkalibruje se i při dalším (příštím) spuštění?
                        }

                    float sensitivityCoef = -((Engine.VRSensitivity / 100f) - 0.5f);
                    float azimDiff = 40 + 35 * sensitivityCoef;

                    float min = Engine.Game.VRListener.AzimBaseAngle - azimDiff;
                    float max = Engine.Game.VRListener.AzimBaseAngle + azimDiff;
                    if (min < 0) min += 360;
                    if (min > 360) min -= 360;
                    if (max < 0) max += 360;
                    if (max > 360) max -= 360;
                    if (max < min) max += 360;

                    // Zjistí, kde v daném rozsahu se hodnota azimutu nachází
                    // - krajní hodnoty nechá, aby byly krajní; namapuje na -1 až 1
                    float azimCoef = (azimuth - min) / (max - min) * 2f - 1f;
                    azimCoef = Math.Min(1f, azimCoef);
                    azimCoef = Math.Max(-1f, azimCoef);

                    // A nastavím osu X
                    Engine.Game.VRListener.CursorPosCoef.X = azimCoef;
                    Engine.Game.VRSampler.SampleValue();
                }

                // Nevolám překreslení / znovu update! (smyčka kreslení VR je na tom nezávisle)
                // - pokud se nic nezměnilo, vykreslovaná textura je zapamatovaná a pořád stejná
                // - červená tečka se překresluje až přes texturu
                if (!Engine.RefreshVRDraw)
                    Engine.Game.ResumeGameLoop();
                Engine.RefreshVRDraw = true;
            }

            private void EvaluateCardboardButton()
            {
                // Spočítá, jestli bylo Cardboard tlačítko smáčknuto (a příp. vyvolá metodu)
                // - zavoláno, pokud už máme plný buffer 40 hodnot
                float[] means = new float[2];
                float[] maximums = new float[2];
                float[] minimums = new float[2];
                float[] baseline = magData[magData.Count - 1];
                for (int i = 0; i < 2; i++)
                {
                    int segmentStart = 20 * i;
                    float[] offs = ComputeOffsets(segmentStart, baseline);
                    means[i] = offs.Sum() / offs.Length;
                    maximums[i] = offs.Concat(new[] { (-1.0F / 0.0F) }).Max();
                    minimums[i] = offs.Concat(new[] { (1.0F / 0.0F) }).Min();
                }
                float min1 = minimums[0];
                float max2 = maximums[1];
                if ((min1 < 30.0F) && (max2 > 130.0F))
                {
                    // Potvrdí tlačítko (podle předchozí pozice kurzoru, nastavené v SelectButonFromVR
                    // z VRInputSamples)
                    //Engine.Game.ConfirmButtonFromVR(); //TODO cardboard tlačítko
                }
            }

            private float[] ComputeOffsets(int segmentStart, float[] baseline)
            {
                // Spočítá offsety pro cardboard (jak to ulítává do stran)
                // - viz https://github.com/Zomega/Cardboard/blob/master/src/com/google/vrtoolkit/cardboard/sensors/MagnetSensor.java
                for (int i = 0; i < 20; i++)
                {
                    float[] point = magData[segmentStart + i];
                    float[] o = { point[0] - baseline[0], point[1] - baseline[1], point[2] - baseline[2] };
                    float magnitude = (float)Math.Sqrt(o[0] * o[0] + o[1] * o[1] + o[2] * o[2]);
                    offsets[i] = magnitude;
                }
                return offsets;
            }

            private void CopyValues(IList<float> list, float[] vals)
            {
                // Zkopíruje hodnoty do zadaného pole
                for (int i = 0; i < 3; i++)
                    vals[i] = list[i];
            }
            private float RadianToDegree(float angle)
            {
                return (float)(angle * (180.0 / Math.PI));
            }
        }

    }
}