using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

// Free Motion Camera Rig
// https://github.com/Viktor286/FMCR
// https://www.behance.net/gallery/44375157/Free-Motion-Camera-Rig

[ExecuteInEditMode]
public class FreeMotionCameraRig : MonoBehaviour
{
    public Mesh helper;
    public Camera FMCRCamera;

    [Header("Base")]
    [NonSerialized]
    public float positionX;
    [NonSerialized]
    public float positionY;
    [NonSerialized]
    public float positionZ;

    [Header("Node")]
    public float distance;
    public float horizon;
    public float vertical;
    Transform node_trans;

    [Header("Camera")]
    public float roll;
    public float tilt;
    public float sideways;
    public float slide;
    Transform camera_trans;

    [Header("Trail")]
    public float framerate = 30.0f;
    public float duration = 30.0f;
    List<Vector3> trail_positions;
    float stamp;

    void Start()
    {
        trail_positions = new List<Vector3>();
        stamp = Time.time;
    }

    void LateUpdate()
    {
        if (FMCRCamera == null)
            return;

        if (node_trans == null)
        {
            node_trans = transform.GetChild(0);
        }

        if (camera_trans == null)
        {
            camera_trans = node_trans.GetChild(0);
        }

        positionX = transform.localPosition.x;
        positionY = transform.localPosition.y;
        positionZ = transform.localPosition.z;
        transform.localRotation = new Quaternion();
        float scale = Mathf.Max(new float[] { transform.localScale.x, transform.localScale.y, transform.localScale.z });
        transform.localScale = Vector3.one * scale;
         
        if (distance < 0)
            distance = 0;

        node_trans.localPosition = HVDToXYZ(horizon, vertical, distance);
        node_trans.localRotation = HVDToRotation(horizon, vertical, distance);

        camera_trans.localPosition = new Vector3(0, 0, slide);
        camera_trans.localRotation = Quaternion.Euler(-tilt, sideways, roll);

        FMCRCamera.transform.position = camera_trans.position;
        FMCRCamera.transform.rotation = camera_trans.rotation;

        float dt = Time.time - stamp;
        if (dt > 1.0f / framerate)
        {
            stamp = Time.time;
            trail_positions.Add(camera_trans.position);
            int max_count = (int)(duration * framerate);
            while (trail_positions.Count > max_count)
                trail_positions.RemoveAt(0);
        }
    }

    public void Setp()
    {
        LateUpdate();
    }

    public Vector3 NodePosition { get { return transform.GetChild(0).position; } }

    void OnDrawGizmosSelected()
    {
        if (FMCRCamera == null)
            return;

        Camera camera = FMCRCamera;
        Gizmos.matrix = Matrix4x4.TRS(camera.transform.position, camera.transform.rotation, Vector3.one);

        if (camera.orthographic)
        {
            float spread = camera.farClipPlane - camera.nearClipPlane;
            float center = (camera.farClipPlane + camera.nearClipPlane) * 0.5f;
            Gizmos.DrawWireCube(new Vector3(0, 0, center), new Vector3(camera.orthographicSize * 2 * camera.aspect, camera.orthographicSize * 2, spread));
        }
        else
        {
            Gizmos.DrawFrustum(new Vector3(0, 0, camera.nearClipPlane), camera.fieldOfView, camera.farClipPlane, camera.nearClipPlane, camera.aspect);
        }
    }

