#include "Gen.h"

#include "bx/math.h"

namespace Gen
{
    bool IsValid(const ModelHandle& h)
    {
        return h.ModelIdx != UINT16_MAX && IsValid(h.Asset);
    }
    bool IsValid(const AssetHandle& h)
    {
        return h.Idx != UINT32_MAX;
    }
    bool IsValid(const TextureHandle& h)
    {
        return h.TextureIdx != UINT16_MAX && IsValid(h.Asset);
    }
    bool operator==(const AssetHandle& lhs, const AssetHandle& rhs)
    {
        return lhs.Idx == rhs.Idx;
    }
    bool operator==(const ModelHandle& lhs, const ModelHandle& rhs)
    {
        if (!IsValid(lhs) || !IsValid(rhs)) return IsValid(lhs) == IsValid(rhs);
        return lhs.ModelIdx == rhs.ModelIdx && lhs.Asset == rhs.Asset;
    }
    PuzPos operator+=(PuzPos lhs, const PuzPos& rhs)
    {
        lhs.X += rhs.X;
        lhs.Y += rhs.Y;
        return lhs;
    }
    PuzPos operator+(PuzPos lhs, const PuzPos& rhs)
    {
        PuzPos res = lhs;
        res.X += rhs.X;
        res.Y += rhs.Y;
        return res;
    }
    PuzPos operator-=(PuzPos lhs, const PuzPos& rhs)
    {
        lhs.X -= rhs.X;
        lhs.Y -= rhs.Y;
        return lhs;
    }
    PuzPos operator-(PuzPos lhs, const PuzPos& rhs)
    {
        PuzPos res = lhs;
        res.X += rhs.X;
        res.Y += rhs.Y;
        return res;
    }

    // Vec4
    Vec4& operator+=(Vec4& lhs, const Vec4& rhs)
    {
        lhs.x += rhs.x;
        lhs.y += rhs.y;
        lhs.z += rhs.z;
        lhs.w += rhs.w;
        return lhs;
    }
    Vec4 operator+(const Vec4& lhs, const Vec4& rhs)
    {
        Vec4 out = lhs;
        return out += rhs;
    }
    Vec4& operator+=(Vec4& lhs, float rhs)
    {
        lhs.x += rhs;
        lhs.y += rhs;
        lhs.z += rhs;
        lhs.w += rhs;
        return lhs;
    }
    Vec4 operator+(Vec4& lhs, float rhs)
    {
        Vec4 out = lhs;
        return out += rhs;
    }

    Vec4& operator-=(Vec4& lhs, const Vec4& rhs)
    {
        lhs.x -= rhs.x;
        lhs.y -= rhs.y;
        lhs.z -= rhs.z;
        lhs.w -= rhs.w;
        return lhs;
    }
    Vec4 operator-(const Vec4& lhs, const Vec4& rhs)
    {
        Vec4 out = lhs;
        return out -= rhs;
    }
    Vec4& operator-=(Vec4& lhs, float rhs)
    {
        lhs.x -= rhs;
        lhs.y -= rhs;
        lhs.z -= rhs;
        lhs.w -= rhs;
        return lhs;
    }
    Vec4 operator-(const Vec4& lhs, float rhs)
    {
        Vec4 out = lhs;
        return out -= rhs;
    }

    Vec4& operator*=(Vec4& lhs, float rhs)
    {
        lhs.x *= rhs;
        lhs.y *= rhs;
        lhs.z *= rhs;
        lhs.w *= rhs;
        return lhs;
    }
    Vec4 operator*(const Vec4& lhs, float rhs)
    {
        Vec4 out = lhs;
        return out *= rhs;
    }

    Vec4& operator/=(Vec4& lhs, float rhs)
    {
        lhs.x /= rhs;
        lhs.y /= rhs;
        lhs.z /= rhs;
        lhs.w /= rhs;
        return lhs;
    }
    Vec4 operator/(const Vec4& lhs, float rhs)
    {
        Vec4 out = lhs;
        return out /= rhs;
    }

    // Vec3
    Vec3& operator+=(Vec3& lhs, const Vec3& rhs)
    {
        lhs.x += rhs.x;
        lhs.y += rhs.y;
        lhs.z += rhs.z;
        return lhs;
    }
    Vec3 operator+(const Vec3& lhs, const Vec3& rhs)
    {
        Vec3 out = lhs;
        return out += rhs;
    }
    Vec3& operator+=(Vec3& lhs, float rhs)
    {
        lhs.x += rhs;
        lhs.y += rhs;
        lhs.z += rhs;
        return lhs;
    }
    Vec3 operator+(Vec3& lhs, float rhs)
    {
        Vec3 out = lhs;
        return out += rhs;
    }

    Vec3& operator-=(Vec3& lhs, const Vec3& rhs)
    {
        lhs.x -= rhs.x;
        lhs.y -= rhs.y;
        lhs.z -= rhs.z;
        return lhs;
    }
    Vec3 operator-(const Vec3& lhs, const Vec3& rhs)
    {
        Vec3 out = lhs;
        return out -= rhs;
    }
    Vec3& operator-=(Vec3& lhs, float rhs)
    {
        lhs.x -= rhs;
        lhs.y -= rhs;
        lhs.z -= rhs;
        return lhs;
    }
    Vec3 operator-(const Vec3& lhs, float rhs)
    {
        Vec3 out = lhs;
        return out -= rhs;
    }

