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

namespace Bausatz
{
    public class TrackPiece : MonoBehaviour
    {
        public Transform end;
        public Vector3[] points = new Vector3[0];
        private Bezier _curve;
        public float length;
        public float rotate;

        public float constantSpeed;
        public float constantBreaking;

        private void OnDrawGizmosSelected()
        {
            CalculateLength();

            Gizmos.color = Color.blue;
            var endPosition = end.position;
            Gizmos.DrawSphere(endPosition, 0.1f);

            Gizmos.color = Color.green;
            var start = transform.position;
            Gizmos.DrawSphere(start, 0.15f);

            var samples = _curve.SampleCurve(10);
            foreach (var p in points)
            {
                Gizmos.color = Color.cyan;
                Gizmos.DrawSphere(transform.TransformPoint(p), 0.08f);
            }
            
            var previous = transform.TransformPoint(Vector3.zero);
            foreach (var sample in samples)
            {
                Gizmos.color = Color.blue;
                var pointGlobal = transform.TransformPoint(sample);
                Gizmos.DrawLine(previous, pointGlobal);

                previous = pointGlobal;
            }

            for (int i = 0; i <= 10; i++)
            {
                var t = i / 10f;

                var p = _curve.DeCasteljau(t);
                _curve.CalculateFrenet(t, rotate, out var up, out var right , out var forward);
                
                Gizmos.color = Color.red;
                var worldP = transform.TransformPoint(p);
                Gizmos.DrawLine(worldP, transform.TransformPoint(p + right));
                
                Gizmos.color = Color.green;
                Gizmos.DrawLine(worldP, transform.TransformPoint(p + up));
                
                Gizmos.color = Color.blue;
                Gizmos.DrawLine(worldP, transform.TransformPoint(p + forward));

                var a = 9.81f * (transform.rotation * forward).y;
                Gizmos.color = Color.yellow;
                Gizmos.DrawLine(worldP, worldP + a * Vector3.up);
            }
            
        }

        private void OnDrawGizmos()
        {
            if (constantBreaking > 0)
            {
                Gizmos.color = Color.red;
                Gizmos.DrawCube(transform.position + transform.up, Vector3.one);
            }
            
            if (constantSpeed > 0)
            {
                Gizmos.color = Color.green;
                Gizmos.DrawCube(transform.position + transform.up, Vector3.one);
            }
            CalculateLength();
            var trans = transform;
            var worldPiece = trans.parent.position.y;
            for (int i = 0; i <= 5; i++)
            {
                var t = i / 5f;

                var p = _curve.DeCasteljau(t);
                _curve.CalculateFrenet(t, rotate, out var up, out var right , out var forward);

                if ((p + trans.localPosition).y > 5 && (trans.rotation * forward).y <= 0.8f)
                {
                    var worldP = trans.TransformPoint(p + right);
                    var worldP2 = trans.TransformPoint(p - right);
                    
                    Gizmos.color = Color.black;
                    Gizmos.DrawLine(worldP, new Vector3(worldP.x, worldPiece, worldP.z));
                    Gizmos.DrawLine(worldP2, new Vector3(worldP2.x, worldPiece, worldP2.z));
                }

            }
        }

        private void Awake()
        {
            CalculateLength();
        }

        public void CalculateLength()
        {
            _curve = new Bezier(points, transform.InverseTransformPoint(end.position));
            length = _curve.ApproximateLength(0.1f);
        }

        public float GetPositionOnTrack(float progress, out Vector3 position, out Quaternion rotation, out float constantV)
        {
            position = transform.TransformPoint(_curve.DeCasteljau(progress));
            
            _curve.CalculateFrenet(progress, rotate, out var up, out var right , out var forward);
            var globalRotation = transform.rotation;
            rotation = globalRotation * Quaternion.LookRotation(-forward, up);

            constantV = 0;

            if (!(constantSpeed > 0)) return constantBreaking > 0 ? constantBreaking : (globalRotation * forward).y;
            constantV = constantSpeed;
            
            return 0;
        }
    }
}