#ifndef _gsGSAffine3DTransform_txx
#define _gsGSAffine3DTransform_txx

#include "gsGSAffine3DTransform.h"


namespace itk
{

// Constructor with default arguments
template <class TScalarType>
GSAffine3DTransform<TScalarType>
::GSAffine3DTransform() :
  Superclass(SpaceDimension, ParametersDimension) 
{
  m_Rotation = VnlQuaternionType(0,0,0,1); // axis * vcl_sin(t/2), vcl_cos(t/2)
  m_S1 = NumericTraits< TScalarType >::One;
  m_S2 = NumericTraits< TScalarType >::One;
  m_S3 = NumericTraits< TScalarType >::One;
  m_K1 = NumericTraits< TScalarType >::Zero;
  m_K2 = NumericTraits< TScalarType >::Zero;
  m_K3 = NumericTraits< TScalarType >::Zero;
}

// Constructor with default arguments
template<class TScalarType>
GSAffine3DTransform<TScalarType>::
GSAffine3DTransform( unsigned int outputSpaceDimension, 
                          unsigned int parametersDimension ) :
  Superclass(outputSpaceDimension, parametersDimension)
{
  m_Rotation = VnlQuaternionType(0,0,0,1); // axis * vcl_sin(t/2), vcl_cos(t/2)
  m_S1 = NumericTraits< TScalarType >::One;
  m_S2 = NumericTraits< TScalarType >::One;
  m_S3 = NumericTraits< TScalarType >::One;
  m_K1 = NumericTraits< TScalarType >::Zero;
  m_K2 = NumericTraits< TScalarType >::Zero;
  m_K3 = NumericTraits< TScalarType >::Zero;
}



// // Constructor with explicit arguments
// template<class TScalarType>
// GSAffine3DTransform<TScalarType>::
// GSAffine3DTransform( const MatrixType & matrix,
//                           const OutputVectorType & offset ) :
//   Superclass(matrix, offset)
// {
//   this->ComputeMatrixParameters();
// }


// Print self
template<class TScalarType>
void
GSAffine3DTransform<TScalarType>::
PrintSelf(std::ostream &os, Indent indent ) const
{
  Superclass::PrintSelf(os,indent);
  os << indent << "Rotation:    " << m_Rotation    << std::endl;
  os << indent << "S1, S2, S3: " << m_S1 <<", "<<m_S2<<", "<<m_S3 << std::endl;
  os << indent << "K1, K2, K3: " << m_K1 <<", "<<m_K2<<", "<<m_K3 << std::endl;
}


// Set rotation
template<class TScalarType>
void
GSAffine3DTransform<TScalarType>::
SetRotation(const VnlQuaternionType &rotation )
{
  m_Rotation        = rotation;

  this->ComputeMatrix();
  this->ComputeOffset();
  this->Modified();

  return;
}


template<class TScalarType>
void
GSAffine3DTransform<TScalarType>::
SetS1(const TScalarType  S1)
{
  m_S1 = S1;

  this->ComputeMatrix();
  this->ComputeOffset();
  this->Modified();

  return;
}

template<class TScalarType>
void
GSAffine3DTransform<TScalarType>::
SetS2(const TScalarType  S2)
{
  m_S2 = S2;

  this->ComputeMatrix();
  this->ComputeOffset();
  this->Modified();

  return;
}

template<class TScalarType>
void
GSAffine3DTransform<TScalarType>::
SetS3(const TScalarType  S3)
{
  m_S3 = S3;

  this->ComputeMatrix();
  this->ComputeOffset();
  this->Modified();

  return;
}

template<class TScalarType>
void
GSAffine3DTransform<TScalarType>::
SetK1(const TScalarType  K1)
{
  m_K1 = K1;

  this->ComputeMatrix();
  this->ComputeOffset();
  this->Modified();

  return;
}

template<class TScalarType>
void
GSAffine3DTransform<TScalarType>::
SetK2(const TScalarType  K2)
{
  m_K2 = K2;

  this->ComputeMatrix();
  this->ComputeOffset();
  this->Modified();

  return;
}


template<class TScalarType>
void
GSAffine3DTransform<TScalarType>::
SetK3(const TScalarType  K3)
{
  m_K3 = K3;

  this->ComputeMatrix();
  this->ComputeOffset();
  this->Modified();

  return;
}


// Set the parameters in order to fit an Identity transform
template<class TScalarType>
void
GSAffine3DTransform<TScalarType>::
SetIdentity( void ) 
{ 
  m_Rotation = VnlQuaternionType(0,0,0,1);
  m_S1 = NumericTraits< TScalarType >::One;
  m_S2 = NumericTraits< TScalarType >::One;
  m_S3 = NumericTraits< TScalarType >::One;
  m_K1 = NumericTraits< TScalarType >::Zero;
  m_K2 = NumericTraits< TScalarType >::Zero;
  m_K3 = NumericTraits< TScalarType >::Zero;
  this->Superclass::SetIdentity();
}


// Set Parameters
template <class TScalarType>
void
GSAffine3DTransform<TScalarType>
::SetParameters( const ParametersType & parameters )
{
  OutputVectorType   translation; 

  // Transfer the quaternion part
  unsigned int par = 0;

  for(unsigned int j=0; j < 4; j++) 
    {
    m_Rotation[j] = parameters[par];
    ++par;
    }

  m_S1 = parameters[par++];
  m_S2 = parameters[par++];
  m_S3 = parameters[par++];
  m_K1 = parameters[par++];
  m_K2 = parameters[par++];
  m_K3 = parameters[par++];
  
  this->ComputeMatrix();

  // Transfer the constant part
  for(unsigned int i=0; i < SpaceDimension; i++) 
    {
    translation[i] = parameters[par];
    ++par;
    }
  this->SetVarTranslation( translation );
  


  this->ComputeOffset();

  // Modified is always called since we just have a pointer to the
  // parameters and cannot know if the parameters have changed.
  this->Modified();

}



// Set Parameters
template <class TScalarType>
const 
typename GSAffine3DTransform<TScalarType>::ParametersType & 
GSAffine3DTransform<TScalarType>
::GetParameters() const
{
  VnlQuaternionType  quaternion  = this->GetRotation();
  OutputVectorType   translation = this->GetTranslation(); 

  // Transfer the quaternion part
  unsigned int par = 0;

  for(unsigned int j=0; j < 4; j++) 
    {
    this->m_Parameters[par] = quaternion[j];
    ++par;
    }


  this->m_Parameters[par++] = m_S1;
  this->m_Parameters[par++] = m_S2;
  this->m_Parameters[par++] = m_S3;
  this->m_Parameters[par++] = m_K1;
  this->m_Parameters[par++] = m_K2;
  this->m_Parameters[par++] = m_K3;

  // Transfer the constant part
  for(unsigned int i=0; i < SpaceDimension; i++) 
    {
    this->m_Parameters[par] = translation[i];
    ++par;
    }

  return this->m_Parameters;
}


// Get parameters
template<class TScalarType>
const typename GSAffine3DTransform<TScalarType>::JacobianType &
GSAffine3DTransform<TScalarType>::
GetJacobian( const InputPointType & p ) const
{


  this->m_Jacobian.Fill(0.0);

	
  TScalarType c1 = this->GetCenter()[0];
  TScalarType c2 = this->GetCenter()[1];
  TScalarType c3 = this->GetCenter()[2];
  TScalarType s1 = this->m_S1;
  TScalarType s2 = this->m_S2;
  TScalarType s3 = this->m_S3;
  TScalarType k1 = this->m_K1;
  TScalarType k2 = this->m_K2;
  TScalarType k3 = this->m_K3;
  TScalarType x1 = p[0];
  TScalarType x2 = p[1];
  TScalarType x3 = p[2];   

  // z1,z2,z3 is the S*K*point p
  TScalarType w1 = (x1-c1)+k1*(x2-c2)+k2*(x3-c3);
  TScalarType w2 = (x2-c2)+k3*(x3-c2);	
  TScalarType w3 = (x3-c3);

  TScalarType z1 = s1*w1; 
  TScalarType z2 = s2*w2; 
  TScalarType z3 = s3*w3; 

  // compute Jacobian with respect to quaternion parameters
  this->m_Jacobian[0][0] =   2.0 * (  m_Rotation.x() * z1 + m_Rotation.y() * z2 
                              + m_Rotation.z() * z3 );
  this->m_Jacobian[0][1] =   2.0 * (- m_Rotation.y() * z1 + m_Rotation.x() * z2 
                              + m_Rotation.r() * z3 );
  this->m_Jacobian[0][2] =   2.0 * (- m_Rotation.z() * z1 - m_Rotation.r() * z2 
                              + m_Rotation.x() * z3 );
  this->m_Jacobian[0][3] = - 2.0 * (- m_Rotation.r() * z1 + m_Rotation.z() * z2 
                              - m_Rotation.y() * z3 );

  this->m_Jacobian[1][0] = - this->m_Jacobian[0][1];
  this->m_Jacobian[1][1] =   this->m_Jacobian[0][0];
  this->m_Jacobian[1][2] =   this->m_Jacobian[0][3];
  this->m_Jacobian[1][3] = - this->m_Jacobian[0][2];

  this->m_Jacobian[2][0] = - this->m_Jacobian[0][2];
  this->m_Jacobian[2][1] = - this->m_Jacobian[0][3];
  this->m_Jacobian[2][2] =   this->m_Jacobian[0][0];
  this->m_Jacobian[2][3] =   this->m_Jacobian[0][1];

  // get rotation matrix first
  // this is done to compensate for the transposed representation
  // between VNL and ITK
  VnlQuaternionType conjugateRotation = m_Rotation.conjugate();
  MatrixType newMatrix;
  newMatrix = conjugateRotation.rotation_matrix_transpose();
  
  TScalarType r11 = newMatrix[0][0];
  TScalarType r12 = newMatrix[0][1];
  TScalarType r13 = newMatrix[0][2];
  TScalarType r21 = newMatrix[1][0];
  TScalarType r22 = newMatrix[1][1];
  TScalarType r23 = newMatrix[1][2];
  TScalarType r31 = newMatrix[2][0];
  TScalarType r32 = newMatrix[2][1];  
  TScalarType r33 = newMatrix[2][2];  

  // compute Jacobian wrt S1/S2/S3
  this->m_Jacobian[0][4] = r11 * w1;
  this->m_Jacobian[0][5] = r12 * w2;
  this->m_Jacobian[0][6] = r13 * w3;
  this->m_Jacobian[1][4] = r21 * w1;
  this->m_Jacobian[1][5] = r22 * w2;
  this->m_Jacobian[1][6] = r23 * w3;
  this->m_Jacobian[2][4] = r31 * w1;
  this->m_Jacobian[2][5] = r32 * w2;
  this->m_Jacobian[2][6] = r33 * w3;

  // compute Jacobian wrt K1/K2/K3
  this->m_Jacobian[0][7] = r11 * s1 * (x2-c2); 
  this->m_Jacobian[0][8] = r11 * s1 * (x3-c3); 
  this->m_Jacobian[0][9] = r12 * s2 * (x3-c3); 
  this->m_Jacobian[1][7] = r21 * s1 * (x2-c2); 
  this->m_Jacobian[1][8] = r21 * s1 * (x3-c3); 
  this->m_Jacobian[1][9] = r22 * s2 * (x3-c3); 
  this->m_Jacobian[2][7] = r31 * s1 * (x2-c2); 
  this->m_Jacobian[2][8] = r31 * s1 * (x3-c3); 
  this->m_Jacobian[2][9] = r32 * s2 * (x3-c3); 


  // for test purpose
  // FORCE ONLY DO DERIVATIVE ON S
//   for(int ii=0; ii <3; ii++){
//     for(int jj=0; jj<=9;jj++){
//       this->m_Jacobian[ii][jj] = 0.0;
//     }
//   }
//   this->m_Jacobian[0][4] = r11 * w1;
//   this->m_Jacobian[1][4] = r21 * w1;
//   this->m_Jacobian[2][4] = r31 * w1;



  // compute derivatives for the translation part
  unsigned int blockOffset = 10;  
  for(unsigned int dim=0; dim < SpaceDimension; dim++ ) 
    {
    this->m_Jacobian[ dim ][ blockOffset + dim ] = 1.0;
    }

  return this->m_Jacobian;



//   // compute derivatives with respect to rotation
//   this->m_Jacobian.Fill(0.0);

//   const TScalarType x = p[0] - this->GetCenter()[0];
//   const TScalarType y = p[1] - this->GetCenter()[1];
//   const TScalarType z = p[2] - this->GetCenter()[2];

//   // compute Jacobian with respect to quaternion parameters
//   this->m_Jacobian[0][0] =   2.0 * (  m_Rotation.x() * x + m_Rotation.y() * y 
//                               + m_Rotation.z() * z );
//   this->m_Jacobian[0][1] =   2.0 * (- m_Rotation.y() * x + m_Rotation.x() * y 
//                               + m_Rotation.r() * z );
//   this->m_Jacobian[0][2] =   2.0 * (- m_Rotation.z() * x - m_Rotation.r() * y 
//                               + m_Rotation.x() * z );
//   this->m_Jacobian[0][3] = - 2.0 * (- m_Rotation.r() * x + m_Rotation.z() * y 
//                               - m_Rotation.y() * z );

//   this->m_Jacobian[1][0] = - this->m_Jacobian[0][1];
//   this->m_Jacobian[1][1] =   this->m_Jacobian[0][0];
//   this->m_Jacobian[1][2] =   this->m_Jacobian[0][3];
//   this->m_Jacobian[1][3] = - this->m_Jacobian[0][2];

//   this->m_Jacobian[2][0] = - this->m_Jacobian[0][2];
//   this->m_Jacobian[2][1] = - this->m_Jacobian[0][3];
//   this->m_Jacobian[2][2] =   this->m_Jacobian[0][0];
//   this->m_Jacobian[2][3] =   this->m_Jacobian[0][1];


//   // compute derivatives for the translation part
//   unsigned int blockOffset = 4;  
//   for(unsigned int dim=0; dim < SpaceDimension; dim++ ) 
//     {
//     this->m_Jacobian[ dim ][ blockOffset + dim ] = 1.0;
//     }

//   return this->m_Jacobian;

}
 
// template<class TScalarType>
// const typename GSAffine3DTransform< TScalarType >::InverseMatrixType &
// GSAffine3DTransform<TScalarType>::
// GetInverseMatrix() const
// {
//   // If the transform has been modified we recompute the inverse
//   if(this->InverseMatrixIsOld())
//     {
//     InverseMatrixType newMatrix;
//     VnlQuaternionType conjugateRotation = m_Rotation.conjugate();
//     VnlQuaternionType inverseRotation = conjugateRotation.inverse();
//     newMatrix = inverseRotation.rotation_matrix_transpose();
//     this->SetVarInverseMatrix(newMatrix);
//     }
//   return this->GetVarInverseMatrix();
// }

template<class TScalarType>
typename GSAffine3DTransform<TScalarType>::MatrixType GSAffine3DTransform<TScalarType>::
ComputeMyRotationMatrix()
{
  VnlQuaternionType conjugateRotation = m_Rotation.conjugate();
  // this is done to compensate for the transposed representation
  // between VNL and ITK
  MatrixType R;
  R = conjugateRotation.rotation_matrix_transpose();
  return R;
}

template<class TScalarType>
void
GSAffine3DTransform<TScalarType>::
ComputeMatrix()
{
  VnlQuaternionType conjugateRotation = m_Rotation.conjugate();
  // this is done to compensate for the transposed representation
  // between VNL and ITK
  MatrixType R;
  R = conjugateRotation.rotation_matrix_transpose();

  MatrixType S;
  S.Fill(NumericTraits< TScalarType >::Zero);
  S[0][0] = this->m_S1;
  S[1][1] = this->m_S2;
  S[2][2] = this->m_S3;

  MatrixType K;
  K.Fill(NumericTraits< TScalarType >::Zero);
  K[0][0] = NumericTraits< TScalarType >::One;
  K[0][1] = this->m_K1;
  K[0][2] = this->m_K2;
  K[1][1] = NumericTraits< TScalarType >::One;
  K[1][2] = this->m_K3;
  K[2][2] = NumericTraits< TScalarType >::One;

  MatrixType newMatrix;

  newMatrix = R * S * K;



  this->SetVarMatrix(newMatrix);
}

// template<class TScalarType>
// void
// GSAffine3DTransform<TScalarType>::
// ComputeMatrixParameters()
// {
//   VnlQuaternionType quat(this->GetMatrix().GetVnlMatrix());
//   m_Rotation = quat;
// }
 
} // namespace

#endif
