// File gp_Ax2d.cxx, JCV 06/90 
// File gp_Trsf.cxx,  REG 03/06/90 nouvelle version
// JCV 08/01/91 Modif introduction des classes Mat2d et XY dans gp

#define No_Standard_OutOfRange

#include <gp_Trsf2d.ixx>
#include <gp.hxx>
#include <Standard_ConstructionError.hxx>

void gp_Trsf2d::SetMirror (const gp_Ax2d& A)
{
  shape = gp_Ax1Mirror;
  scale = - 1.0;
  const gp_Dir2d& V = A.Direction ();
  const gp_Pnt2d& P = A.Location ();
  Standard_Real VX = V.X();
  Standard_Real VY = V.Y();
  Standard_Real X0 = P.X();
  Standard_Real Y0 = P.Y();
  matrix.SetCol (1, gp_XY (1.0 - 2.0 * VX * VX, -2.0 * VX * VY));
  matrix.SetCol (2, gp_XY (-2.0 * VX * VY, 1.0 - 2.0 * VY * VY));

  loc.SetCoord  (-2.0 * ((VX * VX - 1.0) * X0 + (VX * VY * Y0)),
                 -2.0 * ((VX * VY * X0) + (VY * VY - 1.0) * Y0));
}

void gp_Trsf2d::SetTransformation (const gp_Ax2d& FromA1,
				   const gp_Ax2d& ToA2)
{
  shape = gp_CompoundTrsf;
  scale = 1.0;
  //matrix from XOY to A2 :
  const gp_XY& V1 = ToA2.Direction().XY();
  gp_XY V2 (-V1.Y(), V1.X());
  matrix.SetCol (1, V1);
  matrix.SetCol (2, V2);
  loc = ToA2.Location().XY();
  matrix.Transpose();
  loc.Multiply (matrix);
  loc.Reverse();
  //matrix FromA1 to XOY
  const gp_XY& V3 = FromA1.Direction().XY();
  gp_XY V4 (-V3.Y(), V3.X());
  gp_Mat2d MA1 (V3, V4);
  gp_XY MA1loc = FromA1.Location().XY();
  //matrix * MA1 => FromA1 ToA2
  MA1loc.Multiply (matrix);
  loc.Add (MA1loc);
  matrix.Multiply (MA1);
}

void gp_Trsf2d::SetTransformation (const gp_Ax2d& A)
{
  shape = gp_CompoundTrsf;
  scale = 1.0;
  const gp_XY& V1 = A.Direction().XY();
  gp_XY V2 (-V1.Y(), V1.X());
  matrix.SetCol (1, V1);
  matrix.SetCol (2, V2);
  loc = A.Location().XY();
  matrix.Transpose();
  loc.Multiply (matrix);
  loc.Reverse();
}

void gp_Trsf2d::SetTranslationPart (const gp_Vec2d& V)
{   
  loc = V.XY();
  Standard_Real X = loc.X();
  if (X < 0) X = - X;
  Standard_Real Y = loc.Y();
  if (Y < 0) Y = - Y;
  if (X <= gp::Resolution() && Y <= gp::Resolution()) {
    if (shape == gp_Identity  || shape == gp_PntMirror ||
        shape == gp_Scale     || shape == gp_Rotation  ||
        shape == gp_Ax1Mirror ) { }
    else if (shape == gp_Translation) { shape = gp_Identity; }
    else { shape = gp_CompoundTrsf; }
  }
  else {
    if (shape == gp_Translation || shape == gp_Scale ||
        shape == gp_PntMirror) { }
    else if (shape == gp_Identity) { shape = gp_Translation; }
    else { shape = gp_CompoundTrsf; }
  }
}

