using System;
using System.Drawing;
using System.Numerics;   // Vector3 & Quaternion

namespace MosswartMassacre
{
    public static class Geometry
    {
        /* ---------- heading / quaternion helpers ---------- */

        public static Quaternion HeadingToQuaternion(float deg) =>
            ToQuaternion((float)Math.PI * -deg / 180.0f, 0, 0);

        public static Quaternion RadiansToQuaternion(float rad) =>
            ToQuaternion(rad, 0, 0);

        public static double QuaternionToHeading(Quaternion q)
        {
            double sinr = 2 * (q.W * q.X + q.Y * q.Z);
            double cosr = 1 - 2 * (q.X * q.X + q.Y * q.Y);
            return Math.Atan2(sinr, cosr);
        }

        public static double CalculateHeading(Vector3 start, Vector3 target)
        {
            var dy = target.Y - start.Y;
            var dx = target.X - start.X;
            return (360 - (Math.Atan2(dy, dx) * 180 / Math.PI) + 90) % 360;
        }

        // yaw (Z) - pitch (Y) - roll (X)
        public static Quaternion ToQuaternion(float yaw, float pitch, float roll)
        {
            float cy = (float)Math.Cos(yaw * 0.5f); float sy = (float)Math.Sin(yaw * 0.5f);
            float cp = (float)Math.Cos(pitch * 0.5f); float sp = (float)Math.Sin(pitch * 0.5f);
            float cr = (float)Math.Cos(roll * 0.5f); float sr = (float)Math.Sin(roll * 0.5f);

            return new Quaternion(
                cy * cp * sr - sy * sp * cr,   // X
                sy * cp * sr + cy * sp * cr,   // Y
                sy * cp * cr - cy * sp * sr,   // Z
                cy * cp * cr + sy * sp * sr    // W
            );
        }

        /* ---------- landblock / coordinate helpers ---------- */

        public static uint GetLandblockFromCoordinates(double ew, double ns)
        {
            ns = (ns - 0.5) * 10.0;
            ew = (ew - 0.5) * 10.0;

            uint basex = (uint)(ew + 0x400);
            uint basey = (uint)(ns + 0x400);

            byte bx = (byte)(basex >> 3);
            byte by = (byte)(basey >> 3);
            byte cx = (byte)(basex & 7);
            byte cy = (byte)(basey & 7);

            int block = (bx << 8) | by;
            int cell = (cx << 3) | cy;

            return (uint)((block << 16) | (cell + 1));
        }

        public static float LandblockToEW(uint landcell, float xOff)
        {
            uint l = (landcell & 0xFF000000) / 0x200000;
            return (float)(((xOff / 24) + l - 1019.5) / 10);
        }

        public static float LandblockToNS(uint landcell, float yOff)
        {
            uint l = (landcell & 0x00FF0000) / 0x2000;
            return (float)(((yOff / 24) + l - 1019.5) / 10);
        }

        public static float EWToLandblock(uint landcell, float ew)
        {
            uint l = (landcell & 0xFF000000) / 0x200000;
            return (float)(((ew * 10) - l + 1019.5) * 24);
        }

        public static float NSToLandblock(uint landcell, float ns)
        {
            uint l = (landcell & 0x00FF0000) / 0x2000;
            return (float)(((ns * 10) - l + 1019.5) * 24);
        }

        public static int LandblockXDifference(uint orig, uint dest) =>
            (int)(((dest >> 24) - (orig >> 24)) * 192);

        public static int LandblockYDifference(uint orig, uint dest) =>
            (int)((((dest << 8) >> 24) - ((orig << 8) >> 24)) * 192);

        /* ---------- misc helpers ---------- */

        public static float Distance2d(float x1, float y1, float x2, float y2) =>
            (float)Math.Sqrt(Math.Pow(x2 - x1, 2) + Math.Pow(y2 - y1, 2));

        public static bool LineIntersectsRect(Point p1, Point p2, Rectangle r) =>
            LineIntersectsLine(p1, p2, new Point(r.X, r.Y), new Point(r.Right, r.Y)) ||
            LineIntersectsLine(p1, p2, new Point(r.Right, r.Y), new Point(r.Right, r.Bottom)) ||
            LineIntersectsLine(p1, p2, new Point(r.Right, r.Bottom), new Point(r.X, r.Bottom)) ||
            LineIntersectsLine(p1, p2, new Point(r.X, r.Bottom), new Point(r.X, r.Y)) ||
            (r.Contains(p1) && r.Contains(p2));

        public static bool LineIntersectsLine(Point a1, Point a2, Point b1, Point b2)
        {
            float q = (a1.Y - b1.Y) * (b2.X - b1.X) - (a1.X - b1.X) * (b2.Y - b1.Y);
            float d = (a2.X - a1.X) * (b2.Y - b1.Y) - (a2.Y - a1.Y) * (b2.X - b1.X);
            if (d == 0) return false;

            float r = q / d;
            q = (a1.Y - b1.Y) * (a2.X - a1.X) - (a1.X - b1.X) * (a2.Y - a1.Y);
            float s = q / d;
            return (r >= 0 && r <= 1 && s >= 0 && s <= 1);
        }
    }
}
