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

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

#include "amma/Point2d.hh"
#include "amma/Vector.hh"
#include "amma/Vector2d.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"
#include "amma/Matrix.hh"


class istream;
class ostream;

// --------------------------------------------------------------------------
// **********  AffineMotModelC  **************************************************
// --------------------------------------------------------------------------

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

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

  AffineMotModelBodyC(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 AffineMotModelBodyC(*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[5]);}
  // Gets the translational part of the vector

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

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

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

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

    return minP;
}

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

    return maxP;
}

VectorC AffineMotModelBodyC::InitialiseConstP() const
{
    return VectorC(6, 0);
}

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

    return precision_steps;
}

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


Point2dC AffineMotModelBodyC::InverseTransformLoc (const Point2dC & loc, const Point2dC & origin, const VectorC & P) const
{
  Point2dC newloc = loc + Matrix2d2C(P[0], P[1], P[3], P[4]) * (loc - origin) + Vector2dC(P[2], P[5]);
  
  return newloc;
}

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

  return gradient;
}

IndexNdC AffineMotModelBodyC::NextMoveDirection(const VectorC & gradient,const IntSArray1dC & steps_no) const
{
  IndexNdC md(gradient.Dim());
  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,dh5,dh6;
  

  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];
  
  if (dh[4]>0)
    dh5=dh[4];
  else
    dh5=-dh[4];
  
  if (dh[5]>0)
    dh6=dh[5];
  else
    dh6=-dh[5];
  
  max = dh1; index=1;
  if ((tmp = dh4 							)>max) {max =tmp;index =2;}
  if ((tmp = dh3 							)>max) {max =tmp;index =3;}
  if ((tmp = 0.70711*dh3 + 0.70711*dh4 				)>max) {max =tmp;index =4;}
  if ((tmp = dh2 							)>max) {max =tmp;index =5;}
  if ((tmp = 0.70711*dh2 + 0.70711*dh4 				)>max) {max =tmp;index =6;}
  if ((tmp = 0.70711*dh2 + 0.70711*dh3 				)>max) {max =tmp;index =7;}
  if ((tmp = 0.57735*dh2 + 0.57735*dh3 + 0.57735*dh4 			)>max) {max =tmp;index =8;}
  if ((tmp = dh1 							)>max) {max =tmp;index =9;}
  if ((tmp = 0.70711*dh1 + 0.70711*dh4 				)>max) {max =tmp;index =10;}
  if ((tmp = 0.70711*dh1 + 0.70711*dh3 				)>max) {max =tmp;index =11;}
  if ((tmp = 0.57735*dh1 + 0.57735*dh3 + 0.57735*dh4 			)>max) {max =tmp;index =12;}
  if ((tmp = 0.70711*dh1 + 0.70711*dh2 				)>max) {max =tmp;index =13;}
  if ((tmp = 0.57735*dh1 + 0.57735*dh2 + 0.57735*dh4 			)>max) {max =tmp;index =14;}
  if ((tmp = 0.57735*dh1 + 0.57735*dh2 + 0.57735*dh3 			)>max) {max =tmp;index =15;}
  if ((tmp = 0.50000*dh1 + 0.50000*dh2 + 0.50000*dh3 + 0.50000*dh4 	)>max) {max =tmp;index =16;}
  if ((tmp = dh5 							)>max) {max =tmp;index =17;}
  if ((tmp = 0.70711*dh4 + 0.70711*dh5  				)>max) {max =tmp;index =18;}
  if ((tmp = 0.70711*dh3 + 0.70711*dh5  				)>max) {max =tmp;index =19;}
  if ((tmp = 0.57735*dh3 + 0.57735*dh4 + 0.57735*dh5  			)>max) {max =tmp;index =20;}
  if ((tmp = 0.70711*dh2 + 0.70711*dh5  				)>max) {max =tmp;index =21;}
  if ((tmp = 0.57735*dh2 + 0.57735*dh4 + 0.57735*dh5  			)>max) {max =tmp;index =22;}
  if ((tmp = 0.57735*dh2 + 0.57735*dh3 + 0.57735*dh5  			)>max) {max =tmp;index =23;}
  if ((tmp = 0.50000*dh2 + 0.50000*dh3 + 0.50000*dh4 + 0.50000*dh5 	)>max) {max =tmp;index =24;}
  if ((tmp = 0.70711*dh1 + 0.70711*dh5  				)>max) {max =tmp;index =25;}
  if ((tmp = 0.57735*dh1 + 0.57735*dh4 + 0.57735*dh5  			)>max) {max =tmp;index =26;}
  if ((tmp = 0.57735*dh1 + 0.57735*dh3 + 0.57735*dh5  			)>max) {max =tmp;index =27;}
  if ((tmp = 0.50000*dh1 + 0.50000*dh3 + 0.50000*dh4 + 0.50000*dh5  	)>max) {max =tmp;index =28;}
  if ((tmp = 0.57735*dh1 + 0.57735*dh2 + 0.57735*dh5  			)>max) {max =tmp;index =29;}
  if ((tmp = 0.50000*dh1 + 0.50000*dh2 + 0.50000*dh4 + 0.50000*dh5  	)>max) {max =tmp;index =30;}
  if ((tmp = 0.50000*dh1 + 0.50000*dh2 + 0.50000*dh3 + 0.50000*dh5  	)>max) {max =tmp;index =31;}
  if ((tmp = 0.44721*dh1 + 0.44721*dh2 + 0.44721*dh3 + 0.44721*dh4 + 0.44721*dh5)>max) {max =tmp;index =32;}
  if ((tmp = dh6							)>max) {max =tmp;index =33;}
  if ((tmp = 0.70711*dh4 + 0.70711*dh6					)>max) {max =tmp;index =34;}
  if ((tmp = 0.70711*dh3 + 0.70711*dh6					)>max) {max =tmp;index =35;}
  if ((tmp = 0.57735*dh3 + 0.57735*dh4 + 0.57735*dh6			)>max) {max =tmp;index =36;}
  if ((tmp = 0.70711*dh2 + 0.70711*dh6					)>max) {max =tmp;index =37;}
  if ((tmp = 0.57735*dh2 + 0.57735*dh4 + 0.57735*dh6			)>max) {max =tmp;index =38;}
  if ((tmp = 0.57735*dh2 + 0.57735*dh3 + 0.57735*dh6			)>max) {max =tmp;index =39;}
  if ((tmp = 0.50000*dh2 + 0.50000*dh3 + 0.50000*dh4 + 0.50000*dh6	)>max) {max =tmp;index =40;}
  if ((tmp = 0.70711*dh1 + 0.70711*dh6					)>max) {max =tmp;index =41;}
  if ((tmp = 0.57735*dh1 + 0.57735*dh4 + 0.57735*dh6			)>max) {max =tmp;index =42;}
  if ((tmp = 0.57735*dh1 + 0.57735*dh3 + 0.57735*dh6			)>max) {max =tmp;index =43;}
  if ((tmp = 0.50000*dh1 + 0.50000*dh3 + 0.50000*dh4 + 0.50000*dh6	)>max) {max =tmp;index =44;}
  if ((tmp = 0.57735*dh1 + 0.57735*dh2 + 0.57735*dh6			)>max) {max =tmp;index =45;}
  if ((tmp = 0.50000*dh1 + 0.50000*dh2 + 0.50000*dh4 + 0.50000*dh6	)>max) {max =tmp;index =46;}
  if ((tmp = 0.50000*dh1 + 0.50000*dh2 + 0.50000*dh3 + 0.50000*dh6	)>max) {max =tmp;index =47;}
  if ((tmp = 0.44721*dh1 + 0.44721*dh2 + 0.44721*dh3 + 0.44721*dh4 + 0.44721*dh6)>max) {max =tmp;index =48;}
  if ((tmp = 0.70711*dh5 + 0.70711*dh6					)>max) {max =tmp;index =49;}
  if ((tmp = 0.57735*dh4 + 0.57735*dh5 + 0.57735*dh6			)>max) {max =tmp;index =50;}
  if ((tmp = 0.57735*dh3 + 0.57735*dh5 + 0.57735*dh6			)>max) {max =tmp;index =51;}
  if ((tmp = 0.50000*dh3 + 0.50000*dh4 + 0.50000*dh5 + 0.50000*dh6	)>max) {max =tmp;index =52;}
  if ((tmp = 0.57735*dh2 + 0.57735*dh5 + 0.57735 *dh6			)>max) {max =tmp;index =53;}
  if ((tmp = 0.50000*dh2 + 0.50000*dh4 + 0.50000*dh5 + 0.50000*dh6	)>max) {max =tmp;index =54;}
  if ((tmp = 0.50000*dh2 + 0.50000*dh3 + 0.50000*dh5 + 0.50000*dh6	)>max) {max =tmp;index =55;}
  if ((tmp = 0.44721*dh2 + 0.44721*dh3 + 0.44721*dh4 + 0.44721*dh5 + 0.44721*dh6)>max) {max =tmp;index =56;}
  if ((tmp = 0.57735*dh1 + 0.57735*dh5 + 0.57735 *dh6			)>max) {max =tmp;index =57;}
  if ((tmp = 0.50000*dh1 + 0.50000*dh4 + 0.50000*dh5 + 0.50000*dh6	)>max) {max =tmp;index =58;}
  if ((tmp = 0.50000*dh1 + 0.50000*dh3 + 0.50000*dh5 + 0.50000*dh6	)>max) {max =tmp;index =59;}
  if ((tmp = 0.44721*dh1 + 0.44721*dh3 + 0.44721*dh4 + 0.44721*dh5 + 0.44721*dh6)>max) {max =tmp;index =60;}
  if ((tmp = 0.50000*dh1 + 0.50000*dh2 + 0.50000*dh5 + 0.50000*dh6	)>max) {max =tmp;index =61;}
  if ((tmp = 0.44721*dh1 + 0.44721*dh2 + 0.44721*dh4 + 0.44721*dh5 + 0.44721*dh6)>max) {max =tmp;index =62;}
  if ((tmp = 0.44721*dh1 + 0.44721*dh2 + 0.44721*dh3 + 0.44721*dh5 + 0.44721*dh6)>max) {max =tmp;index =63;}
  if ((tmp = 0.40825*dh1 + 0.40825*dh2 + 0.40825*dh3 + 0.40825*dh4 + 0.40825*dh5 + 0.40825*dh6 )>max) {max =tmp;index =64;}
 
  /* Reset direction */


