package nxm.sys.lib;

import nxm.sys.inc.Constants;

/**
  A class to handle Transformation Matrix operations.

  @author Jeff Schoen

*/
public class TMatrix implements Constants {

  /** Azimuth rotation type   */ public static final int AZIM  = 1;
  /** Elevation rotation type */ public static final int ELEV  = 2;
  /** Roll rotation type      */ public static final int ROLL  = 3;
  /** Yaw rotation type       */ public static final int YAW   = 4;
  /** Pitch rotation type     */ public static final int PITCH = 5;

  // matrix, origin, scale
  /** matrix row 1, column 1 */ public double a11;
  /** matrix row 1, column 2 */ public double a21;
  /** matrix row 1, column 3 */ public double a31;
  /** origin row 1           */ public double b1;
  /** scale row 1            */ public double s1;
  /** matrix row 2, column 1 */ public double a12;
  /** matrix row 2, column 2 */ public double a22;
  /** matrix row 2, column 3 */ public double a32;
  /** origin row 2           */ public double b2;
  /** scale row 2            */ public double s2;
  /** matrix row 3, column 1 */ public double a13;
  /** matrix row 3, column 2 */ public double a23;
  /** matrix row 3, column 3 */ public double a33;
  /** origin row 3           */ public double b3;
  /** scale row 3            */ public double s3;

  private TMatrix r;

  /** Constructor that creates an identity matrix via {@link TMatrix#identity()}. */
  public TMatrix () {
    identity();
  }

  /** TMatrix copy constructor via {@link TMatrix#copy(TMatrix, TMatrix)}
      @param p TMatrix to be copied
   */
  public TMatrix (TMatrix p) {
    copy(p,this);
  }

  /** set to the identity matrix */
  public void identity () {
    a11=1; a21=0; a31=0; b1=0; s1=1;
    a12=0; a22=1; a32=0; b2=0; s2=1;
    a13=0; a23=0; a33=1; b3=0; s3=1;
  }

  /** set the origin
   *  @param b1 origin x coordinate
   *  @param b2 origin y coordinate
   *  @param b3 origin z coordinate
   */
  public void offset (double b1, double b2, double b3) {
    this.b1=b1; this.b2=b2; this.b3=b3;
  }

  /** apply rotation in degrees
   *  @param angle degrees of rotATION
   *  @param type type of rotation (AZIM, YAW, ...)
   */
  public void rotate (double angle, int type) {
    if (angle==0) return;
    double a = angle*DEG2RAD, sa = Math.sin(a), ca = Math.cos(a);
    if (r==null) r = new TMatrix(); else r.identity();
    switch (type) {
    case AZIM:  r.a11=ca; r.a12=-sa; r.a21=sa; r.a22=ca; break;
    case YAW:   r.a11=ca; r.a12=-sa; r.a21=sa; r.a22=ca; break;
    case ELEV:  r.a11=ca; r.a13=-sa; r.a31=sa; r.a33=ca; break;
    case PITCH: r.a11=ca; r.a13=sa; r.a31=-sa; r.a33=ca; break;
    case ROLL:  r.a22=ca; r.a23=-sa; r.a32=sa; r.a33=ca; break;
    }
    multiply (r,this,this);
  }

  /** multiply two matrices this = (this * q)
   *  @param q second source TMatrix
   */
  public void multiply (TMatrix q) {
    multiply (this,q,this);
  }

  /** multiply two matrices r = (p * q)
   *  @param p first source TMatrix
   *  @param q second source TMatrix
   *  @param r destination TMatrix
   */
  public static void multiply (TMatrix p, TMatrix q, TMatrix r) {
    if (p==r) p = new TMatrix(p);
    if (q==r) q = new TMatrix(q);
    r.a11 = p.a11*q.a11 + p.a12*q.a21 + p.a13*q.a31;
    r.a12 = p.a11*q.a12 + p.a12*q.a22 + p.a13*q.a32;
    r.a13 = p.a11*q.a13 + p.a12*q.a23 + p.a13*q.a33;
    r.b1  = p.a11*q.b1  + p.a12*q.b2  + p.a13*q.b3 + p.b1;
    r.a21 = p.a21*q.a11 + p.a22*q.a21 + p.a23*q.a31;
    r.a22 = p.a21*q.a12 + p.a22*q.a22 + p.a23*q.a32;
    r.a23 = p.a21*q.a13 + p.a22*q.a23 + p.a23*q.a33;
    r.b2  = p.a21*q.b1  + p.a22*q.b2  + p.a23*q.b3 + p.b2;
    r.a31 = p.a31*q.a11 + p.a32*q.a21 + p.a33*q.a31;
    r.a32 = p.a31*q.a12 + p.a32*q.a22 + p.a33*q.a32;
    r.a33 = p.a31*q.a13 + p.a32*q.a23 + p.a33*q.a33;
    r.b3  = p.a31*q.b1  + p.a32*q.b2  + p.a33*q.b3 + p.b3;
  }

