using System;
using UnityEngine;

namespace Tracking
{
    public class FrontWheelTracker : MonoBehaviour
    {
        private const String KEY_POS_X = "vive_tracker_pos_x";
        private const String KEY_POS_Y = "vive_tracker_pos_y";
        private const String KEY_POS_Z = "vive_tracker_pos_z";
        private const String KEY_ROT_X = "vive_tracker_rot_x";
        private const String KEY_ROT_Y = "vive_tracker_rot_y";
        private const String KEY_ROT_Z = "vive_tracker_rot_z";

        private Transform trackerTransform;
        private Vector3 zeroRot;
        private Vector3 zeroPos;

        public Vector3 Rotation
        {
            get
            {
                var newVal = trackerTransform.localRotation.eulerAngles - zeroRot;
                var x = newVal.x;
                var y = newVal.y;
                var z = newVal.z;
                if (x > 180) x = -(360 - x);
                if (y > 180) y = -(360 - y);
                if (z > 180) z = -(360 - z);
            
                if (x < -180) x = -(-360 - x);
                if (y < -180) y = -(-360 - y);
                if (z < -180) z = -(-360 - z);
                return new Vector3(x,y,z);
            }
        }

        public Vector3  Position => trackerTransform.localPosition - zeroPos;
        public Vector3 ZeroRot => zeroRot;

        public Vector3 ZeroPos => zeroPos;

        // Start is called before the first frame update
        void Start()
        {
            trackerTransform = transform;
            if (PlayerPrefs.HasKey(KEY_POS_X) && PlayerPrefs.HasKey(KEY_POS_Y) && PlayerPrefs.HasKey(KEY_POS_Z))
            {
                var x = PlayerPrefs.GetFloat(KEY_POS_X);
                var y = PlayerPrefs.GetFloat(KEY_POS_Y);
                var z = PlayerPrefs.GetFloat(KEY_POS_Z);

                zeroPos = new Vector3(x, y, z);
            }

            if (PlayerPrefs.HasKey(KEY_ROT_X) && PlayerPrefs.HasKey(KEY_ROT_Y) && PlayerPrefs.HasKey(KEY_ROT_Z))
            {
                var x = PlayerPrefs.GetFloat(KEY_ROT_X);
                var y = PlayerPrefs.GetFloat(KEY_ROT_Y);
                var z = PlayerPrefs.GetFloat(KEY_ROT_Z);

                zeroRot = new Vector3(x, y, z);
            }
        }

        // Update is called once per frame
        void Update()
        {
        }

        //sets current position as zero -> straighten handlebar and call this function
        public void Calibrate()
        {
            zeroRot = trackerTransform.localRotation.eulerAngles;
            zeroPos = trackerTransform.localPosition;
            PlayerPrefs.SetFloat(KEY_ROT_X, zeroRot.x);
            PlayerPrefs.SetFloat(KEY_ROT_Y, zeroRot.y);
            PlayerPrefs.SetFloat(KEY_ROT_Z, zeroRot.z);
            PlayerPrefs.SetFloat(KEY_POS_X, zeroPos.x);
            PlayerPrefs.SetFloat(KEY_POS_Y, zeroPos.y);
            PlayerPrefs.SetFloat(KEY_POS_Z, zeroPos.z);
            PlayerPrefs.Save();
        }
    }
}