/*
 * File: MLS_precompute.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_precompute.h"
#include "MLS_emxutil.h"
#include "repmat.h"
#include "sum.h"

/* Function Declarations */
static double rt_powd_snf(double u0, double u1);

/* Function Definitions */

/*
 * Arguments    : double u0
 *                double u1
 * Return Type  : double
 */
static double rt_powd_snf(double u0, double u1)
{
  double y;
  double d0;
  double d1;
  if (rtIsNaN(u0) || rtIsNaN(u1)) {
    y = rtNaN;
  } else {
    d0 = fabs(u0);
    d1 = fabs(u1);
    if (rtIsInf(u1)) {
      if (d0 == 1.0) {
        y = 1.0;
      } else if (d0 > 1.0) {
        if (u1 > 0.0) {
          y = rtInf;
        } else {
          y = 0.0;
        }
      } else if (u1 > 0.0) {
        y = 0.0;
      } else {
        y = rtInf;
      }
    } else if (d1 == 0.0) {
      y = 1.0;
    } else if (d1 == 1.0) {
      if (u1 > 0.0) {
        y = u0;
      } else {
        y = 1.0 / u0;
      }
    } else if (u1 == 2.0) {
      y = u0 * u0;
    } else if ((u1 == 0.5) && (u0 >= 0.0)) {
      y = sqrt(u0);
    } else if ((u0 < 0.0) && (u1 > floor(u1))) {
      y = rtNaN;
    } else {
      y = pow(u0, u1);
    }
  }

  return y;
}

/*
 * PRECOMPUTATION
 * Arguments    : const emxArray_real_T *p_i
 *                const emxArray_real_T *V
 *                double alpha
 *                emxArray_real_T *pre_w_i
 *                emxArray_real_T *pre_sum_w_i
 *                emxArray_real_T *pre_p_ast
 *                emxArray_real_T *pre_Similarity
 * Return Type  : void
 */