  /** transpose a matrix */
  public void transpose () { transpose (this,this); }
  /** transpose this TMatrix and place it in the destination TMatrix
   *  @param q destination TMatrix
   */
  public void transpose (TMatrix q) { transpose (this,q); }
  /** transpose first TMatrix and place it in the second TMatrix
   *  @param p source TMatrix
   *  @param q destination TMatrix
   */
  public static void transpose (TMatrix p, TMatrix q) {
    double t,tb1,tb2,tb3;
    t=p.a12; q.a12=p.a21; q.a21=t;
    t=p.a13; q.a13=p.a31; q.a31=t;
    t=p.a32; q.a32=p.a23; q.a23=t;
    tb1=p.b1; tb2=p.b2; tb3=p.b3;
    q.b1 = -(q.a11*tb1 + q.a12*tb2 + q.a13*tb3);
    q.b2 = -(q.a21*tb1 + q.a22*tb2 + q.a23*tb3);
    q.b3 = -(q.a31*tb1 + q.a32*tb2 + q.a33*tb3);
  }

  /** invert a matrix */
  public void invert () { invert (this,this); }
  /** invert this TMatrix and place it in destination TMatrix
   *  @param q destination TMatrix */
  public void invert (TMatrix q) { invert (this,q); }
  /** invert first TMatrix and place it in the second TMatrix
   *  @param p source TMatrix
   *  @param q destination TMatrix
   */
  public static void invert (TMatrix p, TMatrix q) {
    if (p==q) p = new TMatrix(p);
    q.a11 = +( p.a22*p.a33 - p.a23*p.a32 );
    q.a12 = -( p.a12*p.a33 - p.a13*p.a32 );
    q.a13 = +( p.a12*p.a23 - p.a13*p.a22 );
    q.a21 = -( p.a21*p.a33 - p.a23*p.a31 );
    q.a22 = +( p.a11*p.a33 - p.a13*p.a31 );
    q.a23 = -( p.a11*p.a23 - p.a13*p.a21 );
    q.a31 = +( p.a21*p.a32 - p.a22*p.a31 );
    q.a32 = -( p.a11*p.a32 - p.a12*p.a31 );
    q.a33 = +( p.a11*p.a22 - p.a12*p.a21 );
    double det = p.a11*q.a11 + p.a12*q.a21 + p.a13*q.a31;
    if (det!=0) det = 1.0/det;
    q.a11*=det; q.a12*=det; q.a13*=det;
    q.a21*=det; q.a22*=det; q.a23*=det;
    q.a31*=det; q.a32*=det; q.a33*=det;
    q.b1 = -(q.a11*p.b1 + q.a12*p.b2 + q.a13*p.b3);
    q.b2 = -(q.a21*p.b1 + q.a22*p.b2 + q.a23*p.b3);
    q.b3 = -(q.a31*p.b1 + q.a32*p.b2 + q.a33*p.b3);
  }

  /** copy contents of one matrix to another */
  public void copy () { copy (this,this); }
  /** Copy this TMatrix to another
   *  @param q desination TMatrix
   */
  public void copy (TMatrix q) { copy (this,q); }
  /** Copies first TMatrix to second TMatrix
   *  @param p source TMatrix
   *  @param q destination TMatrix
   */
  public static void copy (TMatrix p, TMatrix q) {
    q.a11=p.a11; q.a21=p.a21; q.a31=p.a31; q.b1=p.b1; q.s1=p.s1;
    q.a12=p.a12; q.a22=p.a22; q.a32=p.a32; q.b2=p.b2; q.s2=p.s2;
    q.a13=p.a13; q.a23=p.a23; q.a33=p.a33; q.b3=p.b3; q.s3=p.s3;
  }

  @Override
  public String toString() {
    String firstLine = "a11:"+a11+" a21:"+a21+" a31:"+a31+" b1:"+b1+" s1:"+s1;
    String secondLine = "a12:"+a11+" a22:"+a22+" a32:"+a32+" b2:"+b2+" s2:"+s2;
    String thirdLine = "a13:"+a13+" a23:"+a23+" a33:"+a33+" b3:"+b1+" s1:"+s1;
    return firstLine +"\n" + secondLine +"\n" + thirdLine;
  }
}
