using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;


namespace Common.Geom
{
/**
 * vector of three floats with some vector functions (right handed), which can safely passed by value to a native library
 * (if passed as pointer it must also be pinned in memory)
 */
[StructLayout(LayoutKind.Sequential)]
public struct Vector3f : IEquatable<Vector3f>
{
    private static Vector3f _zero = new Vector3f(0f,0f,0f);
    public static Vector3f ZERO
    {
        get
        {
            return _zero;
        }
    }
    private static Vector3f _one = new Vector3f(1f, 1f, 1f);
    public static Vector3f ONE
    {
        get
        {
            return _one;
        }
    }

    public float x, y, z;

    public Vector3f(float x = 0.0f, float y = 0.0f, float z = 0.0f)
    {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    public Vector3f(Vector3f other)
    {
        this.x = other.x;
        this.y = other.y;
        this.z = other.z;
    }

    public Vector3f(UnityEngine.Vector3 vec3)
    {
        this.x = vec3.x;
        this.y = vec3.z;
        this.z = vec3.y;
    }

    public Vector3f(UnityEngine.Vector2 vec2)
    {
        this.x = vec2.x;
        this.y = vec2.y;
        this.z = 0;
    }
    public void set(UnityEngine.Vector3 other)
    {
        this.x = other.x;
        this.y = other.z;
        this.z = other.y;
    }

    public void set(Vector3f other) {
        this.x = other.x;
        this.y = other.y;
        this.z = other.z;
    }

    public void set(float x, float y, float z)
    {
        this.x = x;
        this.y = y;
        this.z = z;
    }
    public bool isZero()
    {
        return x == 0f && y == 0f && z == 0f;
    }
    public bool isZero(float epsilon) {
        float epsSquare = epsilon*epsilon;
        return x * x <= epsSquare && y * y <= epsSquare && z * z <= epsSquare;
    }
    public void setToZero()
    {
        this.x = this.y = this.z = 0.0f;
    }

    public void setToAbsoluteValues()
    {
        if (x < 0.0f) x = -x;
        if (y < 0.0f) y = -y;
        if (z < 0.0f) z = -z;
    }

    public void add(ref Vector3f other) {
        x += other.x;
        y += other.y;
        z += other.z;
    }

    public void add(Vector3f other)
    {
        add(ref other);
    }

    public void subtract(ref Vector3f other) {
        x -= other.x;
        y -= other.y;
        z -= other.z;
    }
    public void subtract(Vector3f other)
    {
        subtract(ref other);
    }

    public void scale(float scale) {
        this.x *= scale;
        this.y *= scale;
        this.z *= scale;
    }

    public void scaleAdd(float scale, ref Vector3f other)
    {
        this.x += scale * other.x;
        this.y += scale * other.y;
        this.z += scale * other.z;
    }

    public void scaleAdd(float scale, Vector3f other)
    {
        scaleAdd(scale, ref other);
    }

    public void divide(float divisor)
    {
        this.x /= divisor;
        this.y /= divisor;
        this.z /= divisor;
    }

    /**
     * scalar product
     */
    public float dot(ref Vector3f other)
    {
        return this.x * other.x + this.y * other.y + this.z * other.z;
    }

    /**
     * scalar product
     */
    public float dot(Vector3f other)
    {
        return this.x * other.x + this.y * other.y + this.z * other.z;
    }

    /**
     * cross product stored in this vector
     */
    public void crossWith(ref Vector3f other)
    {
        float nx = this.y * other.z - this.z * other.y;
        float ny = this.z * other.x - this.x * other.z;
        float nz = this.x * other.y - this.y * other.x;
        this.x = nx;
        this.y = ny;
        this.z = nz;
    }
    public void crossWith(Vector3f other)
    {
        crossWith(ref other);
    }

    public bool normalize()
    {
        float l = (float)length();
        if (l == 0.0f) return false;
        x = x/l; y = y/l; z = z/l;
        return true;
    }

    public float lengthSquared() {
        return x * x  +  y * y  +  z * z;
    }
    public double length() {
        return Math.Sqrt(lengthSquared());
    }
    public float lengthf()
    {
        return (float)Math.Sqrt(lengthSquared());
    }

    public bool setLength(float newLength)
    {
        float l = (float)length();
        if (l == 0.0) return false;
        x = (x / l) * newLength;
        y = (y / l) * newLength;
        z = (z / l) * newLength;
        return true;
    }

    public float distanceSquared(ref Vector3f other) {
        float dx = other.x - x;
        float dy = other.y - y;
        float dz = other.z - z;
        return dx * dx + dy * dy + dz * dz;
    }
    public float distanceSquared(Vector3f other)
    {
        return distanceSquared(ref other); ;
    }
    public double distance(ref Vector3f other) {
        return Math.Sqrt(distanceSquared(ref other));
    }
    public double distance(Vector3f other)
    {
        return distance(ref other);
    }
    public float getMaxComponent() {
        return x >= y ? (x >= z ? x : z) : (y >= z ? y : z);
    }

    public void getLefthanded(ref UnityEngine.Vector3 lhVector3)
    {
        lhVector3.x = x;
        lhVector3.y = z;
        lhVector3.z = y;
    }

    public static implicit operator UnityEngine.Vector3(Vector3f v)
    {
        return new UnityEngine.Vector3(v.x, v.z, v.y);
    }
    public static implicit operator Vector3f(UnityEngine.Vector3 v)
    {
        return new Vector3f(v);
    }

    public static implicit operator Vector3f(UnityEngine.Vector2 v)
    {
        return new Vector3f(v);
    }

    public Vector3f Rotate(float angleDegree)
    {
        float rad = angleDegree * UnityEngine.Mathf.Deg2Rad;
        float rx = (x * UnityEngine.Mathf.Cos(rad)) - (y * UnityEngine.Mathf.Sin(rad));
        float ry = (y * UnityEngine.Mathf.Cos(rad)) + (x * UnityEngine.Mathf.Sin(rad));
        return new Vector3f(rx,ry);
    }

    public static int Side(Vector3f line, Vector3f p)
    {
        Vector3f perp = new Vector3f(-line.y, line.x);
        float d = p.dot(ref perp);
        return Math.Sign(d);
    }

    public Vector3f Ground()
    {
        return new Vector3f(this.x, this.y, 0);
    }

    public override bool Equals(object obj)
    {
        // If parameter is null return false.
        if (obj == null)
        {
            return false;
        }

        // If parameter cannot be cast to Point return false.
        Vector3f? v = obj as Vector3f?;
        if ((System.Object)v == null)
        {
            return false;
        }
        return this == v;
    }
    public bool Equals(Vector3f other)
    {
        return this == other;
    }

    public bool Equals(ref Vector3f other)
    {
        return this == other;
    }

    public override int GetHashCode()
    {

        return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode();
    }

    /**
     * indexing operator
     */
    public float this[int i] {
        get
        {
            switch (i)
            {
                case 0: return x;
                case 1: return y;
                case 2: return z;
                default: throw new IndexOutOfRangeException("Index " + i + " is out of " + GetType() + " Range");
            }
        }
        set {
            switch (i)
            {
                case 0: x = value; break;
                case 1: y = value; break;
                case 2: z = value; break;
                default: throw new IndexOutOfRangeException("Index " + i + " is out of " + GetType() + " Range");
            }
        }
    }

    public static Vector3f operator + (Vector3f a, Vector3f b)
    {
          return new Vector3f(a.x + b.x, a.y + b.y, a.z + b.z);
    }

    public static Vector3f operator + (Vector3f a, float offset)
    {
        return new Vector3f(a.x + offset, a.y + offset, a.z + offset);
    }

    public static Vector3f operator + (float offset, Vector3f a)
    {
        return new Vector3f(offset + a.x, offset + a.y, offset + a.z);
    }

    public static Vector3f operator - (Vector3f a, Vector3f b)
    {
        return new Vector3f(a.x - b.x, a.y - b.y, a.z - b.z);
    }

    public static Vector3f operator - (Vector3f a, float offset)
    {
        return new Vector3f(a.x - offset, a.y - offset, a.z - offset);
    }

    public static Vector3f operator - (float offset, Vector3f a)
    {
        return new Vector3f(offset - a.x, offset - a.y, offset - a.z);
    }

    public static Vector3f operator * (Vector3f a, float scale)
    {
        return new Vector3f(a.x * scale, a.y * scale, a.z * scale);
    }

    public static Vector3f operator * (float scale, Vector3f a)
    {
        return a * scale;
    }

    public static Vector3f operator / (Vector3f a, float scale)
    {
        return new Vector3f(a.x / scale, a.y / scale, a.z / scale);
    }

    /**
     * cross product
     */
    public static Vector3f operator *(Vector3f a, Vector3f b)
    {
        return new Vector3f(
            a.y * b.z - a.z * b.y,
            a.z * b.x - a.x * b.z,
            a.x * b.y - a.y * b.x
        );
    }


    public static bool operator == (Vector3f a, Vector3f b)
    {
        return a.x == b.x && a.y == b.y && a.z == b.z;
    }
    public static bool operator != (Vector3f a, Vector3f b)
    {
        return !(a == b);
    }
    public static bool operator < (Vector3f a, Vector3f b)
    {
        return a.lengthSquared() < b.lengthSquared();
    }
    public static bool operator <= (Vector3f a, Vector3f b)
    {
        return a.lengthSquared() <= b.lengthSquared();
    }
    public static bool operator > (Vector3f a, Vector3f b)
    {
        return a.lengthSquared() > b.lengthSquared();
    }

    public static bool operator >= (Vector3f a, Vector3f b)
    {
        return a.lengthSquared() >= b.lengthSquared();
    }

    public override string ToString() {
        return GetType().Name + "( " + x + " ; " + y + " ; " + z + " )";

    }
}

public static class Vector3Ext
{
    public static bool TryParse(this UnityEngine.Vector2 vec, string arg)
    {
        if (arg == null || arg == "" || !arg.Contains(";"))
        {
            vec = default(UnityEngine.Vector2);
            return false;
        }

        string[] strVals = arg.Split(';');

        if (strVals == null || strVals.Length != 2)
        {
            vec = default(UnityEngine.Vector2);
            return false;
        }

        float vX, vY;
        float.TryParse(strVals[0], out vX);
        float.TryParse(strVals[1], out vY);

        vec = new UnityEngine.Vector2();
        vec.x = vX;
        vec.y = vY;

        return true;
    }
    public static bool TryParse(this UnityEngine.Vector3 vec, string arg)
    {
        if (arg == null || arg == "" || !arg.Contains(";"))
            return false;

        string[] strVals = arg.Split(';');

        if(strVals == null || strVals.Length != 3)
            return false;

        float vX, vY, vZ;
        if(float.TryParse(strVals[0], out vX))
            return false;
        if(float.TryParse(strVals[1], out vY))
            return false;
        if(float.TryParse(strVals[2], out vZ))
            return false;

        vec = new UnityEngine.Vector3();
        vec.x = vX;
        vec.y = vY;
        vec.z = vZ;

        return true;
    }
}
}