// Copyright (c) 2012-2021 Wojciech Figat. All rights reserved.

#include "Vector2.h"
#include "Vector3.h"
#include "Vector4.h"
#include "Color.h"
#include "Quaternion.h"
#include "Matrix.h"
#include "Int3.h"
#include "../Types/String.h"

static_assert(sizeof(Vector3) == 12, "Invalid Vector3 type size.");

const Vector3 Vector3::Zero(0.0f);
const Vector3 Vector3::One(1.0f);
const Vector3 Vector3::Half(0.5f);
const Vector3 Vector3::UnitX(1.0f, 0.0f, 0.0f);
const Vector3 Vector3::UnitY(0.0f, 1.0f, 0.0f);
const Vector3 Vector3::UnitZ(0.0f, 0.0f, 1.0f);
const Vector3 Vector3::Up(0.0f, 1.0f, 0.0f);
const Vector3 Vector3::Down(0.0f, -1.0f, 0.0f);
const Vector3 Vector3::Left(-1.0f, 0.0f, 0.0f);
const Vector3 Vector3::Right(1.0f, 0.0f, 0.0f);
const Vector3 Vector3::Forward(0.0f, 0.0f, 1.0f);
const Vector3 Vector3::Backward(0.0f, 0.0f, -1.0f);
const Vector3 Vector3::Minimum(MIN_float);
const Vector3 Vector3::Maximum(MAX_float);

Vector3::Vector3(const Vector2& xy, float z)
    : X(xy.X)
    , Y(xy.Y)
    , Z(z)
{
}

Vector3::Vector3(const Vector2& xy)
    : X(xy.X)
    , Y(xy.Y)
    , Z(0)
{
}

Vector3::Vector3(const Int3& xyz)
    : X((float)xyz.X)
    , Y((float)xyz.Y)
    , Z((float)xyz.Z)
{
}

Vector3::Vector3(const Vector4& xyz)
    : X(xyz.X)
    , Y(xyz.Y)
    , Z(xyz.Z)
{
}

Vector3::Vector3(const Color& color)
    : X(color.R)
    , Y(color.G)
    , Z(color.B)
{
}

String Vector3::ToString() const
{
    return String::Format(TEXT("{}"), *this);
}

void Vector3::UnwindEuler()
{
    X = Math::UnwindDegrees(X);
    Y = Math::UnwindDegrees(Y);
    Z = Math::UnwindDegrees(Z);
}

Vector3 Vector3::Floor(const Vector3& v)
{
    return Vector3(
        Math::Floor(v.X),
        Math::Floor(v.Y),
        Math::Floor(v.Z)
    );
}

Vector3 Vector3::Frac(const Vector3& v)
{
    return Vector3(
        v.X - (int32)v.X,
        v.Y - (int32)v.Y,
        v.Z - (int32)v.Z
    );
}

Vector3 Vector3::Clamp(const Vector3& value, const Vector3& min, const Vector3& max)
{
    float x = value.X;
    x = x > max.X ? max.X : x;
    x = x < min.X ? min.X : x;

    float y = value.Y;
    y = y > max.Y ? max.Y : y;
    y = y < min.Y ? min.Y : y;

    float z = value.Z;
    z = z > max.Z ? max.Z : z;
    z = z < min.Z ? min.Z : z;

    return Vector3(x, y, z);
}

void Vector3::Clamp(const Vector3& value, const Vector3& min, const Vector3& max, Vector3& result)
{
    float x = value.X;
    x = x > max.X ? max.X : x;
    x = x < min.X ? min.X : x;

    float y = value.Y;
    y = y > max.Y ? max.Y : y;
    y = y < min.Y ? min.Y : y;

    float z = value.Z;
    z = z > max.Z ? max.Z : z;
    z = z < min.Z ? min.Z : z;

    result = Vector3(x, y, z);
}

float Vector3::Distance(const Vector3& value1, const Vector3& value2)
{
    const float x = value1.X - value2.X;
    const float y = value1.Y - value2.Y;
    const float z = value1.Z - value2.Z;
    return Math::Sqrt(x * x + y * y + z * z);
}

float Vector3::DistanceSquared(const Vector3& value1, const Vector3& value2)
{
    const float x = value1.X - value2.X;
    const float y = value1.Y - value2.Y;
    const float z = value1.Z - value2.Z;
    return x * x + y * y + z * z;
}

Vector3 Vector3::Normalize(const Vector3& input)
{
    Vector3 output = input;
    const float length = input.Length();
    if (!Math::IsZero(length))
    {
        const float inv = 1.0f / length;
        output.X *= inv;
        output.Y *= inv;
        output.Z *= inv;
    }
    return output;
}

