/*
 * Copyright (C) 2011 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

// $Id: dbreg.cpp,v 1.31 2011/06/17 14:04:32 mbansal Exp $
#include "dbreg.h"
#include <string.h>
#include <stdio.h>


#if PROFILE
#endif

//#include <iostream>

db_FrameToReferenceRegistration::db_FrameToReferenceRegistration() :
  m_initialized(false),m_nr_matches(0),m_over_allocation(256),m_nr_bins(20),m_max_cost_pix(30), m_quarter_resolution(false)
{
  m_reference_image = NULL;
  m_aligned_ins_image = NULL;

  m_quarter_res_image = NULL;
  m_horz_smooth_subsample_image = NULL;

  m_x_corners_ref = NULL;
  m_y_corners_ref = NULL;

  m_x_corners_ins = NULL;
  m_y_corners_ins = NULL;

  m_match_index_ref = NULL;
  m_match_index_ins = NULL;

  m_inlier_indices = NULL;

  m_num_inlier_indices = 0;

  m_temp_double = NULL;
  m_temp_int = NULL;

  m_corners_ref = NULL;
  m_corners_ins = NULL;

  m_sq_cost = NULL;
  m_cost_histogram = NULL;

  profile_string = NULL;

  db_Identity3x3(m_K);
  db_Identity3x3(m_H_ref_to_ins);
  db_Identity3x3(m_H_dref_to_ref);

  m_sq_cost_computed = false;
  m_reference_set = false;

  m_reference_update_period = 0;
  m_nr_frames_processed = 0;

  return;
}

db_FrameToReferenceRegistration::~db_FrameToReferenceRegistration()
{
  Clean();
}

void db_FrameToReferenceRegistration::Clean()
{
  if ( m_reference_image )
    db_FreeImage_u(m_reference_image,m_im_height);

  if ( m_aligned_ins_image )
    db_FreeImage_u(m_aligned_ins_image,m_im_height);

  if ( m_quarter_res_image )
  {
    db_FreeImage_u(m_quarter_res_image, m_im_height);
  }

  if ( m_horz_smooth_subsample_image )
  {
    db_FreeImage_u(m_horz_smooth_subsample_image, m_im_height*2);
  }

  delete [] m_x_corners_ref;
  delete [] m_y_corners_ref;

  delete [] m_x_corners_ins;
  delete [] m_y_corners_ins;

  delete [] m_match_index_ref;
  delete [] m_match_index_ins;

  delete [] m_temp_double;
  delete [] m_temp_int;

  delete [] m_corners_ref;
  delete [] m_corners_ins;

  delete [] m_sq_cost;
  delete [] m_cost_histogram;

  delete [] m_inlier_indices;

  if(profile_string)
    delete [] profile_string;

  m_reference_image = NULL;
  m_aligned_ins_image = NULL;

  m_quarter_res_image = NULL;
  m_horz_smooth_subsample_image = NULL;

  m_x_corners_ref = NULL;
  m_y_corners_ref = NULL;

  m_x_corners_ins = NULL;
  m_y_corners_ins = NULL;

  m_match_index_ref = NULL;
  m_match_index_ins = NULL;

  m_inlier_indices = NULL;

  m_temp_double = NULL;
  m_temp_int = NULL;

  m_corners_ref = NULL;
  m_corners_ins = NULL;

  m_sq_cost = NULL;
  m_cost_histogram = NULL;
}

void db_FrameToReferenceRegistration::Init(int width, int height,
                       int    homography_type,
                       int    max_iterations,
                       bool   linear_polish,
                       bool   quarter_resolution,
                       double scale,
                       unsigned int reference_update_period,
                       bool   do_motion_smoothing,
                       double motion_smoothing_gain,
                       int    nr_samples,
                       int    chunk_size,
                       int    cd_target_nr_corners,
                       double cm_max_disparity,
                           bool   cm_use_smaller_matching_window,
                       int    cd_nr_horz_blocks,
                       int    cd_nr_vert_blocks
                       )
{
  Clean();

  m_reference_update_period = reference_update_period;
  m_nr_frames_processed = 0;

  m_do_motion_smoothing = do_motion_smoothing;
  m_motion_smoothing_gain = motion_smoothing_gain;

  m_stab_smoother.setSmoothingFactor(m_motion_smoothing_gain);

  m_quarter_resolution = quarter_resolution;

  profile_string = new char[10240];

  if (m_quarter_resolution == true)
  {
    width = width/2;
    height = height/2;

    m_horz_smooth_subsample_image = db_AllocImage_u(width,height*2,m_over_allocation);
    m_quarter_res_image = db_AllocImage_u(width,height,m_over_allocation);
  }

  m_im_width = width;
  m_im_height = height;

  double temp[9];
  db_Approx3DCalMat(m_K,temp,m_im_width,m_im_height);

  m_homography_type = homography_type;
  m_max_iterations = max_iterations;
  m_scale = 2/(m_K[0]+m_K[4]);
  m_nr_samples = nr_samples;
  m_chunk_size = chunk_size;

  double outlier_t1 = 5.0;

  m_outlier_t2 = outlier_t1*outlier_t1;//*m_scale*m_scale;

  m_current_is_reference = false;

  m_linear_polish = linear_polish;

  m_reference_image = db_AllocImage_u(m_im_width,m_im_height,m_over_allocation);
  m_aligned_ins_image = db_AllocImage_u(m_im_width,m_im_height,m_over_allocation);

  // initialize feature detection and matching:
  //m_max_nr_corners = m_cd.Init(m_im_width,m_im_height,cd_target_nr_corners,cd_nr_horz_blocks,cd_nr_vert_blocks,0.0,0.0);
  m_max_nr_corners = m_cd.Init(m_im_width,m_im_height,cd_target_nr_corners,cd_nr_horz_blocks,cd_nr_vert_blocks,DB_DEFAULT_ABS_CORNER_THRESHOLD/500.0,0.0);

    int use_21 = 0;
  m_max_nr_matches = m_cm.Init(m_im_width,m_im_height,cm_max_disparity,m_max_nr_corners,DB_DEFAULT_NO_DISPARITY,cm_use_smaller_matching_window,use_21);

  // allocate space for corner feature locations for reference and inspection images:
  m_x_corners_ref = new double [m_max_nr_corners];
  m_y_corners_ref = new double [m_max_nr_corners];

  m_x_corners_ins = new double [m_max_nr_corners];
  m_y_corners_ins = new double [m_max_nr_corners];

  // allocate space for match indices:
  m_match_index_ref = new int [m_max_nr_matches];
  m_match_index_ins = new int [m_max_nr_matches];

  m_temp_double = new double [12*DB_DEFAULT_NR_SAMPLES+10*m_max_nr_matches];
  m_temp_int = new int [db_maxi(DB_DEFAULT_NR_SAMPLES,m_max_nr_matches)];

  // allocate space for homogenous image points:
  m_corners_ref = new double [3*m_max_nr_corners];
  m_corners_ins = new double [3*m_max_nr_corners];

  // allocate cost array and histogram:
  m_sq_cost = new double [m_max_nr_matches];
  m_cost_histogram = new int [m_nr_bins];

  // reserve array:
  //m_inlier_indices.reserve(m_max_nr_matches);
  m_inlier_indices = new int[m_max_nr_matches];

  m_initialized = true;

  m_max_inlier_count = 0;
}


#define MB 0
// Save the reference image, detect features and update the dref-to-ref transformation
int db_FrameToReferenceRegistration::UpdateReference(const unsigned char * const * im, bool subsample, bool detect_corners)
{
  double temp[9];
  db_Multiply3x3_3x3(temp,m_H_dref_to_ref,m_H_ref_to_ins);
  db_Copy9(m_H_dref_to_ref,temp);

  const unsigned char * const * imptr = im;

  if (m_quarter_resolution && subsample)
  {
    GenerateQuarterResImage(im);
    imptr = m_quarter_res_image;
  }

  // save the reference image, detect features and quit
  db_CopyImage_u(m_reference_image,imptr,m_im_width,m_im_height,m_over_allocation);

  if(detect_corners)
  {
    #if MB
    m_cd.DetectCorners(imptr, m_x_corners_ref,m_y_corners_ref,&m_nr_corners_ref);
    int nr = 0;
    for(int k=0; k<m_nr_corners_ref; k++)
    {
        if(m_x_corners_ref[k]>m_im_width/3)
        {
            m_x_corners_ref[nr] = m_x_corners_ref[k];
            m_y_corners_ref[nr] = m_y_corners_ref[k];
            nr++;
        }

    }
    m_nr_corners_ref = nr;
    #else
    m_cd.DetectCorners(imptr, m_x_corners_ref,m_y_corners_ref,&m_nr_corners_ref);
    #endif
  }
  else
  {
    m_nr_corners_ref = m_nr_corners_ins;

    for(int k=0; k<m_nr_corners_ins; k++)
    {
        m_x_corners_ref[k] = m_x_corners_ins[k];
        m_y_corners_ref[k] = m_y_corners_ins[k];
    }

  }

  db_Identity3x3(m_H_ref_to_ins);

  m_max_inlier_count = 0;   // Reset to 0 as no inliers seen until now
  m_sq_cost_computed = false;
  m_reference_set = true;
  m_current_is_reference = true;
  return 1;
}

void db_FrameToReferenceRegistration::Get_H_dref_to_ref(double H[9])
{
  db_Copy9(H,m_H_dref_to_ref);
}

void db_FrameToReferenceRegistration::Get_H_dref_to_ins(double H[9])
{
  db_Multiply3x3_3x3(H,m_H_dref_to_ref,m_H_ref_to_ins);
}

void db_FrameToReferenceRegistration::Set_H_dref_to_ins(double H[9])
{
    double H_ins_to_ref[9];

    db_Identity3x3(H_ins_to_ref);   // Ensure it has proper values
    db_InvertAffineTransform(H_ins_to_ref,m_H_ref_to_ins);  // Invert to get ins to ref
    db_Multiply3x3_3x3(m_H_dref_to_ref,H,H_ins_to_ref); // Update dref to ref using the input H from dref to ins
}


void db_FrameToReferenceRegistration::ResetDisplayReference()
{
  db_Identity3x3(m_H_dref_to_ref);
}

bool db_FrameToReferenceRegistration::NeedReferenceUpdate()
{
  // If less than 50% of the starting number of inliers left, then its time to update the reference.
  if(m_max_inlier_count>0 && float(m_num_inlier_indices)/float(m_max_inlier_count)<0.5)
    return true;
  else
    return false;
}

int db_FrameToReferenceRegistration::AddFrame(const unsigned char * const * im, double H[9],bool force_reference,bool prewarp)
{
  m_current_is_reference = false;
  if(!m_reference_set || force_reference)
    {
      db_Identity3x3(m_H_ref_to_ins);
      db_Copy9(H,m_H_ref_to_ins);

      UpdateReference(im,true,true);
      return 0;
    }

  const unsigned char * const * imptr = im;

  if (m_quarter_resolution)
  {
    if (m_quarter_res_image)
    {
      GenerateQuarterResImage(im);
    }

    imptr = (const unsigned char * const* )m_quarter_res_image;
  }

  double H_last[9];
  db_Copy9(H_last,m_H_ref_to_ins);
  db_Identity3x3(m_H_ref_to_ins);

  m_sq_cost_computed = false;

  // detect corners on inspection image and match to reference image features:s

  // @jke - Adding code to time the functions.  TODO: Remove after test
#if PROFILE
  double iTimer1, iTimer2;
  char str[255];
  strcpy(profile_string,"\n");
  sprintf(str,"[%dx%d] %p\n",m_im_width,m_im_height,im);
  strcat(profile_string, str);
#endif

  // @jke - Adding code to time the functions.  TODO: Remove after test
#if PROFILE
  iTimer1 = now_ms();
#endif
  m_cd.DetectCorners(imptr, m_x_corners_ins,m_y_corners_ins,&m_nr_corners_ins);
  // @jke - Adding code to time the functions.  TODO: Remove after test
# if PROFILE
  iTimer2 = now_ms();
  double elapsedTimeCorner = iTimer2 - iTimer1;
  sprintf(str,"Corner Detection [%d corners] = %g ms\n",m_nr_corners_ins, elapsedTimeCorner);
  strcat(profile_string, str);
#endif

  // @jke - Adding code to time the functions.  TODO: Remove after test
#if PROFILE
  iTimer1 = now_ms();
#endif
    if(prewarp)
  m_cm.Match(m_reference_image,imptr,m_x_corners_ref,m_y_corners_ref,m_nr_corners_ref,
         m_x_corners_ins,m_y_corners_ins,m_nr_corners_ins,
         m_match_index_ref,m_match_index_ins,&m_nr_matches,H,0);
    else
  m_cm.Match(m_reference_image,imptr,m_x_corners_ref,m_y_corners_ref,m_nr_corners_ref,
         m_x_corners_ins,m_y_corners_ins,m_nr_corners_ins,
         m_match_index_ref,m_match_index_ins,&m_nr_matches);
  // @jke - Adding code to time the functions.  TODO: Remove after test
# if PROFILE
  iTimer2 = now_ms();
  double elapsedTimeMatch = iTimer2 - iTimer1;
  sprintf(str,"Matching [%d] = %g ms\n",m_nr_matches,elapsedTimeMatch);
  strcat(profile_string, str);
#endif


  // copy out matching features:
  for ( int i = 0; i < m_nr_matches; ++i )
    {
      int offset = 3*i;
      m_corners_ref[offset  ] = m_x_corners_ref[m_match_index_ref[i]];
      m_corners_ref[offset+1] = m_y_corners_ref[m_match_index_ref[i]];
      m_corners_ref[offset+2] = 1.0;

      m_corners_ins[offset  ] = m_x_corners_ins[m_match_index_ins[i]];
      m_corners_ins[offset+1] = m_y_corners_ins[m_match_index_ins[i]];
      m_corners_ins[offset+2] = 1.0;
    }

  // @jke - Adding code to time the functions.  TODO: Remove after test
#if PROFILE
  iTimer1 = now_ms();
#endif
  // perform the alignment:
  db_RobImageHomography(m_H_ref_to_ins, m_corners_ref, m_corners_ins, m_nr_matches, m_K, m_K, m_temp_double, m_temp_int,
            m_homography_type,NULL,m_max_iterations,m_max_nr_matches,m_scale,
            m_nr_samples, m_chunk_size);
  // @jke - Adding code to time the functions.  TODO: Remove after test
# if PROFILE
  iTimer2 = now_ms();
  double elapsedTimeHomography = iTimer2 - iTimer1;
  sprintf(str,"Homography = %g ms\n",elapsedTimeHomography);
  strcat(profile_string, str);
#endif


  SetOutlierThreshold();

  // Compute the inliers for the db compute m_H_ref_to_ins
  ComputeInliers(m_H_ref_to_ins);

  // Update the max inlier count
  m_max_inlier_count = (m_max_inlier_count > m_num_inlier_indices)?m_max_inlier_count:m_num_inlier_indices;

  // Fit a least-squares model to just the inliers and put it in m_H_ref_to_ins
  if(m_linear_polish)
    Polish(m_inlier_indices, m_num_inlier_indices);

  if (m_quarter_resolution)
  {
    m_H_ref_to_ins[2] *= 2.0;
    m_H_ref_to_ins[5] *= 2.0;
  }

#if PROFILE
  sprintf(str,"#Inliers = %d \n",m_num_inlier_indices);
  strcat(profile_string, str);
#endif
/*
  ///// CHECK IF CURRENT TRANSFORMATION GOOD OR BAD ////
  ///// IF BAD, then update reference to the last correctly aligned inspection frame;
  if(m_num_inlier_indices<5)//0.9*m_nr_matches || m_nr_matches < 20)
  {
    db_Copy9(m_H_ref_to_ins,H_last);
    UpdateReference(imptr,false);
//  UpdateReference(m_aligned_ins_image,false);
  }
  else
  {
  ///// IF GOOD, then update the last correctly aligned inspection frame to be this;
  //db_CopyImage_u(m_aligned_ins_image,imptr,m_im_width,m_im_height,m_over_allocation);
*/
  if(m_do_motion_smoothing)
    SmoothMotion();

   // Disable debug printing
   // db_PrintDoubleMatrix(m_H_ref_to_ins,3,3);

  db_Copy9(H, m_H_ref_to_ins);

  m_nr_frames_processed++;
{
  if ( (m_nr_frames_processed % m_reference_update_period) == 0 )
  {
    //UpdateReference(imptr,false, false);

    #if MB
    UpdateReference(imptr,false, true);
    #else
    UpdateReference(imptr,false, false);
    #endif
  }


  }



  return 1;
}

