/*******************************************************************************
 * Copyright (c) 2013, Daniel Murphy
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 * 	* Redistributions of source code must retain the above copyright notice,
 * 	  this list of conditions and the following disclaimer.
 * 	* Redistributions in binary form must reproduce the above copyright notice,
 * 	  this list of conditions and the following disclaimer in the documentation
 * 	  and/or other materials provided with the distribution.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/
package org.jbox2d.common;

import java.io.Serializable;

/**
 * A 2-by-2 matrix. Stored in column-major order.
 */
public class Mat22 implements Serializable {
  private static final long serialVersionUID = 2L;

  public final Vec2 ex, ey;

  /** Convert the matrix to printable format. */
  @Override
  public String toString() {
    String s = "";
    s += "[" + ex.x + "," + ey.x + "]\n";
    s += "[" + ex.y + "," + ey.y + "]";
    return s;
  }

  /**
   * Construct zero matrix. Note: this is NOT an identity matrix! djm fixed double allocation
   * problem
   */
  public Mat22() {
    ex = new Vec2();
    ey = new Vec2();
  }

  /**
   * Create a matrix with given vectors as columns.
   * 
   * @param c1 Column 1 of matrix
   * @param c2 Column 2 of matrix
   */
  public Mat22(final Vec2 c1, final Vec2 c2) {
    ex = c1.clone();
    ey = c2.clone();
  }

  /**
   * Create a matrix from four floats.
   * 
   * @param exx
   * @param col2x
   * @param exy
   * @param col2y
   */
  public Mat22(final float exx, final float col2x, final float exy, final float col2y) {
    ex = new Vec2(exx, exy);
    ey = new Vec2(col2x, col2y);
  }

  /**
   * Set as a copy of another matrix.
   * 
   * @param m Matrix to copy
   */
  public final Mat22 set(final Mat22 m) {
    ex.x = m.ex.x;
    ex.y = m.ex.y;
    ey.x = m.ey.x;
    ey.y = m.ey.y;
    return this;
  }

  public final Mat22 set(final float exx, final float col2x, final float exy, final float col2y) {
    ex.x = exx;
    ex.y = exy;
    ey.x = col2x;
    ey.y = col2y;
    return this;
  }

  /**
   * Return a clone of this matrix. djm fixed double allocation
   */
  @Override
  public final Mat22 clone() {
    return new Mat22(ex, ey);
  }

  /**
   * Set as a matrix representing a rotation.
   * 
   * @param angle Rotation (in radians) that matrix represents.
   */
  public final void set(final float angle) {
    final float c = MathUtils.cos(angle), s = MathUtils.sin(angle);
    ex.x = c;
    ey.x = -s;
    ex.y = s;
    ey.y = c;
  }

  /**
   * Set as the identity matrix.
   */
  public final void setIdentity() {
    ex.x = 1.0f;
    ey.x = 0.0f;
    ex.y = 0.0f;
    ey.y = 1.0f;
  }

  /**
   * Set as the zero matrix.
   */
  public final void setZero() {
    ex.x = 0.0f;
    ey.x = 0.0f;
    ex.y = 0.0f;
    ey.y = 0.0f;
  }

  /**
   * Extract the angle from this matrix (assumed to be a rotation matrix).
   * 
   * @return
   */
  public final float getAngle() {
    return MathUtils.atan2(ex.y, ex.x);
  }

  /**
   * Set by column vectors.
   * 
   * @param c1 Column 1
   * @param c2 Column 2
   */
  public final void set(final Vec2 c1, final Vec2 c2) {
    ex.x = c1.x;
    ey.x = c2.x;
    ex.y = c1.y;
    ey.y = c2.y;
  }

  /** Returns the inverted Mat22 - does NOT invert the matrix locally! */
  public final Mat22 invert() {
    final float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
    final Mat22 B = new Mat22();
    float det = a * d - b * c;
    if (det != 0) {
      det = 1.0f / det;
    }
    B.ex.x = det * d;
    B.ey.x = -det * b;
    B.ex.y = -det * c;
    B.ey.y = det * a;
    return B;
  }

