package tc.oc.commons.bukkit.geometry;

import java.util.Optional;
import java.util.function.UnaryOperator;

import org.bukkit.Location;
import org.bukkit.util.Vector;

/**
 * A 4x3 matrix representing an affine transform in homogenous coordinates.
 *
 * May contain slight copypasta from http://geom-java.sourceforge.net/index.html
 */
public class AffineTransform implements UnaryOperator<Vector> {
    private final double
        m00, m01, m02, m03,
        m10, m11, m12, m13,
        m20, m21, m22, m23;

    private AffineTransform(double m00, double m01, double m02, double m03,
                            double m10, double m11, double m12, double m13,
                            double m20, double m21, double m22, double m23) {

        this.m00 = m00; this.m01 = m01; this.m02 = m02; this.m03 = m03;
        this.m10 = m10; this.m11 = m11; this.m12 = m12; this.m13 = m13;
        this.m20 = m20; this.m21 = m21; this.m22 = m22; this.m23 = m23;
    }

    @Override
    public ImVector apply(Vector v) {
        return ImVector.of(m00 * v.getX() + m01 * v.getY() + m02 * v.getZ() + m03,
                                  m10 * v.getX() + m11 * v.getY() + m12 * v.getZ() + m13,
                                  m20 * v.getX() + m21 * v.getY() + m22 * v.getZ() + m23);
    }

    public Direction apply(Direction dir) {
        final ImVector v = dir.toVector();
        return Direction.fromVector(m00 * v.getX() + m01 * v.getY() + m02 * v.getZ(),
                                       m10 * v.getX() + m11 * v.getY() + m12 * v.getZ(),
                                       m20 * v.getX() + m21 * v.getY() + m22 * v.getZ());
    }

    public Location apply(Location location) {
        final Direction dir = apply(Direction.fromLocation(location));
        return apply(location.toVector()).toLocation(location.getWorld(),
                                                     (float) dir.yawDegrees(),
                                                     (float) dir.pitchDegrees());
    }

    public boolean isIdentity() {
        return m00 == 1 && m01 == 0 && m02 == 0 && m03 == 0 &&
               m10 == 0 && m11 == 1 && m12 == 0 && m13 == 0 &&
               m20 == 0 && m21 == 0 && m22 == 1 && m23 == 0;
    }

    private double determinant() {
        return   m00 * (m11 * m22 - m12 * m21)
               - m01 * (m10 * m22 - m20 * m12)
               + m02 * (m10 * m21 - m20 * m11);
    }

    public boolean isInvertible() {
        return determinant() != 0;
    }

    public Optional<AffineTransform> inverse() {
        final double det = this.determinant();
        if(det == 0) return Optional.empty();

        return Optional.of(new AffineTransform(
            (m11 * m22 - m21 * m12) / det,
            (m21 * m01 - m01 * m22) / det,
            (m01 * m12 - m11 * m02) / det,
            (  m01 * (m22 * m13 - m12 * m23)
             + m02 * (m11 * m23 - m21 * m13)
             - m03 * (m11 * m22 - m21 * m12)) / det,

            (m20 * m12 - m10 * m22) / det,
            (m00 * m22 - m20 * m02) / det,
            (m10 * m02 - m00 * m12) / det,
            (  m00 * (m12 * m23 - m22 * m13)
             - m02 * (m10 * m23 - m20 * m13)
             + m03 * (m10 * m22 - m20 * m12)) / det,

            (m10 * m21 - m20 * m11) / det,
            (m20 * m01 - m00 * m21) / det,
            (m00 * m11 - m10 * m01) / det,
            (  m00 * (m21 * m13 - m11 * m23)
             + m01 * (m10 * m23 - m20 * m13)
             - m03 * (m10 * m21 - m20 * m11)) / det
        ));
    }

    public AffineTransform append(AffineTransform o) {
        return new AffineTransform(m00 * o.m00 + m01 * o.m10 + m02 * o.m20,
                                   m00 * o.m01 + m01 * o.m11 + m02 * o.m21,
                                   m00 * o.m02 + m01 * o.m12 + m02 * o.m22,
                                   m00 * o.m03 + m01 * o.m13 + m02 * o.m23 + m03,

                                   m10 * o.m00 + m11 * o.m10 + m12 * o.m20,
                                   m10 * o.m01 + m11 * o.m11 + m12 * o.m21,
                                   m10 * o.m02 + m11 * o.m12 + m12 * o.m22,
                                   m10 * o.m03 + m11 * o.m13 + m12 * o.m23 + m13,

                                   m20 * o.m00 + m21 * o.m10 + m22 * o.m20,
                                   m20 * o.m01 + m21 * o.m11 + m22 * o.m21,
                                   m20 * o.m02 + m21 * o.m12 + m22 * o.m22,
                                   m20 * o.m03 + m21 * o.m13 + m22 * o.m23 + m23);
    }