//void db_FrameToReferenceRegistration::ComputeInliers(double H[9],std::vector<int> &inlier_indices)
void db_FrameToReferenceRegistration::ComputeInliers(double H[9])
{
  double totnummatches = m_nr_matches;
  int inliercount=0;

  m_num_inlier_indices = 0;
//  inlier_indices.clear();

  for(int c=0; c < totnummatches; c++ )
    {
      if (m_sq_cost[c] <= m_outlier_t2)
    {
      m_inlier_indices[inliercount] = c;
      inliercount++;
    }
    }

  m_num_inlier_indices = inliercount;
  double frac=inliercount/totnummatches;
}

//void db_FrameToReferenceRegistration::Polish(std::vector<int> &inlier_indices)
void db_FrameToReferenceRegistration::Polish(int *inlier_indices, int &num_inlier_indices)
{
  db_Zero(m_polish_C,36);
  db_Zero(m_polish_D,6);
  for (int i=0;i<num_inlier_indices;i++)
    {
      int j = 3*inlier_indices[i];
      m_polish_C[0]+=m_corners_ref[j]*m_corners_ref[j];
      m_polish_C[1]+=m_corners_ref[j]*m_corners_ref[j+1];
      m_polish_C[2]+=m_corners_ref[j];
      m_polish_C[7]+=m_corners_ref[j+1]*m_corners_ref[j+1];
      m_polish_C[8]+=m_corners_ref[j+1];
      m_polish_C[14]+=1;
      m_polish_D[0]+=m_corners_ref[j]*m_corners_ins[j];
      m_polish_D[1]+=m_corners_ref[j+1]*m_corners_ins[j];
      m_polish_D[2]+=m_corners_ins[j];
      m_polish_D[3]+=m_corners_ref[j]*m_corners_ins[j+1];
      m_polish_D[4]+=m_corners_ref[j+1]*m_corners_ins[j+1];
      m_polish_D[5]+=m_corners_ins[j+1];
    }

  double a=db_maxd(m_polish_C[0],m_polish_C[7]);
  m_polish_C[0]/=a; m_polish_C[1]/=a;   m_polish_C[2]/=a;
  m_polish_C[7]/=a; m_polish_C[8]/=a; m_polish_C[14]/=a;

  m_polish_D[0]/=a; m_polish_D[1]/=a;   m_polish_D[2]/=a;
  m_polish_D[3]/=a; m_polish_D[4]/=a;   m_polish_D[5]/=a;


  m_polish_C[6]=m_polish_C[1];
  m_polish_C[12]=m_polish_C[2];
  m_polish_C[13]=m_polish_C[8];

  m_polish_C[21]=m_polish_C[0]; m_polish_C[22]=m_polish_C[1]; m_polish_C[23]=m_polish_C[2];
  m_polish_C[28]=m_polish_C[7]; m_polish_C[29]=m_polish_C[8];
  m_polish_C[35]=m_polish_C[14];


  double d[6];
  db_CholeskyDecomp6x6(m_polish_C,d);
  db_CholeskyBacksub6x6(m_H_ref_to_ins,m_polish_C,d,m_polish_D);
}

