/*
 * File: MLS.c
 *
 * MATLAB Coder version            : 3.4
 * C/C++ source code generated on  : 27-Feb-2019 07:49:44
 */

/* Include Files */
#include "rt_nonfinite.h"
#include "MLS.h"
#include "MLS_emxutil.h"
#include "repmat.h"
#include "sum.h"
#include "MLS_precompute.h"
#include <stdio.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 hereall(size(a) == [5,2])
 *    oldImg = img to deform
 *    type = 'Affine', 'Similarity', 'Rigid'
 *    alpha = MLS parameter - pass 1
 *    p = control point position
 *    q = control point deformed position
 *    PRECOMPUTED loopback
 *    v = evaluation points (grid) - pass [stepX,stepY] if you want to
 *        precompute or pass output of MLS_getPoints
 *    imgSize = will get overwritten if you pass v as [stepX,stepY] or pass output of MLS_getPoints
 *    grid_size
 *    pre - output of MLS_precompute
 * Arguments    : const unsigned char oldImg[921600]
 *                const emxArray_char_T *type
 *                double alpha
 *                const emxArray_real_T *p
 *                const emxArray_real_T *q
 *                emxArray_real_T *v
 *                double imgSize[2]
 *                double grid_size[2]
 *                struct0_T *pre
 *                unsigned char newImg[921600]
 * Return Type  : void
 */