void gp_Trsf2d::SetScaleFactor (const Standard_Real S)
{   
  if (S == 1.0) {
    Standard_Real X = loc.X();
    if (X < 0) X = - X;
    Standard_Real Y = loc.Y();
    if (Y < 0) Y = - Y;
    if (X <= gp::Resolution() && Y <= gp::Resolution()) {
      if (shape == gp_Identity || shape == gp_Rotation) { }
      else if (shape == gp_Scale)  { shape = gp_Identity; }
      else if (shape == gp_PntMirror) { shape = gp_Translation; }
      else { shape = gp_CompoundTrsf; }
    }
    else {
      if (shape == gp_Identity || shape == gp_Rotation ||
	  shape == gp_Scale)  { }
      else if (shape == gp_PntMirror) { shape = gp_Translation; }
      else { shape = gp_CompoundTrsf; }
    }
  }
  else if (S == -1) {
    if (shape == gp_PntMirror || shape == gp_Ax1Mirror) { }
    else if (shape == gp_Identity || shape == gp_Scale) { 
      shape = gp_PntMirror;
    }
    else { shape = gp_CompoundTrsf; }
  }
  else {
    if (shape == gp_Scale) { }
    else if (shape == gp_Identity || shape == gp_Translation ||
	     shape == gp_PntMirror) { shape = gp_Scale; }
    else { shape = gp_CompoundTrsf; }
  }
  scale = S;
}

gp_Mat2d gp_Trsf2d::VectorialPart () const
{ 
  if (scale == 1.0)  return matrix;
  gp_Mat2d M = matrix;
  if (shape == gp_Scale || shape == gp_PntMirror)
    M.SetDiagonal (matrix.Value(1,1) * scale, matrix.Value(2,2) * scale);
  else
    M.Multiply (scale);
  return M;
}

Standard_Real gp_Trsf2d::RotationPart () const
{ 
  return ATan2 ( matrix.Value(2,1), matrix.Value(1,1) );
}

void gp_Trsf2d::Invert()
{ 
  //                                    -1
  //  X' = scale * R * X + T  =>  X = (R  / scale)  * ( X' - T)
  //
  // Pour les gp_Trsf2d puisque le scale est extrait de la matrice R
  // on a toujours determinant (R) = 1 et R-1 = R transposee.
  if (shape == gp_Identity) { }
  else if ( shape == gp_Translation || shape == gp_PntMirror) { 
    loc.Reverse(); 
  }
  else if ( shape == gp_Scale) {
    Standard_Real As = scale;
    if (As < 0) As = - As;
    Standard_ConstructionError_Raise_if(As <= gp::Resolution(),""); 
    scale = 1.0 / scale;
    loc.Multiply (-scale);
  }
  else {
    Standard_Real As = scale;
    if (As < 0) As = - As;
    Standard_ConstructionError_Raise_if(As <= gp::Resolution(),""); 
    scale = 1.0 / scale;
    matrix.Transpose();
    loc.Multiply (matrix);
    loc.Multiply (-scale);
  }
}

void gp_Trsf2d::Multiply(const gp_Trsf2d& T)
{
  if (T.shape == gp_Identity) { }
  else if (shape == gp_Identity) {
    shape = T.shape;
    scale = T.scale;
    loc = T.loc;
    matrix = T.matrix;
  } 
  else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
    if (loc.X() != 0.0 || loc.Y() != 0.0) {
      loc.Add (T.loc.Multiplied (matrix));
    }
    matrix.Multiply(T.matrix);
  }
  else if (shape == gp_Translation && T.shape == gp_Translation) {
    loc.Add (T.loc);
  }
  else if (shape == gp_Scale && T.shape == gp_Scale) {
    loc.Add (T.loc.Multiplied(scale));
    scale = scale * T.scale;
  }
  else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
    scale = 1.0;
    shape = gp_Translation;
    loc.Add (T.loc.Reversed());
  }
  else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
    shape = gp_Rotation;
    loc.Add (T.loc.Multiplied (matrix));
    matrix.Multiply(T.matrix);
  }
  else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
	    shape == gp_Ax1Mirror) && T.shape == gp_Translation) {
    gp_XY Tloc (T.loc);
    Tloc.Multiply (matrix);
    if (scale != 1.0) Tloc.Multiply (scale);
    loc.Add (Tloc);
  }
  else if ((shape == gp_Scale || shape == gp_PntMirror)
	   && T.shape == gp_Translation) {
    gp_XY Tloc (T.loc);
    Tloc.Multiply (scale);
    loc.Add (Tloc);
  }
  else if (shape == gp_Translation &&
	   (T.shape == gp_CompoundTrsf ||
	    T.shape == gp_Rotation || T.shape == gp_Ax1Mirror)) {
    shape = gp_CompoundTrsf;
    scale = T.scale;
    loc.Add (T.loc);
    matrix = T.matrix;
  }
  else if (shape == gp_Translation && 
	   (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
    shape = T.shape;
    loc.Add (T.loc);
    scale = T.scale;
  }
  else if ((shape == gp_PntMirror || shape == gp_Scale) &&
	   (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
    shape = gp_CompoundTrsf;
    gp_XY Tloc (T.loc);
    Tloc.Multiply (scale);
    loc.Add (Tloc);
    scale = scale * T.scale;
  }
  else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
	    shape == gp_Ax1Mirror)
	   && (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
    shape = gp_CompoundTrsf;
    gp_XY Tloc (T.loc);
    Tloc.Multiply(matrix);
    if (scale == 1.0)  scale = T.scale;
    else {
      Tloc.Multiply (scale);
      scale = scale * T.scale;
    }
    loc.Add (Tloc);
  }
  else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
	    T.shape == gp_Ax1Mirror)
	   && (shape == gp_Scale || shape == gp_PntMirror)) {
    shape = gp_CompoundTrsf;
    gp_XY Tloc (T.loc);
    Tloc.Multiply (scale);
    scale = scale * T.scale;
    loc.Add (Tloc);
    matrix = T.matrix;
  }
  else {
    shape = gp_CompoundTrsf;
    gp_XY Tloc (T.loc);
    Tloc.Multiply (matrix);
    if (scale != 1.0) {
      Tloc.Multiply (scale);
      scale = scale * T.scale;
    }
    else { scale = T.scale; }
    loc.Add (Tloc);
    matrix.Multiply (T.matrix);
  }
}