void db_FrameToReferenceRegistration::EstimateSecondaryModel(double H[9])
{
  /*      if ( m_current_is_reference )
      {
      db_Identity3x3(H);
      return;
      }
  */

  // select the outliers of the current model:
  SelectOutliers();

  // perform the alignment:
  db_RobImageHomography(m_H_ref_to_ins, m_corners_ref, m_corners_ins, m_nr_matches, m_K, m_K, m_temp_double, m_temp_int,
            m_homography_type,NULL,m_max_iterations,m_max_nr_matches,m_scale,
            m_nr_samples, m_chunk_size);

  db_Copy9(H,m_H_ref_to_ins);
}

void db_FrameToReferenceRegistration::ComputeCostArray()
{
  if ( m_sq_cost_computed ) return;

  for( int c=0, k=0 ;c < m_nr_matches; c++, k=k+3)
    {
      m_sq_cost[c] = SquaredInhomogenousHomographyError(m_corners_ins+k,m_H_ref_to_ins,m_corners_ref+k);
    }

  m_sq_cost_computed = true;
}

void db_FrameToReferenceRegistration::SelectOutliers()
{
  int nr_outliers=0;

  ComputeCostArray();

  for(int c=0, k=0 ;c<m_nr_matches;c++,k=k+3)
    {
      if (m_sq_cost[c] > m_outlier_t2)
    {
      int offset = 3*nr_outliers++;
      db_Copy3(m_corners_ref+offset,m_corners_ref+k);
      db_Copy3(m_corners_ins+offset,m_corners_ins+k);
    }
    }

  m_nr_matches = nr_outliers;
}

