#ifndef QuAffMotModel_HH
#define QuAffMotModel_HH
////////////////////////////////////////////////////////////////////////////
//! author="Ratna Rambaruth"
//! lib=GDMotion
//! date="3/1/100"
//! docentry="Image.Motion.Estimation.Model-based.Motion Model"
//! rcsid="$Id: QuAffMotModel.hh,v 1.13 2001/01/02 18:21:13 ees1wc Exp $"
//! file="amma/Image/Motion/GradDescent/QuAffMotModel.hh"

#include "amma/RCHandleA.hh"
#include "amma/Motion/MotModel.hh"

#include "amma/Point2d.hh"
#include "amma/Vector.hh"
#include "amma/IndexNd.hh"
#include "amma/IntSArr1.hh"
#include "amma/Matrix2d.hh"
#include "amma/SArr1Iter3.hh"
#include "amma/Affine2d.hh"
#include "amma/ImageRec.hh"

class istream;
class ostream;

// --------------------------------------------------------------------------
// **********  TransMotModelC  **************************************************
// --------------------------------------------------------------------------

///////////////////////////////////////////
//! userlevel=Develop
//: Class for methods specific to the quasi-affine motion model

class QuAffMotModelBodyC 
  : public MotModelBodyC
{    
 public:
  //: Constructors, copies, assigment, and destructor
  //: -----------------------------------------------
  
  QuAffMotModelBodyC(){}
  //: Default constructor

  QuAffMotModelBodyC(istream &in){
    assert(0); // Not implemented.
  }
  //: Stream constructor
  // This creates a new instance of the class from an input stream.
  
  //: Operators
  //: ---------
  
  virtual MotModelBodyC &Copy() const{
    return *new QuAffMotModelBodyC(*this); 
  }
  //: Create a copy of this object.
  
  virtual BooleanT Save(ostream &out) const{
    out << (*this);
    return TRUE; 
  }
  //: Save to ostream.
  // This allows a streaming from base class without knowing
  // what the full class type is.
  
  //: Methods
  //: -------
  
  
  inline VectorC InitialiseMinP(const ImageRectangleC & search_window) const;
  // Get the lower bound of the search space.

  inline VectorC InitialiseMaxP(const ImageRectangleC & search_window) const;
  // Get the upper bound of the search space.

  inline VectorC InitialiseConstP() const;
  // Returns the const parameter values and starting point for enabled ones.

  inline IntSArray1dC InitialisePrecisionSteps() const;
  // Returns the number of steps for each parameter.

  inline Point2dC TransformLoc (const Point2dC & loc, const Point2dC & origin, const VectorC & P) const;
  // Get new location of loc

  inline Point2dC InverseTransformLoc (const Point2dC & loc, const Point2dC & origin, const VectorC & P) const;
  // Get new location of loc

  inline VectorC GetModelGradient (const Point2dC & image_gradient, const Point2dC & orig_dist) const;
  // Get the gradient
 
  inline IndexNdC NextMoveDirection(const VectorC & gradient, const IntSArray1dC & steps_no) const;
  // Decides the next move direction given the image gradient

  inline VectorC GradientVector (const VectorC & X, const VectorC & inlier_mask, const SArray1dC<PixelC> & mask, const SArray1dC<VectorC> & gradients, const VectorC & diff, const Point2dC & origin) const;
  //: Evaluates the gradient vector according to equation 3.17 & 3.26 (Diehl).
  

  inline VectorC ComposeVectors (const VectorC & A, const VectorC & B) const;
  //: A * B = Out

  inline VectorC InverseVector (const VectorC & A) const;
  //: returns the inverse 

  inline Vector2dC GetTranslation (const VectorC & mv) const 
    {return Vector2dC(mv[2], mv[3]);}
  // Gets the translational part of the vector

  inline Affine2dC ConstructAffineVector(const VectorC & dP){return Affine2dC(Matrix2d2C(1+dP[0], -dP[1], dP[1], 1+dP[0]), Vector2dC(dP[2], dP[3]));};

  inline VectorC ConstructNormalVector(const Affine2dC & motion_vector){ 
    VectorC dP(4);
    dP[0] = motion_vector.SRMatrix().A00()-1;
    dP[1] = motion_vector.SRMatrix().A10();
    dP[2] = motion_vector.Translation().X();
    dP[3] = motion_vector.Translation().Y();
    return dP;
  }

  //: Friends
  //: -------
    
  friend ostream &operator<<(ostream &s, const QuAffMotModelBodyC &out){
    assert(0); // Not implemented!
    return s;
  }
  //: output stream operator
  
  friend istream &operator>>(istream &s, QuAffMotModelBodyC &in){
    in = QuAffMotModelBodyC(s);
    return s;
  }
  //: input stream operator
  
 protected:
  //: Put all your class members here
  
};