void gp_Trsf2d::Power (const Standard_Integer N)
{
  if (shape == gp_Identity) { }
  else {
    if (N == 0)  {
      scale = 1.0;
      shape = gp_Identity;
      matrix.SetIdentity();
      loc = gp_XY (0.0, 0.0);
    }
    else if (N == 1)  { }
    else if (N == -1) Invert();
    else {
      if (N < 0) Invert();
      if (shape == gp_Translation) { 
	Standard_Integer Npower = N;
	if (Npower < 0) Npower = - Npower;
	Npower--;
	gp_XY Temploc = loc;
	while (1) {
	  if (IsOdd(Npower))  loc.Add (Temploc);
	  if (Npower == 1) break;
	  Temploc.Add (Temploc);
	  Npower = Npower/2;
	}
      }
      else if (shape == gp_Scale) {
	Standard_Integer Npower = N;
	if (Npower < 0) Npower = - Npower;
	Npower--;
	gp_XY Temploc = loc;
	Standard_Real Tempscale = scale;
	while (1) {
	  if (IsOdd(Npower)) {
	    loc.Add (Temploc.Multiplied(scale));
	    scale = scale * Tempscale;
	  }
	  if (Npower == 1) break;
	  Temploc.Add (Temploc.Multiplied(Tempscale));
	  Tempscale = Tempscale * Tempscale;
	  Npower = Npower/2;
	}
      }
      else if (shape == gp_Rotation) {
	Standard_Integer Npower = N;
	if (Npower < 0) Npower = - Npower;
	Npower--;
	gp_Mat2d Tempmatrix (matrix);
	if (loc.X() == 0.0 && loc.Y() == 0.0) {
	  while (1) {
	    if (IsOdd(Npower))  matrix.Multiply (Tempmatrix);
	    if (Npower == 1) break;
	    Tempmatrix.Multiply (Tempmatrix);
	    Npower = Npower/2;
	  }
	}
	else {
	  gp_XY Temploc = loc;
	  while (1) {
	    if (IsOdd(Npower)) {
	      loc.Add (Temploc.Multiplied (matrix));
	      matrix.Multiply (Tempmatrix);
	    }
	    if (Npower == 1) break;
	    Temploc.Add (Temploc.Multiplied (Tempmatrix));
	    Tempmatrix.Multiply (Tempmatrix);
	    Npower = Npower/2;
	  }
	}
      }
      else if (shape == gp_PntMirror || shape == gp_Ax1Mirror) {
	if (IsEven (N)) {
	  shape = gp_Identity;
	  scale = 1.0;
	  matrix.SetIdentity ();
	  loc.SetCoord (0.0, 0.0);
	}
      }
      else {
	shape = gp_CompoundTrsf;
	Standard_Integer Npower = N;
	if (Npower < 0) Npower = - Npower;
	Npower--;
	matrix.SetDiagonal (scale*matrix.Value(1,1),
			    scale*matrix.Value(2,2));
	gp_XY Temploc = loc;
	Standard_Real Tempscale = scale;
	gp_Mat2d Tempmatrix (matrix);
	while (1) {
	  if (IsOdd(Npower)) {
	    loc.Add ((Temploc.Multiplied (matrix)).Multiplied (scale));
	    scale = scale * Tempscale;
	    matrix.Multiply (Tempmatrix);
	  }
	  if (Npower == 1) break;
	  Tempscale = Tempscale * Tempscale;
	  Temploc.Add ( (Temploc.Multiplied (Tempmatrix)).Multiplied 
		       (Tempscale)
		       );
	  Tempmatrix.Multiply (Tempmatrix);
	  Npower = Npower/2;
	}
      }
    }
  }
}