switch (index){

/* dh1 */
case 1: {if (dh[0]>0) md[0]=-1; else md[0]=1;} break;

/* dh4 */
case 2: {if (dh[3]>0) md[3]=-1; else md[3]=1;} break;

/* dh3 */
case 3: {if (dh[2]>0) md[2]=-1; else md[2]=1;} break;

/* dh3 dh4 */
case 4: {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 5: {if (dh[1]>0) md[1]=-1; else md[1]=1;} break;

/* dh2 dh4 */
case 6: {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 7: {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 8: {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 9: {if (dh[0]>0) md[0]=-1; else md[0]=1;} break;

/* dh1 dh4 */
case 10: {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 11: {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 12: {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 13: {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 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[3]>0) md[3]=-1; else md[3]=1;} break; 

/* dh1 dh2 dh3 */
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;} break;

/* dh1 dh2 dh3 dh4 */
case 16: {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;

/* dh5 */
case 17: {if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh4 dh5 */
case 18: {if (dh[3]>0) md[3]=-1; else md[3]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh3 dh5 */
case 19: {if (dh[2]>0) md[2]=-1; else md[2]=1;
	  if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh3 dh4 dh5 */
case 20: {if (dh[2]>0) md[2]=-1; else md[2]=1;
         if (dh[3]>0) md[3]=-1; else md[3]=1;
         if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh2 dh5 */
case 21: {if (dh[1]>0) md[1]=-1; else md[1]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh2 dh4 dh5 */
case 22: {if (dh[1]>0) md[1]=-1; else md[1]=1;
         if (dh[3]>0) md[3]=-1; else md[3]=1;
         if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh2 dh3 dh5 */
case 23: {if (dh[1]>0) md[1]=-1; else md[1]=1;
         if (dh[2]>0) md[2]=-1; else md[2]=1;
         if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh2 dh3 dh4 dh5 */
case 24: {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;
         if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh1 dh5 */
case 25: {if (dh[0]>0) md[0]=-1; else md[0]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh1 dh4 dh5 */
case 26: {if (dh[0]>0) md[0]=-1; else md[0]=1;
         if (dh[3]>0) md[3]=-1; else md[3]=1;
         if (dh[4]>0) md[4]=-1; else md[4]=1;} break;


/* dh1 dh3 dh5 */
case 27: {if (dh[0]>0) md[0]=-1; else md[0]=1;
          if (dh[2]>0) md[2]=-1; else md[2]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh1 dh3 dh4 dh5 */
case 28: {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;
         if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh1 dh2 dh5 */
case 29: {if (dh[0]>0) md[0]=-1; else md[0]=1;
          if (dh[1]>0) md[1]=-1; else md[1]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh1 dh2 dh4 dh5 */
case 30: {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;
          if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh1 dh2 dh3 dh5 */
case 31: {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[4]>0) md[4]=-1; else md[4]=1;} break;

/* dh1 dh2 dh3 dh4 dh5 */
case 32: {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;
          if (dh[4]>0) md[4]=-1; else md[4]=1;} break;

/************************************************************/

/* dh6 */
case 33: {if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh4 dh6 */
case 34: {if (dh[3]>0) md[3]=-1; else md[3]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh3 dh6 */
case 35: {if (dh[2]>0) md[2]=-1; else md[2]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh3 dh4 dh6 */
case 36: {if (dh[2]>0) md[2]=-1; else md[2]=1;
         if (dh[3]>0) md[3]=-1; else md[3]=1;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh2 dh6 */
case 37: {if (dh[1]>0) md[1]=-1; else md[1]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh2 dh4 dh6 */
case 38: {if (dh[1]>0) md[1]=-1; else md[1]=1;
         if (dh[3]>0) md[3]=-1; else md[3]=1;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh2 dh3 dh6 */
case 39: {if (dh[1]>0) md[1]=-1; else md[1]=1;
         if (dh[2]>0) md[2]=-1; else md[2]=1;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh2 dh3 dh4 dh6 */
case 40: {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;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh6 */
case 41: {if (dh[0]>0) md[0]=-1; else md[0]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh4 dh6 */
case 42: {if (dh[0]>0) md[0]=-1; else md[0]=1;
         if (dh[3]>0) md[3]=-1; else md[3]=1;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;


/* dh1 dh3 dh6 */
case 43: {if (dh[0]>0) md[0]=-1; else md[0]=1;
          if (dh[2]>0) md[2]=-1; else md[2]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh3 dh4 dh6 */
case 44: {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;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh2 dh6 */
case 45: {if (dh[0]>0) md[0]=-1; else md[0]=1;
          if (dh[1]>0) md[1]=-1; else md[1]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh2 dh4 dh6 */
case 46: {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;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh2 dh3 dh6 */
case 47: {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[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh2 dh3 dh4 dh6 */
case 48: {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;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/************************************************************/

/* dh5 dh6 */
case 49: {if (dh[4]>0) md[4]=-1; else md[4]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh4 dh5 dh6 */
case 50: {if (dh[3]>0) md[3]=-1; else md[3]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh3 dh5 dh6 */
case 51: {if (dh[2]>0) md[2]=-1; else md[2]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh3 dh4 dh5 dh6 */
case 52: {if (dh[2]>0) md[2]=-1; else md[2]=1;
         if (dh[3]>0) md[3]=-1; else md[3]=1;
         if (dh[4]>0) md[4]=-1; else md[4]=1;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh2 dh5 dh6 */
case 53: {if (dh[1]>0) md[1]=-1; else md[1]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh2 dh4 dh5 dh6 */
case 54: {if (dh[1]>0) md[1]=-1; else md[1]=1;
         if (dh[3]>0) md[3]=-1; else md[3]=1;
         if (dh[4]>0) md[4]=-1; else md[4]=1;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh2 dh3 dh5 dh6 */
case 55: {if (dh[1]>0) md[1]=-1; else md[1]=1;
         if (dh[2]>0) md[2]=-1; else md[2]=1;
         if (dh[4]>0) md[4]=-1; else md[4]=1;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh2 dh3 dh4 dh5 dh6 */
case 56: {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;
         if (dh[4]>0) md[4]=-1; else md[4]=1;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh5 dh6 */
case 57: {if (dh[0]>0) md[0]=-1; else md[0]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh4 dh5 dh6 */
case 58: {if (dh[0]>0) md[0]=-1; else md[0]=1;
         if (dh[3]>0) md[3]=-1; else md[3]=1;
         if (dh[4]>0) md[4]=-1; else md[4]=1;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh3 dh5 dh6 */
case 59: {if (dh[0]>0) md[0]=-1; else md[0]=1;
          if (dh[2]>0) md[2]=-1; else md[2]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1; 
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh3 dh4 dh5 dh6 */
case 60: {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;
         if (dh[4]>0) md[4]=-1; else md[4]=1;
         if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh2 dh5 dh6 */
case 61: {if (dh[0]>0) md[0]=-1; else md[0]=1;
          if (dh[1]>0) md[1]=-1; else md[1]=1;
          if (dh[4]>0) md[4]=-1; else md[4]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh2 dh4 dh5 dh6 */
case 62: {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;
          if (dh[4]>0) md[4]=-1; else md[4]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh2 dh3 dh5 dh6 */
case 63: {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[4]>0) md[4]=-1; else md[4]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

/* dh1 dh2 dh3 dh4 dh5 dh6 */
case 64: {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;
          if (dh[4]>0) md[4]=-1; else md[4]=1;
          if (dh[5]>0) md[5]=-1; else md[5]=1;} break;

default: {cout << "Fatal error in: HardwiredSelectBest6D\n"; exit(-1);} break;

}

  return md;
}

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

  // Calculate Bk; see affine.mws
  MatrixC Bk(6,6); Bk.Fill(0);
  Bk[0][0] = X[4]+1;
  Bk[1][1] = X[4]+1;
  Bk[2][2] = X[4]+1;
  Bk[3][0] = -X[1];
  Bk[4][1] = -X[1];
  Bk[5][2] = -X[1];
  Bk[0][3] = -X[3];
  Bk[1][4] = -X[3];
  Bk[2][5] = -X[3];
  Bk[3][3] = X[0]+1;
  Bk[4][4] = X[0]+1;
  Bk[5][5] = X[0]+1;

  //cout << "Bk=" << Bk << "\n";
   
  // Calculate zTk for all pixels in the region
  VectorC zTk(6, 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;
  //cout << "zTk" << zTk << "\n";
  RealT detA = (X[0]+1)*(X[4]+1) - X[1]*X[3];
  zTk /= detA;
  //cout << "zTk/detA=" << zTk << "\n";
  VectorC gTk = Bk * zTk;
  //cout << "gTk=" << gTk << "\n";

  return zTk;
}


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

  Affine2dC result_aff = A_aff*B_aff;

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

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

  Affine2dC inv_A_aff = A_aff.I();

  VectorC result(6, 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.SRMatrix().A10();
  result[4] = inv_A_aff.SRMatrix().A11()-1;
  result[5] = inv_A_aff.Translation().Y();
  
  return (result);
}


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

class AffineMotModelC 
  : public MotModelC
{   
 public:
  //: Constructors, copies, assigment, and destructor
  //: -----------------------------------------------
  
  AffineMotModelC()
    : MotModelC(* new AffineMotModelBodyC())
    {}
  //: Default constructor
  // This creates an invalid handle.
  // You can remove this if you always want to create a full object.
  
  AffineMotModelC(istream &in)
    : MotModelC(* new AffineMotModelBodyC(in))
    {}
  //: Stream constructor
  // This creates a new instance of the class from an input stream.
  
  
 protected:
  AffineMotModelC(AffineMotModelBodyC &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
  AffineMotModelBodyC &Body() 
    { return static_cast<AffineMotModelBodyC &>(MotModelC::Body());  }
  //: Access body.
  // This isn't really needed, they're just to ensure
  // all derived classes work properly.

  inline
  const AffineMotModelBodyC &Body() const
    { return static_cast<const AffineMotModelBodyC &>(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 AffineMotModelC &out);
  //: output stream operator
  
  friend istream &operator>>(istream &s, AffineMotModelC &in);
  //: input stream operator
  
};

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

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

#endif