void Vector3::Normalize(const Vector3& input, Vector3& result)
{
    result = input;
    const float length = input.Length();
    if (!Math::IsZero(length))
    {
        const float inv = 1.0f / length;
        result.X *= inv;
        result.Y *= inv;
        result.Z *= inv;
    }
}

void Vector3::Hermite(const Vector3& value1, const Vector3& tangent1, const Vector3& value2, const Vector3& tangent2, float amount, Vector3& result)
{
    const float squared = amount * amount;
    const float cubed = amount * squared;
    const float part1 = 2.0f * cubed - 3.0f * squared + 1.0f;
    const float part2 = -2.0f * cubed + 3.0f * squared;
    const float part3 = cubed - 2.0f * squared + amount;
    const float part4 = cubed - squared;

    result.X = value1.X * part1 + value2.X * part2 + tangent1.X * part3 + tangent2.X * part4;
    result.Y = value1.Y * part1 + value2.Y * part2 + tangent1.Y * part3 + tangent2.Y * part4;
    result.Z = value1.Z * part1 + value2.Z * part2 + tangent1.Z * part3 + tangent2.Z * part4;
}

void Vector3::Reflect(const Vector3& vector, const Vector3& normal, Vector3& result)
{
    const float dot = vector.X * normal.X + vector.Y * normal.Y + vector.Z * normal.Z;
    result.X = vector.X - 2.0f * dot * normal.X;
    result.Y = vector.Y - 2.0f * dot * normal.Y;
    result.Z = vector.Z - 2.0f * dot * normal.Z;
}

void Vector3::Transform(const Vector3& vector, const Quaternion& rotation, Vector3& result)
{
    const float x = rotation.X + rotation.X;
    const float y = rotation.Y + rotation.Y;
    const float z = rotation.Z + rotation.Z;
    const float wx = rotation.W * x;
    const float wy = rotation.W * y;
    const float wz = rotation.W * z;
    const float xx = rotation.X * x;
    const float xy = rotation.X * y;
    const float xz = rotation.X * z;
    const float yy = rotation.Y * y;
    const float yz = rotation.Y * z;
    const float zz = rotation.Z * z;

    result = Vector3(
        vector.X * (1.0f - yy - zz) + vector.Y * (xy - wz) + vector.Z * (xz + wy),
        vector.X * (xy + wz) + vector.Y * (1.0f - xx - zz) + vector.Z * (yz - wx),
        vector.X * (xz - wy) + vector.Y * (yz + wx) + vector.Z * (1.0f - xx - yy));
}

Vector3 Vector3::Transform(const Vector3& vector, const Quaternion& rotation)
{
    const float x = rotation.X + rotation.X;
    const float y = rotation.Y + rotation.Y;
    const float z = rotation.Z + rotation.Z;
    const float wx = rotation.W * x;
    const float wy = rotation.W * y;
    const float wz = rotation.W * z;
    const float xx = rotation.X * x;
    const float xy = rotation.X * y;
    const float xz = rotation.X * z;
    const float yy = rotation.Y * y;
    const float yz = rotation.Y * z;
    const float zz = rotation.Z * z;

    return Vector3(
        vector.X * (1.0f - yy - zz) + vector.Y * (xy - wz) + vector.Z * (xz + wy),
        vector.X * (xy + wz) + vector.Y * (1.0f - xx - zz) + vector.Z * (yz - wx),
        vector.X * (xz - wy) + vector.Y * (yz + wx) + vector.Z * (1.0f - xx - yy));
}

void Vector3::Transform(const Vector3& vector, const Matrix& transform, Vector4& result)
{
    result = Vector4(
        vector.X * transform.M11 + vector.Y * transform.M21 + vector.Z * transform.M31 + transform.M41,
        vector.X * transform.M12 + vector.Y * transform.M22 + vector.Z * transform.M32 + transform.M42,
        vector.X * transform.M13 + vector.Y * transform.M23 + vector.Z * transform.M33 + transform.M43,
        vector.X * transform.M14 + vector.Y * transform.M24 + vector.Z * transform.M34 + transform.M44);
}

void Vector3::Transform(const Vector3& vector, const Matrix& transform, Vector3& result)
{
    result = Vector3(
        vector.X * transform.M11 + vector.Y * transform.M21 + vector.Z * transform.M31 + transform.M41,
        vector.X * transform.M12 + vector.Y * transform.M22 + vector.Z * transform.M32 + transform.M42,
        vector.X * transform.M13 + vector.Y * transform.M23 + vector.Z * transform.M33 + transform.M43);
}

