/*
 * Copyright (c) ICG. All rights reserved.
 * See copyright.txt for more information.
 *
 * Institute for Computer Graphics and Vision
 * Graz, University of Technology / Austria
 *
 *
 * This software is distributed WITHOUT ANY WARRANTY; without even
 * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  See the above copyright notices for more information.
 *
 *
 * Project     : MIPItkProjects
 * Module      : Evaluation
 * Class       : $RCSfile: SimulatedBreathingTransformation.cxx,v $
 * Language    : C++
 * Description : 
 *
 * Author     : Martin Urschler
 * EMail      : urschler@icg.tu-graz.ac.at
 * Date       : $Date: 2006-09-14 16:49:25 $
 * Version    : $Revision: 1.4 $
 * Full Id    : $Id: SimulatedBreathingTransformation.cxx,v 1.4 2006-09-14 16:49:25 urschler Exp $
 *
 */


#include "SimulatedBreathingTransformation.h"
#include "SmallUtilityMethods.h"


#define MLDUMP(var) std::cout << #var << ": " << var << std::endl; 
 

/* SimulatedBreathingTransformation::SimulatedBreathingTransformation( 
    const MaskImageType::Pointer& image,
    const double translation_vertical_mm, 
    const double translation_inplane_mm )
{
   //MaskImageType::SpacingType spacing = image->GetSpacing();

   m_variance_factor = 0.33;
   
   m_translation_vertical = translation_vertical_mm; 
   //   translation_vertical_mm / static_cast<double>( spacing[2] );
   MLDUMP(m_translation_vertical)
   m_translation_inplane = translation_inplane_mm;
   //   translation_inplane_mm / ( 0.5 * (spacing[0] + spacing[1]) );
   MLDUMP(m_translation_inplane)

   //    // calculate mean of the x and y values of the voxels
   m_center =
      SmallUtilityMethods<MaskImageType>::calculateCenterInBinaryImage( image );
   MLDUMP(m_center);

   // calculate the x,y bounding box of the volume
   SmallUtilityMethods<MaskImageType>::
      calculateBoundingBoxHavingHomogenousBackground( image, m_bound_box_min,
                                                      m_bound_box_max );

   MLDUMP(m_bound_box_min)
   MLDUMP(m_bound_box_max)

   m_image_size = image->GetLargestPossibleRegion().GetSize();
}*/


/*MaskImageType::PointType SimulatedBreathingTransformation::apply( 
   const MaskImageType::PointType& p )
{
   MaskImageType::PointType q = p;

   const double inv_variance_x = 1.0 / 
      vnl_math_sqr(m_variance_factor * (m_bound_box_max[0]-m_bound_box_min[0]));
   const double inv_variance_y = 1.0 / 
      vnl_math_sqr(m_variance_factor * (m_bound_box_max[1]-m_bound_box_min[1]));

   // vertical transformation component

   double gauss_factor = vcl_exp( -0.5 * ( inv_variance_x * 
      vnl_math_sqr(p[0] - m_center[0]) + inv_variance_y * 
      vnl_math_sqr(p[1] - m_center[1]) ) );
      
   q[2] -= m_translation_vertical * gauss_factor;
   
   if (q[2] < 0.0) // check if we leave the data set borders!
      q[2] = 0.0;

   // inplane transformation component

   const double vector_to_center_x = p[0] - m_center[0];
   const double vector_to_center_y = p[1] - m_center[1];
   const double distance_to_center = 
      vcl_sqrt( vnl_math_sqr(vector_to_center_x) + 
                vnl_math_sqr(vector_to_center_y) );
   
   if ( fabs(distance_to_center) < vcl_numeric_limits<double>::epsilon() )
   {
      q[0] = m_center[0];
      q[1] = m_center[1];
   }
   else
   {
      const double inv_distance_to_center = 1.0 / distance_to_center;

      double transformed_distance_to_center = 
         distance_to_center - (1.0 - gauss_factor) * m_translation_inplane;
      if (transformed_distance_to_center < 0.0)
         transformed_distance_to_center = 0.0;
   
      q[0] = m_center[0] + transformed_distance_to_center * 
         vector_to_center_x * inv_distance_to_center;
      q[1] = m_center[1] + transformed_distance_to_center * 
         vector_to_center_y * inv_distance_to_center;
   }
   
   return q;
}*/