  public final Mat22 invertLocal() {
    final float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
    float det = a * d - b * c;
    if (det != 0) {
      det = 1.0f / det;
    }
    ex.x = det * d;
    ey.x = -det * b;
    ex.y = -det * c;
    ey.y = det * a;
    return this;
  }

  public final void invertToOut(final Mat22 out) {
    final float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
    float det = a * d - b * c;
    // b2Assert(det != 0.0f);
    det = 1.0f / det;
    out.ex.x = det * d;
    out.ey.x = -det * b;
    out.ex.y = -det * c;
    out.ey.y = det * a;
  }



  /**
   * Return the matrix composed of the absolute values of all elements. djm: fixed double allocation
   * 
   * @return Absolute value matrix
   */
  public final Mat22 abs() {
    return new Mat22(MathUtils.abs(ex.x), MathUtils.abs(ey.x), MathUtils.abs(ex.y),
        MathUtils.abs(ey.y));
  }

  /* djm: added */
  public final void absLocal() {
    ex.absLocal();
    ey.absLocal();
  }

  /**
   * Return the matrix composed of the absolute values of all elements.
   * 
   * @return Absolute value matrix
   */
  public final static Mat22 abs(final Mat22 R) {
    return R.abs();
  }

  /* djm created */
  public static void absToOut(final Mat22 R, final Mat22 out) {
    out.ex.x = MathUtils.abs(R.ex.x);
    out.ex.y = MathUtils.abs(R.ex.y);
    out.ey.x = MathUtils.abs(R.ey.x);
    out.ey.y = MathUtils.abs(R.ey.y);
  }

  /**
   * Multiply a vector by this matrix.
   * 
   * @param v Vector to multiply by matrix.
   * @return Resulting vector
   */
  public final Vec2 mul(final Vec2 v) {
    return new Vec2(ex.x * v.x + ey.x * v.y, ex.y * v.x + ey.y * v.y);
  }

  public final void mulToOut(final Vec2 v, final Vec2 out) {
    final float tempy = ex.y * v.x + ey.y * v.y;
    out.x = ex.x * v.x + ey.x * v.y;
    out.y = tempy;
  }

  public final void mulToOutUnsafe(final Vec2 v, final Vec2 out) {
    assert (v != out);
    out.x = ex.x * v.x + ey.x * v.y;
    out.y = ex.y * v.x + ey.y * v.y;
  }


  /**
   * Multiply another matrix by this one (this one on left). djm optimized
   * 
   * @param R
   * @return
   */
  public final Mat22 mul(final Mat22 R) {
    /*
     * Mat22 C = new Mat22();C.set(this.mul(R.ex), this.mul(R.ey));return C;
     */
    final Mat22 C = new Mat22();
    C.ex.x = ex.x * R.ex.x + ey.x * R.ex.y;
    C.ex.y = ex.y * R.ex.x + ey.y * R.ex.y;
    C.ey.x = ex.x * R.ey.x + ey.x * R.ey.y;
    C.ey.y = ex.y * R.ey.x + ey.y * R.ey.y;
    // C.set(ex,col2);
    return C;
  }

  public final Mat22 mulLocal(final Mat22 R) {
    mulToOut(R, this);
    return this;
  }

  public final void mulToOut(final Mat22 R, final Mat22 out) {
    final float tempy1 = this.ex.y * R.ex.x + this.ey.y * R.ex.y;
    final float tempx1 = this.ex.x * R.ex.x + this.ey.x * R.ex.y;
    out.ex.x = tempx1;
    out.ex.y = tempy1;
    final float tempy2 = this.ex.y * R.ey.x + this.ey.y * R.ey.y;
    final float tempx2 = this.ex.x * R.ey.x + this.ey.x * R.ey.y;
    out.ey.x = tempx2;
    out.ey.y = tempy2;
  }