void db_FrameToReferenceRegistration::ComputeCostHistogram()
{
  ComputeCostArray();

  for ( int b = 0; b < m_nr_bins; ++b )
    m_cost_histogram[b] = 0;

  for(int c = 0; c < m_nr_matches; c++)
    {
      double error = db_SafeSqrt(m_sq_cost[c]);
      int bin = (int)(error/m_max_cost_pix*m_nr_bins);
      if ( bin < m_nr_bins )
    m_cost_histogram[bin]++;
      else
    m_cost_histogram[m_nr_bins-1]++;
    }

/*
  for ( int i = 0; i < m_nr_bins; ++i )
    std::cout << m_cost_histogram[i] << " ";
  std::cout << std::endl;
*/
}

void db_FrameToReferenceRegistration::SetOutlierThreshold()
{
  ComputeCostHistogram();

  int i = 0, last=0;
  for (; i < m_nr_bins-1; ++i )
    {
      if ( last > m_cost_histogram[i] )
    break;
      last = m_cost_histogram[i];
    }

  //std::cout << "I " <<  i << std::endl;

  int max = m_cost_histogram[i];

  for (; i < m_nr_bins-1; ++i )
    {
      if ( m_cost_histogram[i] < (int)(0.1*max) )
    //if ( last < m_cost_histogram[i] )
    break;
      last = m_cost_histogram[i];
    }
  //std::cout << "J " <<  i << std::endl;

  m_outlier_t2 = db_sqr(i*m_max_cost_pix/m_nr_bins);

  //std::cout << "m_outlier_t2 " <<  m_outlier_t2 << std::endl;
}