/*
MaskImageType::PointType 
SimulatedBreathingTransformation::applySimpleTranslations( 
   const MaskImageType::PointType& p )
{
   MaskImageType::PointType q = p;

   // vertical transformation component
   q[2] -= m_translation_vertical;
   if (q[2] < 0.0) // check if we leave the data set borders!
      q[2] = 0.0;

   // inplane transformation component

   const double vector_to_center_x = p[0] - m_center[0];
   const double vector_to_center_y = p[1] - m_center[1];
   const double distance_to_center = 
      vcl_sqrt( vnl_math_sqr(vector_to_center_x) + 
                vnl_math_sqr(vector_to_center_y) );

   if ( fabs(distance_to_center) < vcl_numeric_limits<double>::epsilon() )
   {
      q[0] = m_center[0];
      q[1] = m_center[1];
   }
   else
   {
      const double inv_distance_to_center = 1.0 / distance_to_center;

      double transformed_distance_to_center = 
         distance_to_center - m_translation_inplane;
      if (transformed_distance_to_center < 0.0)
         transformed_distance_to_center = 0.0;

      q[0] = m_center[0] + transformed_distance_to_center * 
         vector_to_center_x * inv_distance_to_center;
      q[1] = m_center[1] + transformed_distance_to_center * 
         vector_to_center_y * inv_distance_to_center;
   }
   
   return q;
}*/




SimulatedBreathingTransformation::SimulatedBreathingTransformation( 
    const MaskImageType::Pointer& image,
    const double translation_vertical_mm, 
    const double translation_inplane_mm )
{
   m_variance_factor = 0.33;
   
   m_translation_vertical_mm = translation_vertical_mm; 
   MLDUMP(m_translation_vertical_mm)
   m_translation_inplane_mm = translation_inplane_mm;
   MLDUMP(m_translation_inplane_mm)

   // calculate mean of the x and y values of the voxels
   MaskImageType::IndexType center =
      SmallUtilityMethods<MaskImageType>::calculateCenterInBinaryImage( image );
   image->TransformIndexToPhysicalPoint( center, m_center );
   MLDUMP(m_center);

   MaskImageType::IndexType bound_box_min, bound_box_max;

   // calculate the x,y bounding box of the volume
   SmallUtilityMethods<MaskImageType>::
      calculateBoundingBoxHavingHomogenousBackground( image, bound_box_min,
                                                      bound_box_max );

   image->TransformIndexToPhysicalPoint( bound_box_min, m_bound_box_min );
   image->TransformIndexToPhysicalPoint( bound_box_max, m_bound_box_max );

   MLDUMP(m_bound_box_min)
   MLDUMP(m_bound_box_max)

   //m_image_size = image->GetLargestPossibleRegion().GetSize();
}


// Note we have to apply the movements in the inverse directions in order to
// be consistent with the ITK scheme for transforms and deformation field
// directions!

SimulatedBreathingTransformation::PointType 
SimulatedBreathingTransformation::apply( 
   const SimulatedBreathingTransformation::PointType& p )
{
   PointType q = p;

   const double inv_variance_x = 1.0 / 
      vnl_math_sqr(m_variance_factor * (m_bound_box_max[0]-m_bound_box_min[0]));
   const double inv_variance_y = 1.0 / 
      vnl_math_sqr(m_variance_factor * (m_bound_box_max[1]-m_bound_box_min[1]));

   // vertical transformation component

   const double gauss_factor = vcl_exp( -0.5 * ( inv_variance_x * 
      vnl_math_sqr(p[0] - m_center[0]) + inv_variance_y * 
      vnl_math_sqr(p[1] - m_center[1]) ) );
      
   q[2] += m_translation_vertical_mm * gauss_factor;

   // inplane transformation component

   const double vector_to_center_x = p[0] - m_center[0];
   const double vector_to_center_y = p[1] - m_center[1];
   double distance_to_center = 
      vcl_sqrt( vnl_math_sqr(vector_to_center_x) + 
                vnl_math_sqr(vector_to_center_y) );
   
   // check if we lie exactly on the center of the image, this would lead
   // to a division by zero
   if ( distance_to_center < vcl_numeric_limits<double>::epsilon() )
   {
      distance_to_center = vcl_numeric_limits<double>::epsilon();
   }
   
   const double inv_distance_to_center = 1.0 / distance_to_center;

   double transformed_distance_to_center = 
      distance_to_center + (1.0 - gauss_factor) * m_translation_inplane_mm;

   q[0] = m_center[0] + transformed_distance_to_center * 
      vector_to_center_x * inv_distance_to_center;
   q[1] = m_center[1] + transformed_distance_to_center * 
      vector_to_center_y * inv_distance_to_center;   
   
   return q;
}