VectorC QuAffMotModelBodyC::InitialiseMinP(const ImageRectangleC & search_window) const
{
    VectorC minP(4);
    minP[0] = search_window.Origin().Row();
    minP[1] = search_window.Origin().Col();
    minP[2] = search_window.Origin().Row();
    minP[3] = search_window.Origin().Col();

    return minP;
}

VectorC QuAffMotModelBodyC::InitialiseMaxP(const ImageRectangleC & search_window) const
{
    VectorC maxP(4);
    maxP[0] = search_window.End().Row();
    maxP[1] = search_window.End().Col();
    maxP[2] = search_window.End().Row();
    maxP[3] = search_window.End().Col();

    return maxP;
}

VectorC QuAffMotModelBodyC::InitialiseConstP() const
{
  return VectorC(4, 0);
}

IntSArray1dC QuAffMotModelBodyC::InitialisePrecisionSteps() const
{
    IntSArray1dC precision_steps(4); 
    precision_steps[0] = 1000;
    precision_steps[1] = 1000;
    precision_steps[2] = 1;
    precision_steps[3] = 1;

    return precision_steps;
}

Point2dC QuAffMotModelBodyC::TransformLoc (const Point2dC & loc, const Point2dC & origin, const VectorC & P) const
{
  Point2dC newloc = loc + Matrix2d2C(P[0], -P[1], P[1], P[0]) * (loc - origin) + Vector2dC(P[2], P[3]);

  return newloc;
}


Point2dC QuAffMotModelBodyC::InverseTransformLoc (const Point2dC & loc, const Point2dC & origin, const VectorC & P) const
{
  Affine2dC P_aff = Affine2dC(Matrix2d2C(P[0], P[1], -P[1], P[0]), Vector2dC(P[2], P[4]));
  Point2dC tmploc = loc - P_aff.Translation() + P_aff.SRMatrix()*origin;
  Matrix2d2C mat = P_aff.SRMatrix(); mat[0][0]+=1; mat[1][1]+=1;
  
  Point2dC newloc = mat.I()*tmploc;
  
  return newloc;
}

VectorC QuAffMotModelBodyC::GetModelGradient (const Point2dC & image_gradient, const Point2dC & orig_dist) const
{
  VectorC gradient(4);
  gradient[0] = image_gradient.X()*orig_dist.X() + image_gradient.Y()*orig_dist.Y() ;
  gradient[1] = -image_gradient.X()*orig_dist.Y() + image_gradient.Y()*orig_dist.X();
  gradient[2] = image_gradient.X();
  gradient[3] = image_gradient.Y();

  return gradient;
}