void Vector3::Transform(const Vector3* vectors, const Matrix& transform, Vector3* results, int32 vectorsCount)
{
    for (int32 i = 0; i < vectorsCount; i++)
    {
        Transform(vectors[i], transform, results[i]);
    }
}

Vector3 Vector3::Transform(const Vector3& vector, const Matrix& transform)
{
    return Vector3(
        vector.X * transform.M11 + vector.Y * transform.M21 + vector.Z * transform.M31 + transform.M41,
        vector.X * transform.M12 + vector.Y * transform.M22 + vector.Z * transform.M32 + transform.M42,
        vector.X * transform.M13 + vector.Y * transform.M23 + vector.Z * transform.M33 + transform.M43);
}

void Vector3::TransformCoordinate(const Vector3& coordinate, const Matrix& transform, Vector3& result)
{
    Vector4 vector;
    vector.X = coordinate.X * transform.M11 + coordinate.Y * transform.M21 + coordinate.Z * transform.M31 + transform.M41;
    vector.Y = coordinate.X * transform.M12 + coordinate.Y * transform.M22 + coordinate.Z * transform.M32 + transform.M42;
    vector.Z = coordinate.X * transform.M13 + coordinate.Y * transform.M23 + coordinate.Z * transform.M33 + transform.M43;
    vector.W = 1.0f / (coordinate.X * transform.M14 + coordinate.Y * transform.M24 + coordinate.Z * transform.M34 + transform.M44);
    result = Vector3(vector.X * vector.W, vector.Y * vector.W, vector.Z * vector.W);
}

void Vector3::TransformNormal(const Vector3& normal, const Matrix& transform, Vector3& result)
{
    result = Vector3(
        normal.X * transform.M11 + normal.Y * transform.M21 + normal.Z * transform.M31,
        normal.X * transform.M12 + normal.Y * transform.M22 + normal.Z * transform.M32,
        normal.X * transform.M13 + normal.Y * transform.M23 + normal.Z * transform.M33);
}

Vector3 Vector3::Project(const Vector3& vector, const Vector3& onNormal)
{
    const float sqrMag = Dot(onNormal, onNormal);
    if (sqrMag < ZeroTolerance)
        return Zero;
    return onNormal * Dot(vector, onNormal) / sqrMag;
}

void Vector3::Project(const Vector3& vector, float x, float y, float width, float height, float minZ, float maxZ, const Matrix& worldViewProjection, Vector3& result)
{
    Vector3 v;
    TransformCoordinate(vector, worldViewProjection, v);

    result = Vector3((1.0f + v.X) * 0.5f * width + x, (1.0f - v.Y) * 0.5f * height + y, v.Z * (maxZ - minZ) + minZ);
}

void Vector3::Unproject(const Vector3& vector, float x, float y, float width, float height, float minZ, float maxZ, const Matrix& worldViewProjection, Vector3& result)
{
    Matrix matrix;
    Matrix::Invert(worldViewProjection, matrix);

    const Vector3 v = Vector3((vector.X - x) / width * 2.0f - 1.0f, -((vector.Y - y) / height * 2.0f - 1.0f), (vector.Z - minZ) / (maxZ - minZ));

    TransformCoordinate(v, matrix, result);
}

void Vector3::CreateOrthonormalBasis(Vector3& xAxis, Vector3& yAxis, Vector3& zAxis)
{
    xAxis -= (xAxis | zAxis) / (zAxis | zAxis) * zAxis;
    yAxis -= (yAxis | zAxis) / (zAxis | zAxis) * zAxis;

    if (xAxis.LengthSquared() < ZeroTolerance)
        xAxis = yAxis ^ zAxis;
    if (yAxis.LengthSquared() < ZeroTolerance)
        yAxis = xAxis ^ zAxis;

    xAxis.Normalize();
    yAxis.Normalize();
    zAxis.Normalize();
}

void Vector3::FindBestAxisVectors(Vector3& firstAxis, Vector3& secondAxis) const
{
    const float absX = Math::Abs(X);
    const float absY = Math::Abs(Y);
    const float absZ = Math::Abs(Z);

    if (absZ > absX && absZ > absY)
        firstAxis = Vector3(1, 0, 0);
    else
        firstAxis = Vector3(0, 0, 1);

    firstAxis = (firstAxis - *this * (firstAxis | *this)).GetNormalized();
    secondAxis = firstAxis ^ *this;
}

float Vector3::TriangleArea(const Vector3& v0, const Vector3& v1, const Vector3& v2)
{
    return (v2 - v0 ^ v1 - v0).Length() * 0.5f;
}