SimulatedBreathingTransformation::PointType 
SimulatedBreathingTransformation::applySimpleTranslations( 
   const SimulatedBreathingTransformation::PointType& p )
{
   PointType q = p;

   // vertical transformation component
   q[2] += m_translation_vertical_mm;

   // inplane transformation component

   const double vector_to_center_x = p[0] - m_center[0];
   const double vector_to_center_y = p[1] - m_center[1];
   double distance_to_center = 
      vcl_sqrt( vnl_math_sqr(vector_to_center_x) + 
                vnl_math_sqr(vector_to_center_y) );

   if ( distance_to_center < vcl_numeric_limits<double>::epsilon() )
   {
      distance_to_center = vcl_numeric_limits<double>::epsilon();
   }
   
   const double inv_distance_to_center = 1.0 / distance_to_center;

   double transformed_distance_to_center = 
      distance_to_center + m_translation_inplane_mm;

   q[0] = m_center[0] + transformed_distance_to_center * 
      vector_to_center_x * inv_distance_to_center;
   q[1] = m_center[1] + transformed_distance_to_center * 
      vector_to_center_y * inv_distance_to_center;
   
   return q;
}





// PointSetType::PointType SimulatedBreathingTransformation::applyInverse( const PointSetType::PointType& q )
// {
//    PointSetType::PointType p = q;
// 
//    const double inv_variance_x = 1.0 / SQR(m_variance_factor * (m_bound_box_max[0] - m_bound_box_min[0]));
//    const double inv_variance_y = 1.0 / SQR(m_variance_factor * (m_bound_box_max[1] - m_bound_box_min[1]));
// 
//    // vertical transformation component
// 
//    double gauss_factor = exp( -0.5 * ( inv_variance_x * SQR(q[0] - m_center[0]) + inv_variance_y * SQR(q[1] - m_center[1]) ) );
//    p[2] += m_translation_vertical * gauss_factor;
//    
//    if (p[2] > m_image_size[2] - 1) // check if we leave the data set borders!
//       p[2] = m_image_size[2] - 1;
// 
//    // inplane transformation component
// 
//    const double vector_to_center_x = q[0] - m_center[0];
//    const double vector_to_center_y = q[1] - m_center[1];
//    const double distance_to_center = sqrt( SQR(vector_to_center_x) + SQR(vector_to_center_y) );
//    const double inv_distance_to_center = 1.0 / distance_to_center;
// 
//    double transformed_distance_to_center = distance_to_center + (1.0 - gauss_factor) * m_translation_inplane;
// 
//    p[0] = m_center[0] + transformed_distance_to_center * vector_to_center_x * inv_distance_to_center;
//    p[1] = m_center[1] + transformed_distance_to_center * vector_to_center_y * inv_distance_to_center;
// 
//    // crop p[1] and p[2] at the borders
//    if ( p[0] < 0.0 ) p[0] = 0.0;
//    if ( p[1] < 0.0 ) p[1] = 0.0;
// 
//    if ( p[0] > m_image_size[0] - 1 ) p[0] = m_image_size[0] - 1;
//    if ( p[1] > m_image_size[1] - 1 ) p[1] = m_image_size[1] - 1;
//    
//    return p;
// }