    Vec3& operator*=(Vec3& lhs, float rhs)
    {
        lhs.x *= rhs;
        lhs.y *= rhs;
        lhs.z *= rhs;
        return lhs;
    }
    Vec3 operator*(const Vec3& lhs, float rhs)
    {
        Vec3 out = lhs;
        return out *= rhs;
    }

    Vec3& operator/=(Vec3& lhs, float rhs)
    {
        lhs.x /= rhs;
        lhs.y /= rhs;
        lhs.z /= rhs;
        return lhs;
    }
    Vec3 operator/(const Vec3& lhs, float rhs)
    {
        Vec3 out = lhs;
        return out /= rhs;
    }

    // Vec2
    Vec2& operator+=(Vec2& lhs, const Vec2& rhs)
    {
        lhs.x += rhs.x;
        lhs.y += rhs.y;
        return lhs;
    }
    Vec2 operator+(const Vec2& lhs, const Vec2& rhs)
    {
        Vec2 out = lhs;
        return out += rhs;
    }
    Vec2& operator+=(Vec2& lhs, float rhs)
    {
        lhs.x += rhs;
        lhs.y += rhs;
        return lhs;
    }
    Vec2 operator+(Vec2& lhs, float rhs)
    {
        Vec2 out = lhs;
        return out += rhs;
    }

    Vec2& operator-=(Vec2& lhs, const Vec2& rhs)
    {
        lhs.x -= rhs.x;
        lhs.y -= rhs.y;
        return lhs;
    }
    Vec2 operator-(const Vec2& lhs, const Vec2& rhs)
    {
        Vec2 out = lhs;
        return out -= rhs;
    }
    Vec2& operator-=(Vec2& lhs, float rhs)
    {
        lhs.x -= rhs;
        lhs.y -= rhs;
        return lhs;
    }
    Vec2 operator-(const Vec2& lhs, float rhs)
    {
        Vec2 out = lhs;
        return out -= rhs;
    }

    Vec2& operator*=(Vec2& lhs, float rhs)
    {
        lhs.x *= rhs;
        lhs.y *= rhs;
        return lhs;
    }
    Vec2 operator*(const Vec2& lhs, float rhs)
    {
        Vec2 out = lhs;
        return out *= rhs;
    }

    Vec2& operator/=(Vec2& lhs, float rhs)
    {
        lhs.x /= rhs;
        lhs.y /= rhs;
        return lhs;
    }
    Vec2 operator/(const Vec2& lhs, float rhs)
    {
        Vec2 out = lhs;
        return out /= rhs;
    }

    Mat4 Inverse(const Mat4& mat)
    {
        Mat4 result;
        bx::mtxInverse(result.M, &mat.M[0]);
        return result;
    }
    Mat4 Transpose(const Mat4& mat)
    {
        Mat4 result;
        bx::mtxTranspose(result.M, &mat.M[0]);
        return result;
    }
    Vec4 Mul(const Mat4& mat, const Vec4& vec)
    {
        Vec4 out;
        bx::vec4MulMtx(&out.x, &vec.x, &mat.M[0]);
        return out;
    }

    Vec3 EulerFromRotation(const Mat4& rotation)
    {
        const float r32 = rotation.M[9];
        const float r33 = rotation.M[10];
        float x = bx::atan2(r32, r33);
        float y = bx::atan2(-rotation.M[8], bx::sqrt(r32 * r32 + r33 * r33));
        float z = bx::atan2(rotation.M[4], rotation.M[0]);
        return {x, y, z};
    }

    Mat4 RotationFromEuler(const Vec3& euler)
    {
        Mat4 mat;
        bx::Quaternion quat = bx::fromEuler({euler.x, euler.y, euler.z});
        bx::mtxFromQuaternion(mat.M, quat);
        return mat;
    }