  public final void mulToOutUnsafe(final Mat22 R, final Mat22 out) {
    assert (out != R);
    assert (out != this);
    out.ex.x = this.ex.x * R.ex.x + this.ey.x * R.ex.y;
    out.ex.y = this.ex.y * R.ex.x + this.ey.y * R.ex.y;
    out.ey.x = this.ex.x * R.ey.x + this.ey.x * R.ey.y;
    out.ey.y = this.ex.y * R.ey.x + this.ey.y * R.ey.y;
  }

  /**
   * Multiply another matrix by the transpose of this one (transpose of this one on left). djm:
   * optimized
   * 
   * @param B
   * @return
   */
  public final Mat22 mulTrans(final Mat22 B) {
    /*
     * Vec2 c1 = new Vec2(Vec2.dot(this.ex, B.ex), Vec2.dot(this.ey, B.ex)); Vec2 c2 = new
     * Vec2(Vec2.dot(this.ex, B.ey), Vec2.dot(this.ey, B.ey)); Mat22 C = new Mat22(); C.set(c1, c2);
     * return C;
     */
    final Mat22 C = new Mat22();

    C.ex.x = Vec2.dot(this.ex, B.ex);
    C.ex.y = Vec2.dot(this.ey, B.ex);

    C.ey.x = Vec2.dot(this.ex, B.ey);
    C.ey.y = Vec2.dot(this.ey, B.ey);
    return C;
  }

  public final Mat22 mulTransLocal(final Mat22 B) {
    mulTransToOut(B, this);
    return this;
  }

  public final void mulTransToOut(final Mat22 B, final Mat22 out) {
    /*
     * out.ex.x = Vec2.dot(this.ex, B.ex); out.ex.y = Vec2.dot(this.ey, B.ex); out.ey.x =
     * Vec2.dot(this.ex, B.ey); out.ey.y = Vec2.dot(this.ey, B.ey);
     */
    final float x1 = this.ex.x * B.ex.x + this.ex.y * B.ex.y;
    final float y1 = this.ey.x * B.ex.x + this.ey.y * B.ex.y;
    final float x2 = this.ex.x * B.ey.x + this.ex.y * B.ey.y;
    final float y2 = this.ey.x * B.ey.x + this.ey.y * B.ey.y;
    out.ex.x = x1;
    out.ey.x = x2;
    out.ex.y = y1;
    out.ey.y = y2;
  }

  public final void mulTransToOutUnsafe(final Mat22 B, final Mat22 out) {
    assert (B != out);
    assert (this != out);
    out.ex.x = this.ex.x * B.ex.x + this.ex.y * B.ex.y;
    out.ey.x = this.ex.x * B.ey.x + this.ex.y * B.ey.y;
    out.ex.y = this.ey.x * B.ex.x + this.ey.y * B.ex.y;
    out.ey.y = this.ey.x * B.ey.x + this.ey.y * B.ey.y;
  }

  /**
   * Multiply a vector by the transpose of this matrix.
   * 
   * @param v
   * @return
   */
  public final Vec2 mulTrans(final Vec2 v) {
    // return new Vec2(Vec2.dot(v, ex), Vec2.dot(v, col2));
    return new Vec2((v.x * ex.x + v.y * ex.y), (v.x * ey.x + v.y * ey.y));
  }

  /* djm added */
  public final void mulTransToOut(final Vec2 v, final Vec2 out) {
    /*
     * out.x = Vec2.dot(v, ex); out.y = Vec2.dot(v, col2);
     */
    final float tempx = v.x * ex.x + v.y * ex.y;
    out.y = v.x * ey.x + v.y * ey.y;
    out.x = tempx;
  }

  /**
   * Add this matrix to B, return the result.
   * 
   * @param B
   * @return
   */
  public final Mat22 add(final Mat22 B) {
    // return new Mat22(ex.add(B.ex), col2.add(B.ey));
    Mat22 m = new Mat22();
    m.ex.x = ex.x + B.ex.x;
    m.ex.y = ex.y + B.ex.y;
    m.ey.x = ey.x + B.ey.x;
    m.ey.y = ey.y + B.ey.y;
    return m;
  }

