/*
 * File: MLS_applyDeform.c
 *
 * MATLAB Coder version            : 3.4
 * C/C++ source code generated on  : 18-Feb-2019 15:40:18
 */

/* Include Files */
#include "rt_nonfinite.h"
#include "MLS_applyDeform.h"

/* Function Declarations */
static double rt_roundd_snf(double u);

/* Function Definitions */

/*
 * Arguments    : double u
 * Return Type  : double
 */
static double rt_roundd_snf(double u)
{
  double y;
  if (fabs(u) < 4.503599627370496E+15) {
    if (u >= 0.5) {
      y = floor(u + 0.5);
    } else if (u > -0.5) {
      y = u * 0.0;
    } else {
      y = ceil(u - 0.5);
    }
  } else {
    y = u;
  }

  return y;
}

/*
 * UNTITLED2 Summary of this function goes here
 *    f(v) - deformation vector
 *    v - point of evaluation
 *    grid_size - loop info for v points
 * Arguments    : const unsigned char oldImg[921600]
 *                double f[614400]
 *                const double v[308]
 *                const double grid_size[2]
 *                unsigned char newImg[921600]
 * Return Type  : void
 */
void MLS_applyDeform(const unsigned char oldImg[921600], double f[614400], const
                     double v[308], const double grid_size[2], unsigned char
                     newImg[921600])
{
  double stepX;
  double stepY;
  int i0;
  int i;
  double b_i;
  int i1;
  int j;
  double b_j;
  double ni;
  double nj;
  double x2;
  double y2;
  int x;
  int y;
  double vx;
  double vy;
  double fy;
  double fx;
  double ny[2];
  int c_i;
  int c_j;
  int i2;
  double fpx;
  double x22;
  double fpy;
  double y22;
  double x11;
  double y11;
  unsigned char u0;
  unsigned char u1;
  unsigned char u2;
  (void)v;
  memcpy(&newImg[0], &oldImg[0], 921600U * sizeof(unsigned char));

  /*  iterate over evaluated points  */
  /*  +--------> x,i=width */
  /*  | */
  /*  | */
  /*  | */
  /*  V y,j=height */
  stepX = grid_size[1];
  stepY = grid_size[0];

  /*  for i = 1:stepX:imw+stepX */
  /*       */
  /*      if i >= imw; i = imw; end    */
  /*       */
  /*      for j = 1:stepY:imh+stepY  */
  /*          if j == 1; continue;end % skip the [1,1] point (already created) */
  /*          if j >= imh; j = imh; end % hit the end row of column */
  /*          v = [v;[j,i]] ;           % append the eval point */
  /*          if j == imh; break;end    % next colimn */
  /*      end */
  /*       */
  /*      if i == imw; break;end        % end after last columm  */
  /*  end */
  /* %% */
  i0 = (int)((640.0 + (grid_size[1] - 1.0)) / grid_size[1]);
  for (i = 0; i < i0; i++) {
    b_i = 1.0 + (double)i * stepX;
    i1 = (int)((480.0 + (stepY - 1.0)) / stepY);
    for (j = 0; j < i1; j++) {
      b_j = 1.0 + (double)j * stepY;

      /*  next i,j to create square for bilinear interp */
      ni = b_i + stepX;
      nj = b_j + stepY;

      /*  length of square sides */
      x2 = stepX;
      y2 = stepY;

      /*  check image borders */
      if (ni > 640.0) {
        ni = 640.0;
        x2 = 640.0 - b_i;
      }

      if (nj > 480.0) {
        nj = 480.0;
        y2 = 480.0 - b_j;
      }

      /*      A */
      /*      |            x2,y2 */
      /*  f01 |-----------+  f11 */
      /*      |           | */
      /*      |           | */
      /*      |           | */
      /*      |           | */
      /*  f00 +----------------> */
      /*    x1,y1 = 0      f10 */
      for (x = 0; x < (int)((x2 - 1.0) + 1.0); x++) {
        for (y = 0; y < (int)((y2 - 1.0) + 1.0); y++) {
          /*  fill deformation function for area between control points */
          /*  bilinear_interpol(x,y,f00,f10,f01,f11),      f[y,x,[y,x]] */
          vx = (double)x / x2;
          vy = (double)y / y2;

          /*  bilinear interpolation for 1*1 square, if you pass x=x/x2 and y=y/y2 you */
          /*  get bilinear interpolation for rectangle in begining of cordinate system */
          fy = (f[((int)b_j + 480 * ((int)b_i - 1)) - 1] * (1.0 - vx) + f[((int)
                 b_j + 480 * ((int)ni - 1)) - 1] * vx) * (1.0 - vy) + (f[((int)
            nj + 480 * ((int)b_i - 1)) - 1] * (1.0 - vx) + f[((int)nj + 480 *
            ((int)ni - 1)) - 1] * vx) * vy;
          vx = (double)x / x2;
          vy = (double)y / y2;

          /*  bilinear interpolation for 1*1 square, if you pass x=x/x2 and y=y/y2 you */
          /*  get bilinear interpolation for rectangle in begining of cordinate system */
          fx = (f[((int)b_j + 480 * ((int)b_i - 1)) + 307199] * (1.0 - vx) + f
                [((int)b_j + 480 * ((int)ni - 1)) + 307199] * vx) * (1.0 - vy) +
            (f[((int)nj + 480 * ((int)b_i - 1)) + 307199] * (1.0 - vx) + f[((int)
              nj + 480 * ((int)ni - 1)) + 307199] * vx) * vy;

          /*  get new coordinates for this point */
          /* newx = i + x + fx; */
          /* newy = j + y + fy; */
          /*  check borders */
          if (fx > 640.0) {
            fx = 640.0;
          }

          if (fx < 1.0) {
            fx = 1.0;
          }

          if (fy > 480.0) {
            fy = 480.0;
          }

          if (fy < 1.0) {
            fy = 1.0;
          }

          ny[0] = fy;
          ny[1] = fx;
          c_i = (int)(b_i + (double)x);
          c_j = (int)(b_j + (double)y);
          for (i2 = 0; i2 < 2; i2++) {
            f[((c_j + 480 * (c_i - 1)) + 307200 * i2) - 1] = ny[i2];
          }

          /*  do interpolation with new and old pixel */
          /*  vector from interpolated p to q */
          vx = fx - (b_i + (double)x);
          vy = fy - (b_j + (double)y);

          /*  float part of this vector and x2,y2 for interpolation */
          if (vx > 0.0) {
            fpx = fabs(vx - floor(vx));
            x22 = (b_i + (double)x) + 1.0;
          } else if (vx < 0.0) {
            fpx = fabs(vx - ceil(vx));
            x22 = (b_i + (double)x) - 1.0;
          } else {
            fpx = 0.0;
            x22 = b_i + (double)x;
          }

          if (vy > 0.0) {
            fpy = fabs(vy - floor(vy));
            y22 = (b_j + (double)y) + 1.0;
          } else if (vy < 0.0) {
            fpy = fabs(vy - ceil(vy));
            y22 = (b_j + (double)y) - 1.0;
          } else {
            fpy = 0.0;
            y22 = b_j + (double)y;
          }

          x11 = b_i + (double)x;
          y11 = b_j + (double)y;
          fx = floor(fx);
          vx = floor(fy);
          for (c_i = 0; c_i < 3; c_i++) {
            /*  interpolate the pixel at x+x2+float,y+y2+float and */
            /*  move it to nx,ny */
            /*  bilinear interpolation for 1*1 square, if you pass x=x/x2 and y=y/y2 you */
            /*  get bilinear interpolation for rectangle in begining of cordinate system */
            vy = rt_roundd_snf((double)oldImg[(((int)y11 + 480 * ((int)x11 - 1))
              + 307200 * c_i) - 1] * (1.0 - fpx));
            if (vy < 256.0) {
              if (vy >= 0.0) {
                u0 = (unsigned char)vy;
              } else {
                u0 = 0;
              }
            } else {
              u0 = 0;
            }

            vy = rt_roundd_snf((double)oldImg[(((int)y11 + 480 * ((int)x22 - 1))
              + 307200 * c_i) - 1] * fpx);
            if (vy < 256.0) {
              u1 = (unsigned char)vy;
            } else if (vy >= 256.0) {
              u1 = MAX_uint8_T;
            } else {
              u1 = 0;
            }

            i2 = (int)((unsigned int)u0 + u1);
            if ((unsigned int)i2 > 255U) {
              i2 = 255;
            }

            vy = rt_roundd_snf((double)i2 * (1.0 - fpy));
            if (vy < 256.0) {
              if (vy >= 0.0) {
                u0 = (unsigned char)vy;
              } else {
                u0 = 0;
              }
            } else {
              u0 = 0;
            }

            vy = rt_roundd_snf((double)oldImg[(((int)y22 + 480 * ((int)x11 - 1))
              + 307200 * c_i) - 1] * (1.0 - fpx));
            if (vy < 256.0) {
              if (vy >= 0.0) {
                u1 = (unsigned char)vy;
              } else {
                u1 = 0;
              }
            } else {
              u1 = 0;
            }

            vy = rt_roundd_snf((double)oldImg[(((int)y22 + 480 * ((int)x22 - 1))
              + 307200 * c_i) - 1] * fpx);
            if (vy < 256.0) {
              u2 = (unsigned char)vy;
            } else if (vy >= 256.0) {
              u2 = MAX_uint8_T;
            } else {
              u2 = 0;
            }

            i2 = (int)((unsigned int)u1 + u2);
            if ((unsigned int)i2 > 255U) {
              i2 = 255;
            }

            vy = rt_roundd_snf((double)i2 * fpy);
            if (vy < 256.0) {
              u1 = (unsigned char)vy;
            } else if (vy >= 256.0) {
              u1 = MAX_uint8_T;
            } else {
              u1 = 0;
            }

            i2 = (int)((unsigned int)u0 + u1);
            if ((unsigned int)i2 > 255U) {
              i2 = 255;
            }

            newImg[(((int)vx + 480 * ((int)fx - 1)) + 307200 * c_i) - 1] =
              (unsigned char)i2;
          }
        }
      }
    }
  }
}

/*
 * File trailer for MLS_applyDeform.c
 *
 * [EOF]
 */