void MLS(const unsigned char oldImg[921600], const emxArray_char_T *type, double
         alpha, const emxArray_real_T *p, const emxArray_real_T *q,
         emxArray_real_T *v, double imgSize[2], double grid_size[2], struct0_T
         *pre, unsigned char newImg[921600])
{
  int i2;
  boolean_T y;
  boolean_T x[2];
  int k;
  boolean_T exitg1;
  emxArray_real_T *b_v;
  emxArray_real_T *f;
  double stepX;
  double stepY;
  int i;
  double b_i;
  int exitg3;
  int i3;
  int j;
  static const char cv0[6] = { 'A', 'f', 'f', 'i', 'n', 'e' };

  boolean_T exitg2;
  double b_j;
  static const char cv1[10] = { 'S', 'i', 'm', 'i', 'l', 'a', 'r', 'i', 't', 'y'
  };

  static const char cv2[5] = { 'R', 'i', 'g', 'i', 'd' };

  int i4;
  int c_v;
  emxArray_real_T *b_f;
  double dv0[3];
  double ni;
  double nj;
  double x2;
  double y2;
  emxArray_real_T *w_i;
  emxArray_real_T *Ai_mu;
  emxArray_real_T *b_w_i;
  int b_x;
  double vx;
  int b_y;
  double vy;
  double fy;
  double fx;
  double q_ast[2];
  double fpx;
  double x22;
  double fpy;
  double y22;
  double x11;
  double y11;
  unsigned char u0;
  unsigned char u1;
  double sum_f[2];
  double b[4];
  unsigned char u2;

  /*  crete evaluation grid if required */
  for (i2 = 0; i2 < 2; i2++) {
    x[i2] = (v->size[i2] == 1 + i2);
  }

  y = true;
  k = 0;
  exitg1 = false;
  while ((!exitg1) && (k < 2)) {
    if (!x[k]) {
      y = false;
      exitg1 = true;
    } else {
      k++;
    }
  }

  emxInit_real_T1(&b_v, 2);
  if (y) {
    /*  [stepX,stepY] */
    stepX = v->data[0];
    stepY = v->data[1];

    /* UNTITLED5 Summary of this function goes here */
    /*    Detailed explanation goes here */
    /*  inconsistent coordinate system - world standard seems to be: */
    /*  +--------> x,i=width */
    /*  | */
    /*  | */
    /*  | */
    /*  V y,j=height */
    i2 = v->size[0] * v->size[1];
    v->size[0] = 0;
    v->size[1] = 2;
    emxEnsureCapacity_real_T(v, i2);
    i2 = (int)(((640.0 + stepX) + (stepX - 1.0)) / stepX);
    i = 0;
    exitg1 = false;
    while ((!exitg1) && (i <= i2 - 1)) {
      b_i = 1.0 + (double)i * stepX;
      if (b_i >= 640.0) {
        b_i = 640.0;
      }

      i3 = (int)(((480.0 + stepY) + (stepY - 1.0)) / stepY);
      j = 0;
      exitg2 = false;
      while ((!exitg2) && (j <= i3 - 1)) {
        b_j = 1.0 + (double)j * stepY;

        /*          if (j == 1) && (i==1); continue;end % skip the [1,1] point (already created) */
        if (b_j >= 480.0) {
          b_j = 480.0;
        }

        /*  hit the end row of column */
        i4 = b_v->size[0] * b_v->size[1];
        b_v->size[0] = v->size[0] + 1;
        b_v->size[1] = 2;
        emxEnsureCapacity_real_T(b_v, i4);
        for (i4 = 0; i4 < 2; i4++) {
          k = v->size[0];
          for (c_v = 0; c_v < k; c_v++) {
            b_v->data[c_v + b_v->size[0] * i4] = v->data[c_v + v->size[0] * i4];
          }
        }

        b_v->data[v->size[0]] = b_j;
        b_v->data[v->size[0] + b_v->size[0]] = b_i;
        i4 = v->size[0] * v->size[1];
        v->size[0] = b_v->size[0];
        v->size[1] = 2;
        emxEnsureCapacity_real_T(v, i4);
        for (i4 = 0; i4 < 2; i4++) {
          k = b_v->size[0];
          for (c_v = 0; c_v < k; c_v++) {
            v->data[c_v + v->size[0] * i4] = b_v->data[c_v + b_v->size[0] * i4];
          }
        }

        /*  append the eval point */
        if (b_j == 480.0) {
          exitg2 = true;
        } else {
          /*  next colimn */
          j++;
        }
      }

      if (b_i == 640.0) {
        exitg1 = true;
      } else {
        /*  end after last columm  */
        i++;
      }
    }

    grid_size[0] = stepY;
    grid_size[1] = stepX;
    for (i2 = 0; i2 < 2; i2++) {
      imgSize[i2] = 480.0 + 160.0 * (double)i2;
    }
  }

  emxInit_real_T2(&f, 3);

  /*  precompute if required */
  MLS_precompute(p, v, alpha, pre->w_i, pre->sum_w_i, pre->p_ast,
                 pre->Similarity);

  /*  MLS deformation */
  /* UNTITLED2 Summary of this function goes here */
  /*    type: */
  /*         Affine = translation, rotation, nonuniform scaling and shear */
  /*         Similarity = translation, rotation and uniform scaling */
  /*         Rigid = translation, rotation */
  /*    alpha: for weigth function (precompute) */
  /*    p_i,q_i,v_i are row vectros eg. [x,y] so p,q,v = [x1,y1;x2,y2;x3,y3;...] */
  /*    size_of_img = [height,width] */
  i2 = f->size[0] * f->size[1] * f->size[2];
  f->size[0] = 0;
  f->size[1] = 0;
  f->size[2] = 1;
  emxEnsureCapacity_real_T2(f, i2);

  /*  switch the MLS deformation type */
  y = false;
  if (type->size[1] == 6) {
    k = 0;
    do {
      exitg3 = 0;
      if (k + 1 < 7) {
        if (type->data[k] != cv0[k]) {
          exitg3 = 1;
        } else {
          k++;
        }
      } else {
        y = true;
        exitg3 = 1;
      }
    } while (exitg3 == 0);
  }

  if (!y) {
    y = false;
    if (type->size[1] == 10) {
      k = 0;
      do {
        exitg3 = 0;
        if (k + 1 < 11) {
          if (type->data[k] != cv1[k]) {
            exitg3 = 1;
          } else {
            k++;
          }
        } else {
          y = true;
          exitg3 = 1;
        }
      } while (exitg3 == 0);
    }

    if (y) {
      /*  MLS deformation */
      /*  %% rigid deformation */
      /*  function f = MLS_Rigid(q_i,V,pre,size_of_img) */
      /*  f = zeros([size_of_img,2]); */
      /*   */
      /*  % loop through evaluation points v */
      /*  for i = 1:size(V,1) */
      /*      v = V(i,:); */
      /*      % get param for given evaluation point v */
      /*      w_i = pre{v(1),v(2)}.w_i; */
      /*      sum_w_i = pre{v(1),v(2)}.sum_w_i; */
      /*      Ai= pre{v(1),v(2)}.Rigid;    */
      /*      p_ast = pre{v(1),v(2)}.p_ast; */
      /*       */
      /*      [q_ast,q_hat_i] = MLS_q(w_i,sum_w_i,q_i); */
      /*       */
      /*      % loop through control points */
      /*      f_vec=zeros(1,2); */
      /*      for j = 1:size(q_i,1) */
      /*           */
      /*          f_vec = f_vec + q_hat_i(j,:)*Ai(:,:,j); */
      /*      end */
      /*      len_f_vec = sqrt(sum(f_vec.^2)); */
      /*     */
      /*      f_r = sqrt(sum((v-p_ast).^2)) * f_vec / len_f_vec + q_ast; */
      /*       */
      /*      % save deformation for given control point */
      /*      f(v(1),v(2),[1,2]) = f_r; */
      /*  end */
      /*  end */
      /*  similatrity defromation */
      for (i2 = 0; i2 < 2; i2++) {
        dv0[i2] = imgSize[i2];
      }

      emxInit_real_T2(&b_f, 3);
      i2 = b_f->size[0] * b_f->size[1] * b_f->size[2];
      b_f->size[0] = (int)dv0[0];
      b_f->size[1] = (int)dv0[1];
      b_f->size[2] = 2;
      emxEnsureCapacity_real_T2(b_f, i2);
      k = ((int)dv0[0] * (int)dv0[1]) << 1;
      for (i2 = 0; i2 < k; i2++) {
        b_f->data[i2] = 0.0;
      }

      /*  loop through evaluation points v */
      i = 0;
      emxInit_real_T(&w_i, 1);
      emxInit_real_T1(&Ai_mu, 2);
      emxInit_real_T1(&b_w_i, 2);
      while (i <= v->size[0] - 1) {
        /*  get param for given evaluation point v */
        k = pre->w_i->size[0];
        i2 = w_i->size[0];
        w_i->size[0] = k;
        emxEnsureCapacity_real_T1(w_i, i2);
        for (i2 = 0; i2 < k; i2++) {
          w_i->data[i2] = pre->w_i->data[i2 + pre->w_i->size[0] * i];
        }

        k = pre->Similarity->size[0];
        vx = 2.0 * (1.0 + (double)i);
        i2 = Ai_mu->size[0] * Ai_mu->size[1];
        Ai_mu->size[0] = k;
        Ai_mu->size[1] = 2;
        emxEnsureCapacity_real_T(Ai_mu, i2);
        for (i2 = 0; i2 < 2; i2++) {
          for (i3 = 0; i3 < k; i3++) {
            Ai_mu->data[i3 + Ai_mu->size[0] * i2] = pre->Similarity->data[i3 +
              pre->Similarity->size[0] * ((int)(vx + (-1.0 + (double)i2)) - 1)];
          }
        }

        /*  [w_i,w_i] to make matlab coder happy */
        /*  %% affine deformation */
        /*  function f = MLS_Affine(q_i,V,pre,size_of_img) */
        /*  f = zeros([size_of_img,2]); */
        /*   */
        /*  % loop through evaluation points v */
        /*  for i = 1:size(V,1) */
        /*      v = V(i,:); */
        /*      % get param for given evaluation point v */
        /*      w_i = pre{v(1),v(2)}.w_i; */
        /*      sum_w_i = pre{v(1),v(2)}.sum_w_i; */
        /*      Aj = pre{v(1),v(2)}.Affine;     */
        /*   */
        /*      [q_ast,q_hat_i] = MLS_q(w_i,sum_w_i,q_i); */
        /*       */
        /*      % loop through control points */
        /*      sum_f=[0,0]; */
        /*      for j = 1:size(q_i,1) */
        /*          sum_f = sum_f + Aj(j)*q_hat_i(j,:); */
        /*      end */
        /*      sum_f = sum_f + q_ast; */
        /*      % save deformation for given control point */
        /*      %if isequal(v,p) */
        /*       */
        /*      f(v(1),v(2),[1,2]) = sum_f;    */
        /*  end */
        /*  end */
        /*  parameters for deformation function, q = deformed positions of the control points p */
        i2 = b_w_i->size[0] * b_w_i->size[1];
        b_w_i->size[0] = w_i->size[0];
        b_w_i->size[1] = 2;
        emxEnsureCapacity_real_T(b_w_i, i2);
        k = w_i->size[0];
        for (i2 = 0; i2 < k; i2++) {
          b_w_i->data[i2] = w_i->data[i2];
        }

        k = w_i->size[0];
        for (i2 = 0; i2 < k; i2++) {
          b_w_i->data[i2 + b_w_i->size[0]] = w_i->data[i2];
        }

        i2 = b_v->size[0] * b_v->size[1];
        b_v->size[0] = b_w_i->size[0];
        b_v->size[1] = 2;
        emxEnsureCapacity_real_T(b_v, i2);
        for (i2 = 0; i2 < 2; i2++) {
          k = b_w_i->size[0];
          for (i3 = 0; i3 < k; i3++) {
            b_v->data[i3 + b_v->size[0] * i2] = b_w_i->data[i3 + b_w_i->size[0] *
              i2] * q->data[i3 + q->size[0] * i2];
          }
        }

        b_sum(b_v, q_ast);
        vx = pre->sum_w_i->data[i];
        for (i2 = 0; i2 < 2; i2++) {
          q_ast[i2] /= vx;
        }

        repmat(q_ast, q->size[0], b_v);
        i2 = b_v->size[0] * b_v->size[1];
        b_v->size[0] = q->size[0];
        b_v->size[1] = 2;
        emxEnsureCapacity_real_T(b_v, i2);
        k = q->size[0] * q->size[1];
        for (i2 = 0; i2 < k; i2++) {
          b_v->data[i2] = q->data[i2] - b_v->data[i2];
        }

        /*  loop through control points */
        for (i2 = 0; i2 < 2; i2++) {
          sum_f[i2] = 0.0;
        }

        for (j = 0; j < q->size[0]; j++) {
          vx = 2.0 * (1.0 + (double)j);
          for (i2 = 0; i2 < 2; i2++) {
            for (i3 = 0; i3 < 2; i3++) {
              b[i3 + (i2 << 1)] = Ai_mu->data[((int)(vx + (-1.0 + (double)i3)) +
                Ai_mu->size[0] * i2) - 1];
            }
          }

          for (i2 = 0; i2 < 2; i2++) {
            vx = 0.0;
            for (i3 = 0; i3 < 2; i3++) {
              vx += b_v->data[j + b_v->size[0] * i3] * b[i3 + (i2 << 1)];
            }

            sum_f[i2] += vx;
          }
        }

        for (i2 = 0; i2 < 2; i2++) {
          sum_f[i2] += q_ast[i2];
        }

        /*  save deformation for given control point */
        k = (int)v->data[i + v->size[0]];
        c_v = (int)v->data[i];
        for (i2 = 0; i2 < 2; i2++) {
          b_f->data[((c_v + b_f->size[0] * (k - 1)) + b_f->size[0] * b_f->size[1]
                     * i2) - 1] = sum_f[i2];
        }

        i++;
      }

      emxFree_real_T(&b_w_i);
      emxFree_real_T(&Ai_mu);
      emxFree_real_T(&w_i);
      i2 = f->size[0] * f->size[1] * f->size[2];
      f->size[0] = b_f->size[0];
      f->size[1] = b_f->size[1];
      f->size[2] = 2;
      emxEnsureCapacity_real_T2(f, i2);
      k = b_f->size[0] * b_f->size[1] * b_f->size[2];
      for (i2 = 0; i2 < k; i2++) {
        f->data[i2] = b_f->data[i2];
      }

      emxFree_real_T(&b_f);
    } else {
      y = false;
      if (type->size[1] == 5) {
        k = 0;
        do {
          exitg3 = 0;
          if (k + 1 < 6) {
            if (type->data[k] != cv2[k]) {
              exitg3 = 1;
            } else {
              k++;
            }
          } else {
            y = true;
            exitg3 = 1;
          }
        } while (exitg3 == 0);
      }

      if (!y) {
        /* ME = MException('type:PassedWrongVariable', ... */
        /*     'Variable %s should be one of following: Affine, Similarity, Rigid',type); */
        /* throw(ME) */
        printf("PassedWrongVariable");
        fflush(stdout);
      } else {
        /*      f = MLS_Rigid(q,v,pre,size_of_img); */
      }
    }
  } else {
    /* f = MLS_Affine(q,v,pre,size_of_img); */
  }

  emxFree_real_T(&b_v);

  /*  Interpolate img */
  /* UNTITLED2 Summary of this function goes here */
  /*    f(v) - deformation vector */
  /*    v - point of evaluation */
  /*    grid_size - loop info for v points */
  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 */
  /* %% */
  i2 = (int)((640.0 + (grid_size[1] - 1.0)) / grid_size[1]);
  for (i = 0; i < i2; i++) {
    b_i = 1.0 + (double)i * stepX;
    i3 = (int)((480.0 + (stepY - 1.0)) / stepY);
    for (j = 0; j < i3; 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 (b_x = 0; b_x < (int)((x2 - 1.0) + 1.0); b_x++) {
        for (b_y = 0; b_y < (int)((y2 - 1.0) + 1.0); b_y++) {
          /*  fill deformation function for area between control points */
          /*  bilinear_interpol(x,y,f00,f10,f01,f11),      f[y,x,[y,x]] */
          vx = (double)b_x / x2;
          vy = (double)b_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->data[((int)b_j + f->size[0] * ((int)b_i - 1)) - 1] * (1.0 -
                 vx) + f->data[((int)b_j + f->size[0] * ((int)ni - 1)) - 1] * vx)
            * (1.0 - vy) + (f->data[((int)nj + f->size[0] * ((int)b_i - 1)) - 1]
                            * (1.0 - vx) + f->data[((int)nj + f->size[0] * ((int)
            ni - 1)) - 1] * vx) * vy;
          vx = (double)b_x / x2;
          vy = (double)b_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->data[(((int)b_j + f->size[0] * ((int)b_i - 1)) + f->size[0] *
                         f->size[1]) - 1] * (1.0 - vx) + f->data[(((int)b_j +
                  f->size[0] * ((int)ni - 1)) + f->size[0] * f->size[1]) - 1] *
                vx) * (1.0 - vy) + (f->data[(((int)nj + f->size[0] * ((int)b_i -
            1)) + f->size[0] * f->size[1]) - 1] * (1.0 - vx) + f->data[(((int)nj
            + f->size[0] * ((int)ni - 1)) + f->size[0] * f->size[1]) - 1] * 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;
          }

          q_ast[0] = fy;
          q_ast[1] = fx;
          k = (int)(b_i + (double)b_x);
          c_v = (int)(b_j + (double)b_y);
          for (i4 = 0; i4 < 2; i4++) {
            f->data[((c_v + f->size[0] * (k - 1)) + f->size[0] * f->size[1] * i4)
              - 1] = q_ast[i4];
          }

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

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

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

          x11 = b_i + (double)b_x;
          y11 = b_j + (double)b_y;
          fx = floor(fx);
          vy = floor(fy);
          for (k = 0; k < 3; k++) {
            /*  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 */
            vx = rt_roundd_snf((double)oldImg[(((int)y11 + 480 * ((int)x11 - 1))
              + 307200 * k) - 1] * (1.0 - fpx));
            if (vx < 256.0) {
              if (vx >= 0.0) {
                u0 = (unsigned char)vx;
              } else {
                u0 = 0;
              }
            } else {
              u0 = 0;
            }

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

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

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

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

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

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

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

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

            newImg[(((int)vy + 480 * ((int)fx - 1)) + 307200 * k) - 1] =
              (unsigned char)i4;
          }
        }
      }
    }
  }

  emxFree_real_T(&f);
}

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