using System.Linq;
using System.Collections;
using System.Collections.Generic;

using UnityEngine;
using UnityEngine.InputSystem;

public class InputHandler : MonoBehaviour, IGameComponent
{
    private Controls inputSystem;
    private Transform cameraTransform;

    private float X0;
    private float P0;
    private readonly float F = 1;
    private readonly float Q = 2;
    private readonly float H = 1;
    private readonly float R = 15;

    private float Covariance = .1f;
    private float state = 0;

    float GetValueAfterFilter(float value)
    {
        X0 = F * state;
        P0 = F * Covariance * F + Q;

        var K = H *P0 / (H * P0 * H + R);
        state = X0 + K * (value - H * X0);
        Covariance = (1 - K * H) * P0;

        return state;
    }

    void Awake()
    {
        inputSystem = new Controls();
        cameraTransform = transform;
    }

    void Start()
    {
        GameSystem.Instance.AddGameComponent(this);
    }

    void OnEnable()
    {
        inputSystem.Enable();
        InputSystem.EnableDevice(UnityEngine.InputSystem.AttitudeSensor.current);
    }

    void OnDisable()
    {
        InputSystem.DisableDevice(UnityEngine.InputSystem.AttitudeSensor.current);
        inputSystem.Disable();
    }

    public void OnUpdate()
    {
        var phoneRotation = inputSystem
            .Rotation
            .Gyroscope
            .ReadValue<Quaternion>();

        // phoneRotation = new Quaternion
        // (
        //     GetValueAfterFilter(phoneRotation.x),
        //     GetValueAfterFilter(phoneRotation.y),
        //     GetValueAfterFilter(phoneRotation.z),
        //     GetValueAfterFilter(phoneRotation.w)
        // );

        Quaternion filtredRotation = new Quaternion
        (
            GetValueAfterFilter(phoneRotation.x),
            GetValueAfterFilter(phoneRotation.y),
            GetValueAfterFilter(phoneRotation.z),
            GetValueAfterFilter(phoneRotation.w)
        );

        cameraTransform.rotation = filtredRotation;
        cameraTransform.Rotate(0, 0, 180, Space.Self);
        cameraTransform.Rotate(90, 180, 0, Space.World);
    }

    public void OnFixedUpdate() { }
    public void OnLateUpdate() { }

    public void OnDestroy()
    {
        GameSystem.Instance.RemoveGameComponent(this);
    }
}