void db_FrameToReferenceRegistration::SmoothMotion(void)
{
  VP_MOTION inmot,outmot;

  double H[9];

  Get_H_dref_to_ins(H);

      MXX(inmot) = H[0];
    MXY(inmot) = H[1];
    MXZ(inmot) = H[2];
    MXW(inmot) = 0.0;

    MYX(inmot) = H[3];
    MYY(inmot) = H[4];
    MYZ(inmot) = H[5];
    MYW(inmot) = 0.0;

    MZX(inmot) = H[6];
    MZY(inmot) = H[7];
    MZZ(inmot) = H[8];
    MZW(inmot) = 0.0;

    MWX(inmot) = 0.0;
    MWY(inmot) = 0.0;
    MWZ(inmot) = 0.0;
    MWW(inmot) = 1.0;

    inmot.type = VP_MOTION_AFFINE;

    int w = m_im_width;
    int h = m_im_height;

    if(m_quarter_resolution)
    {
    w = w*2;
    h = h*2;
    }

#if 0
    m_stab_smoother.smoothMotionAdaptive(w,h,&inmot,&outmot);
#else
    m_stab_smoother.smoothMotion(&inmot,&outmot);
#endif

    H[0] = MXX(outmot);
    H[1] = MXY(outmot);
    H[2] = MXZ(outmot);

    H[3] = MYX(outmot);
    H[4] = MYY(outmot);
    H[5] = MYZ(outmot);

    H[6] = MZX(outmot);
    H[7] = MZY(outmot);
    H[8] = MZZ(outmot);

    Set_H_dref_to_ins(H);
}