    float Magnitude(const Vec4& vec)
    {
        return bx::sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z + vec.w * vec.w);
    }
    float Magnitude(const Vec3& vec)
    {
        return bx::sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
    }
    float Magnitude(const Vec2& vec)
    {
        return bx::sqrt(vec.x * vec.x + vec.y * vec.y);
    }

    Vec4 Normalized(const Vec4& vec)
    {
        Vec4 res = vec;
        return res /= Magnitude(vec);
    }
    Vec3 Normalized(const Vec3& vec)
    {
        Vec3 res = vec;
        return res /= Magnitude(vec);
    }
    Vec2 Normalized(const Vec2& vec)
    {
        Vec2 res = vec;
        return res /= Magnitude(vec);
    }

    float DotProduct(Vec3 a, Vec3 b)
    {
        return a.x * b.x + a.y * b.y + a.z * b.z;
    }
    Vec3 CrossProduct(Vec3 a, Vec3 b)
    {
        float x = a.y * b.z - a.z * b.y;
        float y = a.z * b.x - a.x * b.z;
        float z = a.x * b.y - a.y * b.x;
        return {x, y, z};
    }
    Vec3 CrossProductFromPlane(Vec3 a, Vec3 b, Vec3 c)
    {
        // TODO: normalize might not be necessary
        Vec3 lineA = Normalized(b - a);
        Vec3 lineB = Normalized(c - a);
        return CrossProduct(lineA, lineB);
    }
    bool RayPlaneIntersect(Vec3 l1, Vec3 l2, Vec3 p1, Vec3 p2, Vec3 p3, Vec3& out)
    {
        // thanks to Paul Bourke and Bryan Hanson
        // l1,l2 constitute the line. P1,P2,P3 constitute the plane
        out = {};
        Vec3 N = CrossProductFromPlane(p1, p2, p3); // N is the normal of the plane
        float n = DotProduct(N, Vec3{p3.x - l1.x, p3.y - l1.y, p3.z - l1.z});
        Vec3 LbMinusLa = Vec3{l2.x - l1.x, l2.y - l1.y, l2.z - l1.z};
        float d = DotProduct(N, LbMinusLa);
        if (d == 0) return false; // Line is parallel to or in the plane
        float u = n / d;
        if ((u >= 0.0) && (u <= 1.0))
        { // Plane is between the two points
        } // can be used for checking but does not influence the outcome
        out = Vec3{l1.x + u * LbMinusLa.x, l1.y + u * LbMinusLa.y, l1.z + u * LbMinusLa.z};
        return true;
    }

    void Translate(Transform& trans, Vec3 offset)
    {
        trans.Position += Vec3{offset.x, offset.y, offset.z};
    }

    void TranslateLocal(Transform& trans, Vec3 offset)
    {
        Vec3 localOffset = GlobalToLocalDirection(trans, offset);
        trans.Position += Vec3{localOffset.x, localOffset.y, localOffset.z};
    }

    void Rotate(Transform& trans, Vec3 rotation)
    {
        float rot[16]{0};
        bx::mtxRotateXYZ(rot, rotation.x, rotation.y, rotation.z);
        float temp[16]{0};
        bx::mtxMul(temp, rot, trans.Rotation.M);
        bx::memCopy(trans.Rotation.M, temp, sizeof(temp));
    }

    void RotateLocal(Transform& trans, Vec3 rotation)
    {
        float rot[16]{0};
        bx::mtxRotateXYZ(rot, rotation.x, rotation.y, rotation.z);
        float temp[16]{0};
        bx::mtxMul(temp, trans.Rotation.M, rot);
        bx::memCopy(trans.Rotation.M, temp, sizeof(temp));
    }

    Vec3 AxisRight(const Mat4& trans)
    {
        return {trans.M[0], trans.M[1], trans.M[2]};
    }

    Vec3 AxisUp(const Mat4& trans)
    {
        return {trans.M[4], trans.M[5], trans.M[6]};
    }

    Vec3 AxisForward(const Mat4& trans)
    {
        return {trans.M[8], trans.M[9], trans.M[10]};
    }

    void UpdateMatrix(Transform& trans)
    {
        Mat4 pos;
        Mat4 scale;
        bx::mtxTranslate(pos.M, trans.Position.x, trans.Position.y, trans.Position.z);
        bx::mtxScale(scale.M, trans.Scale.x, trans.Scale.y, trans.Scale.z);
        Mat4 temp;
        bx::mtxMul(temp.M, scale.M, trans.Rotation.M);
        bx::mtxMul(trans.M.M, temp.M, pos.M);
        bx::mtxInverse(trans.MI.M, trans.M.M);
    }

    Vec3 GlobalToLocalDirection(Transform& trans, Vec3 global)
    {
        UpdateMatrix(trans);
        float in[4]{global.x, global.y, global.z, 0.0f};
        float out[4]{0.0f};
        bx::vec4MulMtx(out, in, Transpose(trans.MI).M);
        return {out[0], out[1], out[2]};
    }
    Vec3 GlobalToLocalPoint(Transform& trans, Vec3 global)
    {
        UpdateMatrix(trans);
        float in[4]{global.x, global.y, global.z, 1.0f};
        float out[4]{0.0f};
        bx::vec4MulMtx(out, in, trans.MI.M);
        return {out[0], out[1], out[2]};
    }
    Vec3 LocalToGlobalDirection(Transform& trans, Vec3 local)
    {
        UpdateMatrix(trans);
        float in[4]{local.x, local.y, local.z, 0.0f};
        float out[4]{0.0f};
        bx::vec4MulMtx(out, in, Transpose(trans.M).M);
        return {out[0], out[1], out[2]};
    }
    Vec3 LocalToGlobalPoint(Transform& trans, Vec3 local)
    {
        UpdateMatrix(trans);
        float in[4]{local.x, local.y, local.z, 1.0f};
        float out[4]{0.0f};
        bx::vec4MulMtx(out, in, trans.M.M);
        return {out[0], out[1], out[2]};
    }
} // namespace Gen