    public AffineTransform prepend(AffineTransform o) {
        return o.append(this);
    }

    private static final AffineTransform IDENTITY = of(1, 0, 0,
                                                       0, 1, 0,
                                                       0, 0, 1);
    public static AffineTransform identity() {
        return IDENTITY;
    }

    public static AffineTransform of(double m00, double m01, double m02, double m03,
                                     double m10, double m11, double m12, double m13,
                                     double m20, double m21, double m22, double m23) {
        return new AffineTransform(m00, m01, m02, m03,
                                   m10, m11, m12, m13,
                                   m20, m21, m22, m23);
    }

    public static AffineTransform of(double m00, double m01, double m02,
                                     double m10, double m11, double m12,
                                     double m20, double m21, double m22) {
        return of(m00, m01, m02, 0,
                  m10, m11, m12, 0,
                  m20, m21, m22, 0);
    }

    public static AffineTransform of(LinearFunction x,
                                     LinearFunction y,
                                     LinearFunction z) {
        return new AffineTransform(x.linear(), 0, 0, x.constant(),
                                   0, y.linear(), 0, y.constant(),
                                   0, 0, z.linear(), z.constant());
    }

    public static AffineTransform translate(double x, double y, double z) {
        return new AffineTransform(1, 0, 0, x,
                                   0, 1, 0, y,
                                   0, 0, 1, z);
    }

    public static AffineTransform translate(Vector v) {
        return translate(v.getX(), v.getY(), v.getZ());
    }

    public static AffineTransform rotateX(double degrees) {
        double sin = sin(degrees);
        double cos = cos(degrees);
        return of(1, 0,   0,
                  0, cos, -sin,
                  0, sin, cos);
    }

    public static AffineTransform rotateY(double degrees) {
        double sin = sin(degrees);
        double cos = cos(degrees);
        return of(cos,  0,  sin,
                  0,    1,  0,
                  -sin, 0,  cos);
    }

    public static AffineTransform rotateZ(double degrees) {
        double sin = sin(degrees);
        double cos = cos(degrees);
        return of(cos, -sin, 0,
                  sin, cos,  0,
                  0,   0,    1);
    }

    public static AffineTransform forYaw(double degrees) {
        return rotateY(-degrees);
    }

    public static AffineTransform forPitch(double degrees) {
        return rotateX(degrees);
    }

    public static AffineTransform forDirection(double yaw, double pitch) {
        return forYaw(yaw).append(forPitch(pitch));
    }

    public static AffineTransform forDirection(Location location) {
        return forDirection(location.getYaw(), location.getPitch());
    }

    public static AffineTransform mirror(boolean x, boolean y, boolean z) {
        return of(x ? 1 : -1, 0, 0,
                  0, y ? 1 : -1, 0,
                  0, 0, z ? 1 : -1);
    }

    public static AffineTransform mirrorX() {
        return mirror(true, false, false);
    }

    public static AffineTransform mirrorY() {
        return mirror(false, true, false);
    }

    public static AffineTransform mirrorZ() {
        return mirror(false, false, true);
    }

    protected static double sin(double degrees) {
        int t = (int) degrees;
        if(t == degrees && t % 90 == 0) {
            t %= 360;
            if(t < 0) t += 360;

            switch(t) {
                case 0:
                case 180:
                    return 0;
                case 90:
                    return 1;
                case 270:
                    return -1;
            }
        }
        return Math.sin(Math.toRadians(degrees));
    }

    protected static double cos(double degrees) {
        int t = (int) degrees;
        if(t == degrees && t % 90 == 0) {
            t %= 360;
            if(t < 0) t += 360;

            switch(t) {
                case 90:
                case 270:
                    return 0;
                case 0:
                    return 1;
                case 180:
                    return -1;
            }
        }
        return Math.cos(Math.toRadians(degrees));
    }
}