  /**
   * Add B to this matrix locally.
   * 
   * @param B
   * @return
   */
  public final Mat22 addLocal(final Mat22 B) {
    // ex.addLocal(B.ex);
    // col2.addLocal(B.ey);
    ex.x += B.ex.x;
    ex.y += B.ex.y;
    ey.x += B.ey.x;
    ey.y += B.ey.y;
    return this;
  }

  /**
   * Solve A * x = b where A = this matrix.
   * 
   * @return The vector x that solves the above equation.
   */
  public final Vec2 solve(final Vec2 b) {
    final float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
    float det = a11 * a22 - a12 * a21;
    if (det != 0.0f) {
      det = 1.0f / det;
    }
    final Vec2 x = new Vec2(det * (a22 * b.x - a12 * b.y), det * (a11 * b.y - a21 * b.x));
    return x;
  }

  public final void solveToOut(final Vec2 b, final Vec2 out) {
    final float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
    float det = a11 * a22 - a12 * a21;
    if (det != 0.0f) {
      det = 1.0f / det;
    }
    final float tempy = det * (a11 * b.y - a21 * b.x);
    out.x = det * (a22 * b.x - a12 * b.y);
    out.y = tempy;
  }

  public final static Vec2 mul(final Mat22 R, final Vec2 v) {
    // return R.mul(v);
    return new Vec2(R.ex.x * v.x + R.ey.x * v.y, R.ex.y * v.x + R.ey.y * v.y);
  }

  public final static void mulToOut(final Mat22 R, final Vec2 v, final Vec2 out) {
    final float tempy = R.ex.y * v.x + R.ey.y * v.y;
    out.x = R.ex.x * v.x + R.ey.x * v.y;
    out.y = tempy;
  }

  public final static void mulToOutUnsafe(final Mat22 R, final Vec2 v, final Vec2 out) {
    assert (v != out);
    out.x = R.ex.x * v.x + R.ey.x * v.y;
    out.y = R.ex.y * v.x + R.ey.y * v.y;
  }

  public final static Mat22 mul(final Mat22 A, final Mat22 B) {
    // return A.mul(B);
    final Mat22 C = new Mat22();
    C.ex.x = A.ex.x * B.ex.x + A.ey.x * B.ex.y;
    C.ex.y = A.ex.y * B.ex.x + A.ey.y * B.ex.y;
    C.ey.x = A.ex.x * B.ey.x + A.ey.x * B.ey.y;
    C.ey.y = A.ex.y * B.ey.x + A.ey.y * B.ey.y;
    return C;
  }

  public final static void mulToOut(final Mat22 A, final Mat22 B, final Mat22 out) {
    final float tempy1 = A.ex.y * B.ex.x + A.ey.y * B.ex.y;
    final float tempx1 = A.ex.x * B.ex.x + A.ey.x * B.ex.y;
    final float tempy2 = A.ex.y * B.ey.x + A.ey.y * B.ey.y;
    final float tempx2 = A.ex.x * B.ey.x + A.ey.x * B.ey.y;
    out.ex.x = tempx1;
    out.ex.y = tempy1;
    out.ey.x = tempx2;
    out.ey.y = tempy2;
  }

  public final static void mulToOutUnsafe(final Mat22 A, final Mat22 B, final Mat22 out) {
    assert (out != A);
    assert (out != B);
    out.ex.x = A.ex.x * B.ex.x + A.ey.x * B.ex.y;
    out.ex.y = A.ex.y * B.ex.x + A.ey.y * B.ex.y;
    out.ey.x = A.ex.x * B.ey.x + A.ey.x * B.ey.y;
    out.ey.y = A.ex.y * B.ey.x + A.ey.y * B.ey.y;
  }