    void OnDrawGizmos()
    {
        if (FMCRCamera == null)
            return;

        Color hex_color;
        float scale = transform.localScale.x;

        Vector3 up = new Vector3(0, 0, 1);

        // main helper
        if (ColorUtility.TryParseHtmlString("#0E3D59", out hex_color) && node_trans != null)
        {
            Gizmos.color = hex_color;
            Quaternion quat = node_trans.localRotation * Quaternion.Euler(90, 0, 0);
            Gizmos.DrawWireMesh(helper, transform.position, quat, transform.localScale);

            up = quat * up;
        }

        // line to cc
        if (ColorUtility.TryParseHtmlString("#88A61B", out hex_color) && camera_trans != null)
        {
            Gizmos.color = hex_color;
            Gizmos.DrawLine(transform.position, camera_trans.position);
        }

        // camera helper
        if (ColorUtility.TryParseHtmlString("#F25C05", out hex_color) && node_trans != null)
        {
            Gizmos.color = hex_color;
            Vector3 dir = (transform.position - node_trans.position).normalized;
            Vector3 n_dir = Vector3.Cross(dir, up).normalized;
            Vector3 p1 = node_trans.position + dir * 0.18f * scale;
            Vector3 p2 = node_trans.position + n_dir * 0.07f * scale;
            Vector3 p3 = node_trans.position - dir * 0.1f * scale;
            Vector3 p4 = node_trans.position - n_dir * 0.07f * scale;
            Gizmos.DrawLine(p1, p2);
            Gizmos.DrawLine(p2, p3);
            Gizmos.DrawLine(p3, p4);
            Gizmos.DrawLine(p4, p1);
        }

        // camera
        if (ColorUtility.TryParseHtmlString("#D92525", out hex_color) && camera_trans != null)
        {
            Gizmos.color = hex_color;
            Vector3 dir = camera_trans.rotation * Vector3.forward;
            Vector3 n_dir = camera_trans.rotation * Vector3.right;
            Vector3 dir_offset = dir * 0.04f * scale;
            Vector3 p1 = camera_trans.position + dir_offset;
            Vector3 p2 = camera_trans.position - dir_offset + n_dir * 0.09f * scale;
            Vector3 p3 = camera_trans.position - dir_offset - n_dir * 0.09f * scale;
            Gizmos.DrawLine(p1, p2);
            Gizmos.DrawLine(p2, p3);
            Gizmos.DrawLine(p3, p1);
        }
    }

    static Quaternion HVDToRotation(float horizon, float vertical, float distance)
    {
        Vector3 xyz = HVDToXYZ(horizon, vertical, distance + 0.01f);
        return Quaternion.LookRotation(-xyz);
    }

    public Vector3 RotationToHVD(Quaternion roataion, float distance)
    {
        Vector3 xyz = roataion * (Vector3.forward * -distance);
        return XYZToHVDNearest(xyz);
    }

    public static Vector3 HVDToXYZ(float horizon, float vertical, float distance)
    {
        float a = horizon * Mathf.Deg2Rad;
        float b = vertical * Mathf.Deg2Rad;
        float r = distance;

        //z up, right handed
        float x = Mathf.Cos(b) * Mathf.Sin(a) * r;
        float y = -Mathf.Cos(b) * Mathf.Cos(a) * r;
        float z = -Mathf.Sin(b) * r;

        return new Vector3(x, z, y);
    }

    Vector3 XYZToHVD(Vector3 xyz)
    {
        //to z up, right handed
        float x = xyz.x;
        float y = xyz.z;
        float z = xyz.y;

        float horizon = Mathf.Atan2(x, -y) * Mathf.Rad2Deg;
        float vertical = Mathf.Atan2(-z, Mathf.Sqrt(x * x + y * y)) * Mathf.Rad2Deg;
        float distance = xyz.magnitude;

        return new Vector3(horizon, vertical, distance);
    }

    public Vector3 XYZToHVDNearest(Vector3 xyz)
    {
        Vector3 hvd = XYZToHVD(xyz);
        bool vertOver90 = Mathf.Repeat(vertical + 90, 360) > 180;

        float hDiff = horizon - hvd.x;
        float vDiff = vertical - hvd.y;
        
        float hAdj = vertOver90 ? Mathf.Round(hDiff / 180) * 180 : Mathf.Round(hDiff / 360) * 360;
        float vAdj = vertOver90 ? Mathf.Round((vertical + hvd.y) / 180) * 180 : Mathf.Round(vDiff / 360) * 360;

        hvd.x += hAdj;
        hvd.y = vertOver90 ? -hvd.y + vAdj : hvd.y + vAdj;

        return hvd;
    }
}