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 Transform bicycleTransform;
        public float trackerDistanceToWheelCenterPoint;

        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 float CalculatedSteerRotation
        {
            get
            {
                var posOnBikePlane = Vector3.ProjectOnPlane(Position, bicycleTransform.up);
                var theta = Mathf.Acos(Mathf.Clamp(posOnBikePlane.x / trackerDistanceToWheelCenterPoint, -1f, 1f));
                return 90f - theta * Mathf.Rad2Deg;
            }
        }

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

        public Vector3 ZeroPos => zeroPos;

        public void Init()
        {
            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);
            }
        }

        //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();
        }
        
    }
}