void gp_Trsf2d::PreMultiply (const gp_Trsf2d& T)
{
  if (T.shape == gp_Identity) { }
  else if (shape == gp_Identity) {
    shape = T.shape;
    scale = T.scale;
    loc = T.loc;
    matrix = T.matrix;
  } 
  else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
    loc.Multiply (T.matrix);
    loc.Add (T.loc);
    matrix.PreMultiply(T.matrix);
  }
  else if (shape == gp_Translation && T.shape == gp_Translation) {
    loc.Add (T.loc);
  }
  else if (shape == gp_Scale && T.shape == gp_Scale) {
    loc.Multiply (T.scale);
    loc.Add (T.loc);
    scale = scale * T.scale;
  }
  else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
    scale = 1.0;
    shape = gp_Translation;
    loc.Reverse();
    loc.Add (T.loc);
  }
  else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
    shape = gp_Rotation;
    loc.Multiply (T.matrix);
    loc.Add (T.loc);
    matrix.PreMultiply(T.matrix);
  }
  else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
	    shape == gp_Ax1Mirror) && T.shape == gp_Translation) {
    loc.Add (T.loc);
  }
  else if ((shape == gp_Scale || shape == gp_PntMirror)
	   && T.shape == gp_Translation) {
    loc.Add (T.loc);
  }
  else if (shape == gp_Translation &&
	   (T.shape == gp_CompoundTrsf ||
	    T.shape == gp_Rotation || T.shape == gp_Ax1Mirror)) {
    shape = gp_CompoundTrsf;
    matrix = T.matrix;
    if (T.scale == 1.0)  loc.Multiply (T.matrix);
    else {
      scale = T.scale;
      loc.Multiply (matrix);
      loc.Multiply (scale);
    }
    loc.Add (T.loc);
  }
  else if ((T.shape == gp_Scale || T.shape == gp_PntMirror)
	   && shape == gp_Translation) {
    loc.Multiply (T.scale);
    loc.Add (T.loc);
    scale = T.scale;
    shape = T.shape;
  }
  else if ((shape == gp_PntMirror || shape == gp_Scale) &&
	   (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
    shape = gp_CompoundTrsf;
    loc.Multiply (T.scale);
    loc.Add (T.loc);
    scale = scale * T.scale;
  }
  else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
	    shape == gp_Ax1Mirror)
	   && (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
    shape = gp_CompoundTrsf;
    loc.Multiply (T.scale);
    loc.Add (T.loc);
    scale = scale * T.scale;
  } 
  else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
	    T.shape == gp_Ax1Mirror) 
	   && (shape == gp_Scale || shape == gp_PntMirror)) {
    shape = gp_CompoundTrsf;
    matrix = T.matrix;
    if (T.scale == 1.0)  loc.Multiply (T.matrix);
    else {
      loc.Multiply (matrix);
      loc.Multiply (T.scale);
      scale = T.scale * scale;
    }
    loc.Add (T.loc);
  } 
  else {
    shape = gp_CompoundTrsf;
    loc.Multiply (T.matrix);
    if (T.scale != 1.0) {
      loc.Multiply(T.scale);   scale = scale * T.scale;
    }
    loc.Add (T.loc);
    matrix.PreMultiply(T.matrix);
  }
}