  public final static Vec2 mulTrans(final Mat22 R, final Vec2 v) {
    return new Vec2((v.x * R.ex.x + v.y * R.ex.y), (v.x * R.ey.x + v.y * R.ey.y));
  }

  public final static void mulTransToOut(final Mat22 R, final Vec2 v, final Vec2 out) {
    float outx = v.x * R.ex.x + v.y * R.ex.y;
    out.y = v.x * R.ey.x + v.y * R.ey.y;
    out.x = outx;
  }

  public final static void mulTransToOutUnsafe(final Mat22 R, final Vec2 v, final Vec2 out) {
    assert (out != v);
    out.y = v.x * R.ey.x + v.y * R.ey.y;
    out.x = v.x * R.ex.x + v.y * R.ex.y;
  }

  public final static Mat22 mulTrans(final Mat22 A, final Mat22 B) {
    final Mat22 C = new Mat22();
    C.ex.x = A.ex.x * B.ex.x + A.ex.y * B.ex.y;
    C.ex.y = A.ey.x * B.ex.x + A.ey.y * B.ex.y;
    C.ey.x = A.ex.x * B.ey.x + A.ex.y * B.ey.y;
    C.ey.y = A.ey.x * B.ey.x + A.ey.y * B.ey.y;
    return C;
  }

  public final static void mulTransToOut(final Mat22 A, final Mat22 B, final Mat22 out) {
    final float x1 = A.ex.x * B.ex.x + A.ex.y * B.ex.y;
    final float y1 = A.ey.x * B.ex.x + A.ey.y * B.ex.y;
    final float x2 = A.ex.x * B.ey.x + A.ex.y * B.ey.y;
    final float y2 = A.ey.x * B.ey.x + A.ey.y * B.ey.y;

    out.ex.x = x1;
    out.ex.y = y1;
    out.ey.x = x2;
    out.ey.y = y2;
  }

  public final static void mulTransToOutUnsafe(final Mat22 A, final Mat22 B, final Mat22 out) {
    assert (A != out);
    assert (B != out);
    out.ex.x = A.ex.x * B.ex.x + A.ex.y * B.ex.y;
    out.ex.y = A.ey.x * B.ex.x + A.ey.y * B.ex.y;
    out.ey.x = A.ex.x * B.ey.x + A.ex.y * B.ey.y;
    out.ey.y = A.ey.x * B.ey.x + A.ey.y * B.ey.y;
  }

  public final static Mat22 createRotationalTransform(float angle) {
    Mat22 mat = new Mat22();
    final float c = MathUtils.cos(angle);
    final float s = MathUtils.sin(angle);
    mat.ex.x = c;
    mat.ey.x = -s;
    mat.ex.y = s;
    mat.ey.y = c;
    return mat;
  }

  public final static void createRotationalTransform(float angle, Mat22 out) {
    final float c = MathUtils.cos(angle);
    final float s = MathUtils.sin(angle);
    out.ex.x = c;
    out.ey.x = -s;
    out.ex.y = s;
    out.ey.y = c;
  }

  public final static Mat22 createScaleTransform(float scale) {
    Mat22 mat = new Mat22();
    mat.ex.x = scale;
    mat.ey.y = scale;
    return mat;
  }

  public final static void createScaleTransform(float scale, Mat22 out) {
    out.ex.x = scale;
    out.ey.y = scale;
  }

  @Override
  public int hashCode() {
    final int prime = 31;
    int result = 1;
    result = prime * result + ((ex == null) ? 0 : ex.hashCode());
    result = prime * result + ((ey == null) ? 0 : ey.hashCode());
    return result;
  }

  @Override
  public boolean equals(Object obj) {
    if (this == obj) return true;
    if (obj == null) return false;
    if (getClass() != obj.getClass()) return false;
    Mat22 other = (Mat22) obj;
    if (ex == null) {
      if (other.ex != null) return false;
    } else if (!ex.equals(other.ex)) return false;
    if (ey == null) {
      if (other.ey != null) return false;
    } else if (!ey.equals(other.ey)) return false;
    return true;
  }
}