void MLS_precompute(const emxArray_real_T *p_i, const emxArray_real_T *V, double
                    alpha, emxArray_real_T *pre_w_i, emxArray_real_T
                    *pre_sum_w_i, emxArray_real_T *pre_p_ast, emxArray_real_T
                    *pre_Similarity)
{
  int i0;
  int nx;
  int eval;
  emxArray_real_T *w_i;
  emxArray_real_T *p_hat_i;
  emxArray_real_T *r0;
  emxArray_real_T *a;
  emxArray_real_T *y;
  double sz[2];
  unsigned int a_idx_0;
  int vstride;
  double sum_w_i;
  unsigned int y_idx_0;
  int i1;
  double mu_s;
  double b_w_i;
  double vec[2];
  double Matrix_T[4];
  double b_vec[2];
  int iv0[2];
  double c_w_i[4];
  double A_i_temp[4];

  /*  common for all  MLS tranformation methods */
  /*  for each eval point v all control points need to be looped */
  /*  new variables */
  i0 = pre_w_i->size[0] * pre_w_i->size[1];
  pre_w_i->size[0] = p_i->size[0];
  pre_w_i->size[1] = V->size[0];
  emxEnsureCapacity_real_T(pre_w_i, i0);
  nx = p_i->size[0] * V->size[0];
  for (i0 = 0; i0 < nx; i0++) {
    pre_w_i->data[i0] = 0.0;
  }

  i0 = pre_sum_w_i->size[0];
  pre_sum_w_i->size[0] = V->size[0];
  emxEnsureCapacity_real_T1(pre_sum_w_i, i0);
  nx = V->size[0];
  for (i0 = 0; i0 < nx; i0++) {
    pre_sum_w_i->data[i0] = 0.0;
  }

  i0 = pre_p_ast->size[0] * pre_p_ast->size[1];
  pre_p_ast->size[0] = V->size[0];
  pre_p_ast->size[1] = 2;
  emxEnsureCapacity_real_T(pre_p_ast, i0);
  nx = V->size[0] << 1;
  for (i0 = 0; i0 < nx; i0++) {
    pre_p_ast->data[i0] = 0.0;
  }

  i0 = pre_Similarity->size[0] * pre_Similarity->size[1];
  pre_Similarity->size[0] = (int)(2.0 * (double)p_i->size[0]);
  pre_Similarity->size[1] = (int)(2.0 * (double)V->size[0]);
  emxEnsureCapacity_real_T(pre_Similarity, i0);
  nx = (int)(2.0 * (double)p_i->size[0]) * (int)(2.0 * (double)V->size[0]);
  for (i0 = 0; i0 < nx; i0++) {
    pre_Similarity->data[i0] = 0.0;
  }

  /* pre.Rigid = zeros(2*size(p_i,1),2*size(V,1)); */
  /* pre = cell(size_of_img); */
  eval = 0;
  emxInit_real_T(&w_i, 1);
  emxInit_real_T1(&p_hat_i, 2);
  emxInit_real_T1(&r0, 2);
  emxInit_real_T1(&a, 2);
  emxInit_real_T(&y, 1);
  while (eval <= V->size[0] - 1) {
    /*  weights w_i = 1/length_of_vect(p_i-v)^(2alpha) */
    for (i0 = 0; i0 < 2; i0++) {
      sz[i0] = V->data[eval + V->size[0] * i0];
    }

    repmat(sz, p_i->size[0], a);
    i0 = a->size[0] * a->size[1];
    a->size[0] = p_i->size[0];
    a->size[1] = 2;
    emxEnsureCapacity_real_T(a, i0);
    nx = p_i->size[0] * p_i->size[1];
    for (i0 = 0; i0 < nx; i0++) {
      a->data[i0] = p_i->data[i0] - a->data[i0];
    }

    i0 = p_hat_i->size[0] * p_hat_i->size[1];
    p_hat_i->size[0] = a->size[0];
    p_hat_i->size[1] = 2;
    emxEnsureCapacity_real_T(p_hat_i, i0);
    a_idx_0 = (unsigned int)a->size[0];
    nx = (int)a_idx_0 << 1;
    for (vstride = 0; vstride + 1 <= nx; vstride++) {
      p_hat_i->data[vstride] = a->data[vstride] * a->data[vstride];
    }

    if (p_hat_i->size[0] == 0) {
      i0 = y->size[0];
      y->size[0] = 0;
      emxEnsureCapacity_real_T1(y, i0);
    } else {
      vstride = p_hat_i->size[0];
      i0 = y->size[0];
      y->size[0] = p_hat_i->size[0];
      emxEnsureCapacity_real_T1(y, i0);
      for (nx = 0; nx + 1 <= vstride; nx++) {
        y->data[nx] = p_hat_i->data[nx];
      }

      for (nx = 0; nx + 1 <= vstride; nx++) {
        y->data[nx] += p_hat_i->data[vstride + nx];
      }
    }

    nx = y->size[0];
    for (vstride = 0; vstride + 1 <= nx; vstride++) {
      y->data[vstride] = sqrt(y->data[vstride]);
    }

    sum_w_i = 2.0 * alpha;
    a_idx_0 = (unsigned int)y->size[0];
    y_idx_0 = (unsigned int)y->size[0];
    i0 = w_i->size[0];
    w_i->size[0] = (int)y_idx_0;
    emxEnsureCapacity_real_T1(w_i, i0);
    for (vstride = 0; vstride + 1 <= (int)a_idx_0; vstride++) {
      w_i->data[vstride] = rt_powd_snf(y->data[vstride], sum_w_i);
    }

    i0 = w_i->size[0];
    w_i->size[0] = p_i->size[0];
    emxEnsureCapacity_real_T1(w_i, i0);
    nx = p_i->size[0];
    for (i0 = 0; i0 < nx; i0++) {
      w_i->data[i0] = 1.0 / w_i->data[i0];
    }

    /*  p? and q? are weighted centroids. */
    sum_w_i = sum(w_i);
    i0 = p_hat_i->size[0] * p_hat_i->size[1];
    p_hat_i->size[0] = w_i->size[0];
    p_hat_i->size[1] = 2;
    emxEnsureCapacity_real_T(p_hat_i, i0);
    nx = w_i->size[0];
    for (i0 = 0; i0 < nx; i0++) {
      p_hat_i->data[i0] = w_i->data[i0];
    }

    nx = w_i->size[0];
    for (i0 = 0; i0 < nx; i0++) {
      p_hat_i->data[i0 + p_hat_i->size[0]] = w_i->data[i0];
    }

    i0 = a->size[0] * a->size[1];
    a->size[0] = p_hat_i->size[0];
    a->size[1] = 2;
    emxEnsureCapacity_real_T(a, i0);
    for (i0 = 0; i0 < 2; i0++) {
      nx = p_hat_i->size[0];
      for (i1 = 0; i1 < nx; i1++) {
        a->data[i1 + a->size[0] * i0] = p_hat_i->data[i1 + p_hat_i->size[0] * i0]
          * p_i->data[i1 + p_i->size[0] * i0];
      }
    }

    b_sum(a, sz);
    for (i0 = 0; i0 < 2; i0++) {
      sz[i0] /= sum_w_i;
    }

    /*  p^_i for substitution */
    repmat(sz, p_i->size[0], p_hat_i);
    i0 = p_hat_i->size[0] * p_hat_i->size[1];
    p_hat_i->size[0] = p_i->size[0];
    p_hat_i->size[1] = 2;
    emxEnsureCapacity_real_T(p_hat_i, i0);
    nx = p_i->size[0] * p_i->size[1];
    for (i0 = 0; i0 < nx; i0++) {
      p_hat_i->data[i0] = p_i->data[i0] - p_hat_i->data[i0];
    }

    nx = w_i->size[0];
    for (i0 = 0; i0 < nx; i0++) {
      pre_w_i->data[i0 + pre_w_i->size[0] * eval] = w_i->data[i0];
    }

    pre_sum_w_i->data[eval] = sum_w_i;
    for (i0 = 0; i0 < 2; i0++) {
      pre_p_ast->data[eval + pre_p_ast->size[0] * i0] = sz[i0];
    }

    /*  Similarity transform precompute */
    /*  Aj - scalar for each control point p at given evaluation point v */
    /*  -> for each v vector (Aj_1;Aj_2;Aj_3;...) */
    /* function Affine = precompute_Affine(p_hat_i,p_ast,w_i,v) */
    /*  Affine transform precompute */
    /*  Single scalar for each control point p -> vector */
    /*  ([2,2]-[1,1])*([1;1]*1*[1,1])*[1;1] */
    /* s = zeros(2); */
    /* for i = 1:length(w_i) */
    /*    s = s + (p_hat_i(i,:))' * w_i(i) * p_hat_i(i,:); */
    /*  end */
    /*  inv_sum = 1/(s(1)*s(4)-s(3)*s(2)) * [s(4) -s(3); -s(2) s(1)]; */
    /*  inv_sum = -inv(s); %added -  */
    /*  Affine = zeros(length(w_i),1); */
    /*  for j = 1:length(w_i) */
    /*      Affine(j) = (v-p_ast)*inv_sum*(p_hat_i(j,:)'); */
    /*  end */
    /*  end */
    /*  Ai - matrix for each control point p at given evaluation point v */
    /*  mu_s scalar for each evaluation point v */
    mu_s = 0.0;
    for (nx = 0; nx < w_i->size[0]; nx++) {
      b_w_i = w_i->data[nx];
      sum_w_i = 0.0;
      for (i0 = 0; i0 < 2; i0++) {
        sum_w_i += b_w_i * p_hat_i->data[nx + p_hat_i->size[0] * i0] *
          p_hat_i->data[nx + p_hat_i->size[0] * i0];
      }

      mu_s += sum_w_i;
    }

    for (i0 = 0; i0 < 2; i0++) {
      vec[i0] = V->data[eval + V->size[0] * i0] - sz[i0];
    }

    /*  support function for precompute_SimilarityAndRigid */
    /*  (x,y) -> (-y,x) */
    sum_w_i = vec[0];
    vec[0] = -vec[1];
    vec[1] = sum_w_i;
    for (i0 = 0; i0 < 2; i0++) {
      Matrix_T[i0] = V->data[eval + V->size[0] * i0] - sz[i0];
    }

    for (i0 = 0; i0 < 2; i0++) {
      Matrix_T[2 + i0] = -vec[i0];
    }

    /* Similarity = zeros(2,2,length(w_i)); */
    /* Rigid = zeros(2,2,length(w_i)); */
    i0 = r0->size[0] * r0->size[1];
    r0->size[0] = (int)(2.0 * (double)w_i->size[0]);
    r0->size[1] = 2;
    emxEnsureCapacity_real_T(r0, i0);
    nx = (int)(2.0 * (double)w_i->size[0]) << 1;
    for (i0 = 0; i0 < nx; i0++) {
      r0->data[i0] = 0.0;
    }

    for (nx = 0; nx < w_i->size[0]; nx++) {
      /*  support function for precompute_SimilarityAndRigid */
      /*  (x,y) -> (-y,x) */
      b_vec[0] = -p_hat_i->data[nx + p_hat_i->size[0]];
      b_vec[1] = p_hat_i->data[nx];
      b_w_i = w_i->data[nx];
      for (i0 = 0; i0 < 2; i0++) {
        c_w_i[i0 << 1] = b_w_i * p_hat_i->data[nx + p_hat_i->size[0] * i0];
      }

      for (i0 = 0; i0 < 2; i0++) {
        c_w_i[1 + (i0 << 1)] = b_w_i * -b_vec[i0];
      }

      for (i0 = 0; i0 < 2; i0++) {
        for (i1 = 0; i1 < 2; i1++) {
          A_i_temp[i0 + (i1 << 1)] = 0.0;
          for (vstride = 0; vstride < 2; vstride++) {
            A_i_temp[i0 + (i1 << 1)] += c_w_i[i0 + (vstride << 1)] *
              Matrix_T[vstride + (i1 << 1)];
          }
        }
      }

      sum_w_i = 2.0 * (1.0 + (double)nx);
      for (i0 = 0; i0 < 2; i0++) {
        for (i1 = 0; i1 < 2; i1++) {
          r0->data[((int)(sum_w_i + (-1.0 + (double)i1)) + r0->size[0] * i0) - 1]
            = A_i_temp[i1 + (i0 << 1)] / mu_s;
        }
      }
    }

    sum_w_i = 2.0 * (1.0 + (double)eval);
    for (i0 = 0; i0 < 2; i0++) {
      iv0[i0] = (int)(sum_w_i + (-1.0 + (double)i0)) - 1;
    }

    for (i0 = 0; i0 < 2; i0++) {
      nx = r0->size[0];
      for (i1 = 0; i1 < nx; i1++) {
        pre_Similarity->data[i1 + pre_Similarity->size[0] * iv0[i0]] = r0->
          data[i1 + r0->size[0] * i0];
      }
    }

    eval++;
  }

  emxFree_real_T(&y);
  emxFree_real_T(&a);
  emxFree_real_T(&r0);
  emxFree_real_T(&p_hat_i);
  emxFree_real_T(&w_i);
}

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