IndexNdC QuAffMotModelBodyC::NextMoveDirection(const VectorC & gradient,const IntSArray1dC & steps_no) const
{
  IndexNdC md(gradient.Dim());
  SArray1dIterC<IndexC> md_it(md);
  for (md_it.First(); md_it.IsElm(); md_it.Next()){
    md_it.Data1() = 0;
  }

  VectorC dh(gradient.Dim());
  SArray1dIter3C<RealT, RealT, IntT> step_it(dh, gradient, steps_no);
  for (step_it.First(); step_it.IsElm(); step_it.Next()){
    step_it.Data1() = step_it.Data2()/step_it.Data3();
  }
  
  register int index;
  float tmp,max;
  float dh1,dh2,dh3,dh4;
  
  if (dh[0]>0)
    dh1=dh[0];
  else
    dh1=-dh[0];
  
  if (dh[1]>0)
    dh2=dh[1];
  else
    dh2=-dh[1];
  
  if (dh[2]>0)
    dh3=dh[2];
  else
    dh3=-dh[2];
  
  if (dh[3]>0)
    dh4=dh[3];
  else
    dh4=-dh[3];
  
  max=dh4; index=1;
  if ((tmp = dh3)>max)                                    {max =tmp;index =2;}
  if ((tmp = 0.7071*dh3+0.7071*dh4)>max) 			{max =tmp;index =3;}
  if ((tmp = dh2)>max) 					{max =tmp;index =4;}
  if ((tmp = 0.7071*dh2+0.7071*dh4)>max)			{max =tmp;index =5;}
  if ((tmp = 0.7071*dh2+0.7071*dh3)>max)			{max =tmp;index =6;}
  if ((tmp = 0.5774*dh2+0.5774*dh3+0.5774*dh4)>max)	{max =tmp;index =7;}
  if ((tmp = dh1)>max)					{max =tmp;index =8;}	
  if ((tmp = 0.7071*dh1+0.7071*dh4)>max)			{max =tmp;index =9;}
  if ((tmp = 0.7071*dh1+0.7071*dh3)>max)			{max =tmp;index =10;}
  if ((tmp = 0.5774*dh1+0.5774*dh3+0.5774*dh4)>max)	{max =tmp;index =11;}
  if ((tmp = 0.7071*dh1+0.7071*dh2)>max)			{max =tmp;index =12;}
  if ((tmp = 0.5774*dh1+0.5774*dh2+0.5774*dh4)>max)	{max =tmp;index =13;}
  if ((tmp = 0.5774*dh1+0.5774*dh2+0.5774*dh3)>max)	{max =tmp;index =14;}
  if ((tmp = 0.5000*dh1+0.5000*dh2+0.5000*dh3+0.5000*dh4)>max) {max =tmp;index =15;}
  
  switch (index){
    
    /* dh4 */
  case 1: {if (dh[3]>0) md[3]=-1; else md[3]=1;} break;
    
    /* dh3 */
  case 2: {if (dh[2]>0) md[2]=-1; else md[2]=1;} break;
    
    /* dh3 dh4 */
  case 3: {if (dh[2]>0) md[2]=-1; else md[2]=1;
  if (dh[3]>0) md[3]=-1; else md[3]=1;} break;
  
  /* dh2 */
  case 4: {if (dh[1]>0) md[1]=-1; else md[1]=1;} break;
    
    /* dh2 dh4 */
  case 5: {if (dh[1]>0) md[1]=-1; else md[1]=1;
  if (dh[3]>0) md[3]=-1; else md[3]=1;} break;
  
  /* dh2 dh3 */
  case 6: {if (dh[1]>0) md[1]=-1; else md[1]=1;
  if (dh[2]>0) md[2]=-1; else md[2]=1;} break;
  
  /* dh2 dh3 dh4 */
  case 7: {if (dh[1]>0) md[1]=-1; else md[1]=1;
  if (dh[2]>0) md[2]=-1; else md[2]=1;
  if (dh[3]>0) md[3]=-1; else md[3]=1;} break;

  /* dh1 */
  case 8: {if (dh[0]>0) md[0]=-1; else md[0]=1;} break;
    
    /* dh1 dh4 */
  case 9: {if (dh[0]>0) md[0]=-1; else md[0]=1;
  if (dh[3]>0) md[3]=-1; else md[3]=1;} break;
  
  /* dh1 dh3 */
  case 10: {if (dh[0]>0) md[0]=-1; else md[0]=1;
  if (dh[2]>0) md[2]=-1; else md[2]=1;} break;
  
  /* dh1 dh3 dh4 */
  case 11: {if (dh[0]>0) md[0]=-1; else md[0]=1;
  if (dh[2]>0) md[2]=-1; else md[2]=1;
  if (dh[3]>0) md[3]=-1; else md[3]=1;} break;
  
  /* dh1 dh2 */
  case 12: {if (dh[0]>0) md[0]=-1; else md[0]=1;
  if (dh[1]>0) md[1]=-1; else md[1]=1;} break;
  
  /* dh1 dh2 dh4 */
  case 13: {if (dh[0]>0) md[0]=-1; else md[0]=1;
  if (dh[1]>0) md[1]=-1; else md[1]=1; 
  if (dh[3]>0) md[3]=-1; else md[3]=1;} break; 
  
  /* dh1 dh2 dh3 */
  case 14: {if (dh[0]>0) md[0]=-1; else md[0]=1;
  if (dh[1]>0) md[1]=-1; else md[1]=1; 
  if (dh[2]>0) md[2]=-1; else md[2]=1;} break;
  
  /* dh1 dh2 dh3 dh4 */
  case 15: {if (dh[0]>0) md[0]=-1; else md[0]=1;
  if (dh[1]>0) md[1]=-1; else md[1]=1;
  if (dh[2]>0) md[2]=-1; else md[2]=1; 
  if (dh[3]>0) md[3]=-1; else md[3]=1;} break;
  
  default: {cout << "Fatal error in: HardwiredSelectBest4D\n";
    exit(-1);} break;
    
  }

  return md;
}