void db_FrameToReferenceRegistration::GenerateQuarterResImage(const unsigned char* const* im)
{
  int input_h = m_im_height*2;
  int input_w = m_im_width*2;

  for (int j = 0; j < input_h; j++)
  {
    const unsigned char* in_row_ptr = im[j];
    unsigned char* out_row_ptr = m_horz_smooth_subsample_image[j]+1;

    for (int i = 2; i < input_w-2; i += 2)
    {
      int smooth_val = (
            6*in_row_ptr[i] +
            ((in_row_ptr[i-1]+in_row_ptr[i+1])<<2) +
            in_row_ptr[i-2]+in_row_ptr[i+2]
            ) >> 4;
      *out_row_ptr++ = (unsigned char) smooth_val;

      if ( (smooth_val < 0) || (smooth_val > 255))
      {
        return;
      }

    }
  }

  for (int j = 2; j < input_h-2; j+=2)
  {

    unsigned char* in_row_ptr = m_horz_smooth_subsample_image[j];
    unsigned char* out_row_ptr = m_quarter_res_image[j/2];

    for (int i = 1; i < m_im_width-1; i++)
    {
      int smooth_val = (
            6*in_row_ptr[i] +
            ((in_row_ptr[i-m_im_width]+in_row_ptr[i+m_im_width]) << 2)+
            in_row_ptr[i-2*m_im_width]+in_row_ptr[i+2*m_im_width]
            ) >> 4;
      *out_row_ptr++ = (unsigned char)smooth_val;

      if ( (smooth_val < 0) || (smooth_val > 255))
      {
        return;
      }

    }
  }
}