using Unity.Mathematics;
using UnityEngine;

namespace pdinklag
{
    /// <summary>
    /// Local space of a triangle used for barycentric coordinates.
    /// </summary>
    public struct TriangleSpace
    {
        private double3 _a;
        private double3 _ab;
        private double3 _ac;
        private double3 _normal;

        private double _d00;
        private double _d01;
        private double _d11;
        private double _invDenom;

        public TriangleSpace(Vector3 a, Vector3 b, Vector3 c)
        {
            _a = a.ToDouble3();
            _ab = b.ToDouble3() - _a;
            _ac = c.ToDouble3() - _a;
            _d00 = math.dot(_ab, _ab);
            _d01 = math.dot(_ab, _ac);
            _d11 = math.dot(_ac, _ac);

            var denom = _d00 * _d11 - _d01 * _d01;
            _invDenom = 1.0 / denom;

            _normal = math.normalize(math.cross(_ac, _ab));
        }

        /// <summary>
        /// Computes the barycentric coordinates of the given point.
        /// </summary>
        /// <param name="p">the point</param>
        private double2 InverseTransformSurface(double3 p)
        {
            var ap = p - _a;
            var d20 = math.dot(ap, _ab);
            var d21 = math.dot(ap, _ac);

            var u = (_d11 * d20 - _d01 * d21) * _invDenom;
            var v = (_d00 * d21 - _d01 * d20) * _invDenom;

            return new double2(u, v);
        }

        /// <summary>
        /// Projects a point to the surface of the plane defined by the triangle.
        /// </summary>
        /// <param name="p">the point</param>
        public double3 Project(Vector3 p)
        {
            var uv = InverseTransformSurface(p.ToDouble3());
            return _a + uv.x * _ab + uv.y * _ac;
        }

        /// <summary>
        /// Computes the barycentric coordinates (X and Z) of the given point and its distance from the triangle (Y coordinate).
        /// </summary>
        /// <param name="p">the point</param>
        public double3 InverseTransform(Vector3 p)
        {
            var p3d = p.ToDouble3();
            var uv = InverseTransformSurface(p3d);

            var q = _a + uv.x * _ab + uv.y * _ac;
            var d = math.dot(p3d - q, _normal);

            return new double3(uv.x, d, uv.y);
        }

        /// <summary>
        /// Converts barycentric coordinates and a distance to a world point.
        /// </summary>
        /// <param name="p">the barycentric coordinates</param>
        public Vector3 Transform(double3 p)
        {
            return (_a + p.x * _ab + p.y * _normal + p.z * _ac).ToVector3();
        }
    }
}