VectorC QuAffMotModelBodyC::GradientVector (const VectorC & X, const VectorC & inlier_mask, const SArray1dC<PixelC> & mask, const SArray1dC<VectorC> & gradients, const VectorC & diff, const Point2dC & origin) const{

  VectorC zTk(4, 0);
  int count=0;
  SArray1dIter3C<PixelC, VectorC, RealT> arr_it(mask, gradients, diff);
  for(arr_it.First(); arr_it.IsElm(); arr_it.Next()){
    if(inlier_mask[arr_it.Index()]==1){
      VectorC partial_derivatives = arr_it.Data2()*arr_it.Data3();

      zTk += partial_derivatives;
      count++;
    }
  }
	
  zTk/=(RealT)count;
  return zTk;
}


VectorC QuAffMotModelBodyC::ComposeVectors (const VectorC & A, const VectorC & B) const
{
  Affine2dC A_aff = Affine2dC(Matrix2d2C(A[0]+1, A[1], -A[1], A[0]+1), Vector2dC(A[2], A[3]));
  Affine2dC B_aff = Affine2dC(Matrix2d2C(B[0]+1, B[1], -B[1], B[0]+1), Vector2dC(B[2], B[3]));

  Affine2dC result_aff = A_aff * B_aff;

  VectorC result(4, 0);
  result[0] = result_aff.SRMatrix().A00()-1;
  result[1] = result_aff.SRMatrix().A01();
  result[2] = result_aff.Translation().X();
  result[3] = result_aff.Translation().Y();
  
  return result;
}

VectorC QuAffMotModelBodyC::InverseVector (const VectorC & A) const
{
  Affine2dC A_aff = Affine2dC(Matrix2d2C(A[0]+1, A[1], -A[1], A[0]+1), Vector2dC(A[2], A[3]));

  Affine2dC inv_A_aff = A_aff.I();

  VectorC result(4, 0);
  result[0] = inv_A_aff.SRMatrix().A00()-1;
  result[1] = inv_A_aff.SRMatrix().A01();
  result[2] = inv_A_aff.Translation().X();
  result[3] = inv_A_aff.Translation().Y();
  
  return result;
}


///////////////////////////////////////////////////////////
//! userlevel=Normal
//: Class for methods specific to the quasi-affine motion model

class QuAffMotModelC 
  : public MotModelC
{   
 public:
  //: Constructors, copies, assigment, and destructor
  //: -----------------------------------------------
  
  QuAffMotModelC()
    : MotModelC(* new QuAffMotModelBodyC())
    {}
  //: Default constructor
  // This creates an invalid handle.
  // You can remove this if you always want to create a full object.
  
  QuAffMotModelC(istream &in)
    : MotModelC(* new QuAffMotModelBodyC(in))
    {}
  //: Stream constructor
  // This creates a new instance of the class from an input stream.
  
  
 protected:
  QuAffMotModelC(QuAffMotModelBodyC &bod)
    : MotModelC(bod)
  {}
  //: Body Constructor
  // This creates a new handle to a body class, it can be usefull if
  // you wish to return handles to the class from within a body function.
  
  inline
  QuAffMotModelBodyC &Body() 
    { return static_cast<QuAffMotModelBodyC &>(MotModelC::Body());}
  //: Access body.
  // This isn't really needed, they're just to ensure
  // all derived classes work properly.

  inline
  const QuAffMotModelBodyC &Body() const
    { return static_cast<const QuAffMotModelBodyC &>(MotModelC::Body());}
  //: Constant access body.
  // This isn't really needed, they're just to ensure
  // all derived classes work properly.

 public:    
  //: Operators
  //: ---------
  
  
  //: Methods
  //: -------
   
  
  inline VectorC InitialiseMinP(const ImageRectangleC & search_window) const
    { return Body().InitialiseMinP(search_window); };
  // Get the lower bound of the search space.

  inline VectorC InitialiseMaxP(const ImageRectangleC & search_window) const
    { return Body().InitialiseMaxP(search_window); };
  // Get the upper bound of the search space.

  inline VectorC InitialiseConstP() const
    { return Body().InitialiseConstP(); };
  // Returns the const parameter values and starting point for enabled ones.

  inline IntSArray1dC InitialisePrecisionSteps() const
    { return Body().InitialisePrecisionSteps(); };
  // Returns the number of steps for each parameter.

  inline Point2dC TransformLoc(const Point2dC & loc, const Point2dC & origin, const VectorC & P) const
    { return Body().TransformLoc(loc, origin, P); }
  //: Get new location of loc
  
  inline Point2dC InverseTransformLoc (const Point2dC & loc, const Point2dC & origin, const VectorC & P) const
    { return Body().InverseTransformLoc(loc, origin, P); }
  // Get new location of loc
  
  inline VectorC GetModelGradient(const Point2dC & image_gradient, const Point2dC & orig_dist ) const
    { return Body().GetModelGradient(image_gradient, orig_dist); }
  //: Get the gradient
  
  inline IndexNdC NextMoveDirection(const VectorC & gradient, const IntSArray1dC & steps_no) const 
    {return Body().NextMoveDirection(gradient, steps_no);}
  //: Decides the next move direction given the image gradient


  inline VectorC GradientVector (const VectorC & X, const VectorC & inlier_mask, const SArray1dC<PixelC> & mask, const SArray1dC<VectorC> & gradients, const VectorC & diff, const Point2dC & origin) const
    {return Body().GradientVector(X, inlier_mask, mask, gradients, diff, origin);}
  //: Evaluates the gradient vector according to equation 3.17 & 3.26 (Diehl).

  inline VectorC InverseVector (const VectorC & mv) const {return Body().InverseVector(mv);}
  // Inverts the vector 'mv'

  inline Vector2dC GetTranslation (const VectorC & mv) const {return Body().GetTranslation(mv);}
  // Gets the translational part of the vector

  
  inline VectorC ComposeVectors (const VectorC & A, const VectorC & B) const
    {return Body().ComposeVectors(A, B);};
  //: A * B = Out

  inline Affine2dC ConstructAffineVector(const VectorC & dP)
    {return Body().ConstructAffineVector(dP);}

  inline VectorC ConstructNormalVector(const Affine2dC & motion_vector)
    {return Body().ConstructNormalVector(motion_vector);}
  
  //: Friends
  //: -------
  
  friend ostream &operator<<(ostream &s, const QuAffMotModelC &out);
  //: output stream operator
  
  friend istream &operator>>(istream &s, QuAffMotModelC &in);
  //: input stream operator
  
};

inline
ostream &operator<<(ostream &s, const QuAffMotModelC &out)
{
  out.Save(s);
  return s;
}
//: output stream operator

inline
istream &operator>>(istream &s, QuAffMotModelC &in)
{
  in = QuAffMotModelC(s);
  return s;
}
//: input stream operator

#endif