/*
 * Copyright (c) 2016, Alliance for Open Media. All rights reserved
 *
 * This source code is subject to the terms of the BSD 2 Clause License and
 * the Alliance for Open Media Patent License 1.0. If the BSD 2 Clause License
 * was not distributed with this source code in the LICENSE file, you can
 * obtain it at www.aomedia.org/license/software. If the Alliance for Open
 * Media Patent License 1.0 was not distributed with this source code in the
 * PATENTS file, you can obtain it at www.aomedia.org/license/patent.
 */

#include <memory.h>
#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>

#include "aom_dsp/flow_estimation/ransac.h"
#include "aom_dsp/mathutils.h"
#include "aom_mem/aom_mem.h"

// TODO(rachelbarker): Remove dependence on code in av1/encoder/
#include "av1/common/mv.h"
#include "av1/encoder/random.h"

#define MAX_MINPTS 4
#define MINPTS_MULTIPLIER 5

#define INLIER_THRESHOLD 1.25
#define INLIER_THRESHOLD_SQUARED (INLIER_THRESHOLD * INLIER_THRESHOLD)

// Number of initial models to generate
#define NUM_TRIALS 20

// Number of times to refine the best model found
#define NUM_REFINES 5

// Flag to enable functions for finding TRANSLATION type models.
//
// These modes are not considered currently due to a spec bug (see comments
// in gm_get_motion_vector() in av1/common/mv.h). Thus we don't need to compile
// the corresponding search functions, but it is nice to keep the source around
// but disabled, for completeness.
#define ALLOW_TRANSLATION_MODELS 0

typedef struct {
  int num_inliers;
  double sse;  // Sum of squared errors of inliers
  int *inlier_indices;
} RANSAC_MOTION;

////////////////////////////////////////////////////////////////////////////////
// ransac
typedef bool (*FindTransformationFunc)(const Correspondence *points,
                                       const int *indices, int num_indices,
                                       double *params);
typedef bool (*FindTransformationQuantizeFunc)(const Correspondence *points,
                                               const int *indices,
                                               int num_indices,
                                               int32_t *params);
typedef void (*ScoreModelFunc)(const double *mat, const Correspondence *points,
                               int num_points, RANSAC_MOTION *model);

// vtable-like structure which stores all of the information needed by RANSAC
// for a particular model type
typedef struct {
  FindTransformationFunc find_transformation;
  FindTransformationQuantizeFunc find_transformation_quantize;
  ScoreModelFunc score_model;

  // The minimum number of points which can be passed to find_transformation
  // to generate a model.
  //
  // This should be set as small as possible. This is due to an observation
  // from section 4 of "Optimal Ransac" by A. Hast, J. Nysjö and
  // A. Marchetti (https://dspace5.zcu.cz/bitstream/11025/6869/1/Hast.pdf):
  // using the minimum possible number of points in the initial model maximizes
  // the chance that all of the selected points are inliers.
  //
  // That paper proposes a method which can deal with models which are
  // contaminated by outliers, which helps in cases where the inlier fraction
  // is low. However, for our purposes, global motion only gives significant
  // gains when the inlier fraction is high.
  //
  // So we do not use the method from this paper, but we do find that
  // minimizing the number of points used for initial model fitting helps
  // make the best use of the limited number of models we consider.
  int minpts;
} RansacModelInfo;

#if ALLOW_TRANSLATION_MODELS
static void score_translation(const double *mat, const Correspondence *points,
                              int num_points, RANSAC_MOTION *model) {
  model->num_inliers = 0;
  model->sse = 0.0;

  for (int i = 0; i < num_points; ++i) {
    const double x1 = points[i].x;
    const double y1 = points[i].y;
    const double x2 = points[i].rx;
    const double y2 = points[i].ry;

    const double proj_x = x1 + mat[0];
    const double proj_y = y1 + mat[1];

    const double dx = proj_x - x2;
    const double dy = proj_y - y2;
    const double sse = dx * dx + dy * dy;

    if (sse < INLIER_THRESHOLD_SQUARED) {
      model->inlier_indices[model->num_inliers++] = i;
      model->sse += sse;
    }
  }
}
#endif  // ALLOW_TRANSLATION_MODELS

static void score_affine(const double *mat, const Correspondence *points,
                         int num_points, RANSAC_MOTION *model) {
  model->num_inliers = 0;
  model->sse = 0.0;

  for (int i = 0; i < num_points; ++i) {
    const double x1 = points[i].x;
    const double y1 = points[i].y;
    const double x2 = points[i].rx;
    const double y2 = points[i].ry;

    const double proj_x = mat[2] * x1 + mat[3] * y1 + mat[0];
    const double proj_y = mat[4] * x1 + mat[5] * y1 + mat[1];

    const double dx = proj_x - x2;
    const double dy = proj_y - y2;
    const double sse = dx * dx + dy * dy;

    if (sse < INLIER_THRESHOLD_SQUARED) {
      model->inlier_indices[model->num_inliers++] = i;
      model->sse += sse;
    }
  }
}

#if ALLOW_TRANSLATION_MODELS
static bool find_translation(const Correspondence *points, const int *indices,
                             int num_indices, double *params) {
  double sumx = 0;
  double sumy = 0;

  for (int i = 0; i < num_indices; ++i) {
    int index = indices[i];
    const double sx = points[index].x;
    const double sy = points[index].y;
    const double dx = points[index].rx;
    const double dy = points[index].ry;

    sumx += dx - sx;
    sumy += dy - sy;
  }

  params[0] = sumx / np;
  params[1] = sumy / np;
  params[2] = 1;
  params[3] = 0;
  params[4] = 0;
  params[5] = 1;
  return true;
}

static bool find_translation_quantize(const Correspondence *points,
                                      const int *indices, int num_indices,
                                      int32_t *params) {
  double sumx = 0;
  double sumy = 0;

  for (int i = 0; i < num_indices; ++i) {
    int index = indices[i];
    const double sx = points[index].x;
    const double sy = points[index].y;
    const double dx = points[index].rx;
    const double dy = points[index].ry;

    sumx += dx - sx;
    sumy += dy - sy;
  }

  const double tx = sumx / np;
  const double ty = sumy / np;

  params[0] =
      clamp((int)rint(tx * (1 << GM_TRANS_ONLY_PREC_BITS)),
            -(1 << GM_ABS_TRANS_ONLY_BITS), (1 << GM_ABS_TRANS_ONLY_BITS)) *
      (1 << GM_TRANS_ONLY_PREC_DIFF);
  params[1] =
      clamp((int)rint(ty * (1 << GM_TRANS_ONLY_PREC_BITS)),
            -(1 << GM_ABS_TRANS_ONLY_BITS), (1 << GM_ABS_TRANS_ONLY_BITS)) *
      (1 << GM_TRANS_ONLY_PREC_DIFF);
  params[2] = 1 << WARPEDMODEL_PREC_BITS;
  params[3] = 0;
  params[4] = 0;
  params[5] = 1 << WARPEDMODEL_PREC_BITS;
  return true;
}
#endif  // ALLOW_TRANSLATION_MODELS

static bool find_rotzoom(const Correspondence *points, const int *indices,
                         int num_indices, double *params) {
  const int n = 4;    // Size of least-squares problem
  double mat[4 * 4];  // Accumulator for A'A
  double y[4];        // Accumulator for A'b
  double a[4];        // Single row of A
  double b;           // Single element of b

  least_squares_init(mat, y, n);
  for (int i = 0; i < num_indices; ++i) {
    int index = indices[i];
    const double sx = points[index].x;
    const double sy = points[index].y;
    const double dx = points[index].rx;
    const double dy = points[index].ry;

    a[0] = 1;
    a[1] = 0;
    a[2] = sx;
    a[3] = sy;
    b = dx;
    least_squares_accumulate(mat, y, a, b, n);

    a[0] = 0;
    a[1] = 1;
    a[2] = sy;
    a[3] = -sx;
    b = dy;
    least_squares_accumulate(mat, y, a, b, n);
  }

  // Fill in params[0] .. params[3] with output model
  if (!least_squares_solve(mat, mat, y, params, n)) {
    return false;
  }

  // Fill in remaining parameters
  params[4] = -params[3];
  params[5] = params[2];

  return true;
}

static bool find_rotzoom_quantize(const Correspondence *points,
                                  const int *indices, int num_indices,
                                  int32_t *params) {
  const int n = 4;    // Size of least-squares problem
  double mat[4 * 4];  // Accumulator for A'A
  double y[4];        // Accumulator for A'b
  double a[4];        // Single row of A
  double b;           // Single element of b

  least_squares_init(mat, y, n);
  for (int i = 0; i < num_indices; ++i) {
    int index = indices[i];
    const double sx = points[index].x;
    const double sy = points[index].y;
    const double dx = points[index].rx;
    const double dy = points[index].ry;

    a[0] = 1;
    a[1] = 0;
    a[2] = sx;
    a[3] = sy;
    b = dx;
    least_squares_accumulate(mat, y, a, b, n);

    a[0] = 0;
    a[1] = 1;
    a[2] = sy;
    a[3] = -sx;
    b = dy;
    least_squares_accumulate(mat, y, a, b, n);
  }

  // Fill in params[0] .. params[3] with output model
  double prec[4] = { 1 << GM_TRANS_PREC_BITS, 1 << GM_TRANS_PREC_BITS,
                     1 << GM_ALPHA_PREC_BITS, 1 << GM_ALPHA_PREC_BITS };
  int32_t min[4] = { GM_TRANS_MIN, GM_TRANS_MIN,
                     (1 << GM_ALPHA_PREC_BITS) + GM_ALPHA_MIN, GM_ALPHA_MIN };
  int32_t max[4] = { GM_TRANS_MAX, GM_TRANS_MAX,
                     (1 << GM_ALPHA_PREC_BITS) + GM_ALPHA_MAX, GM_ALPHA_MAX };
  int32_t scale[4] = { 1 << GM_TRANS_PREC_DIFF, 1 << GM_TRANS_PREC_DIFF,
                       1 << GM_ALPHA_PREC_DIFF, 1 << GM_ALPHA_PREC_DIFF };
  if (!least_squares_solve_quant(mat, mat, y, y, params, n, prec, min, max,
                                 scale)) {
    return false;
  }

  // Fill in remaining parameters
  params[4] = -params[3];
  params[5] = params[2];

  return true;
}

static bool find_affine(const Correspondence *points, const int *indices,
                        int num_indices, double *params) {
  // Note: The least squares problem for affine models is 6-dimensional,
  // but it splits into two independent 3-dimensional subproblems.
  // Solving these two subproblems separately and recombining at the end
  // results in less total computation than solving the 6-dimensional
  // problem directly.
  //
  // The two subproblems correspond to all the parameters which contribute
  // to the x output of the model, and all the parameters which contribute
  // to the y output, respectively.

  const int n = 3;       // Size of each least-squares problem
  double mat[2][3 * 3];  // Accumulator for A'A
  double y[2][3];        // Accumulator for A'b
  double x[2][3];        // Output vector
  double a[2][3];        // Single row of A
  double b[2];           // Single element of b

  least_squares_init(mat[0], y[0], n);
  least_squares_init(mat[1], y[1], n);
  for (int i = 0; i < num_indices; ++i) {
    int index = indices[i];
    const double sx = points[index].x;
    const double sy = points[index].y;
    const double dx = points[index].rx;
    const double dy = points[index].ry;

    a[0][0] = 1;
    a[0][1] = sx;
    a[0][2] = sy;
    b[0] = dx;
    least_squares_accumulate(mat[0], y[0], a[0], b[0], n);

    a[1][0] = 1;
    a[1][1] = sx;
    a[1][2] = sy;
    b[1] = dy;
    least_squares_accumulate(mat[1], y[1], a[1], b[1], n);
  }

  // Each of the two models produces 1 translational + 2 non-translational
  // params
  if (!least_squares_solve(mat[0], mat[0], y[0], x[0], n)) {
    return false;
  }
  if (!least_squares_solve(mat[1], mat[1], y[1], x[1], n)) {
    return false;
  }

  // Rearrange least squares result to form output model
  params[0] = x[0][0];
  params[1] = x[1][0];
  params[2] = x[0][1];
  params[3] = x[0][2];
  params[4] = x[1][1];
  params[5] = x[1][2];

  return true;
}

static bool find_affine_quantize(const Correspondence *points,
                                 const int *indices, int num_indices,
                                 int32_t *params) {
  // Note: The least squares problem for affine models is 6-dimensional,
  // but it splits into two independent 3-dimensional subproblems.
  // Solving these two subproblems separately and recombining at the end
  // results in less total computation than solving the 6-dimensional
  // problem directly.
  //
  // The two subproblems correspond to all the parameters which contribute
  // to the x output of the model, and all the parameters which contribute
  // to the y output, respectively.

  const int n = 3;       // Size of each least-squares problem
  double mat[2][3 * 3];  // Accumulator for A'A
  double y[2][3];        // Accumulator for A'b
  int32_t x[2][3];       // Output vector
  double a[2][3];        // Single row of A
  double b[2];           // Single element of b

  least_squares_init(mat[0], y[0], n);
  least_squares_init(mat[1], y[1], n);
  for (int i = 0; i < num_indices; ++i) {
    int index = indices[i];
    const double sx = points[index].x;
    const double sy = points[index].y;
    const double dx = points[index].rx;
    const double dy = points[index].ry;

    a[0][0] = 1;
    a[0][1] = sx;
    a[0][2] = sy;
    b[0] = dx;
    least_squares_accumulate(mat[0], y[0], a[0], b[0], n);

    a[1][0] = 1;
    a[1][1] = sx;
    a[1][2] = sy;
    b[1] = dy;
    least_squares_accumulate(mat[1], y[1], a[1], b[1], n);
  }

  // Each of the two least squares problems produces 1 translational +
  // 2 non-translational params
  double prec[3] = { 1 << GM_TRANS_PREC_BITS, 1 << GM_ALPHA_PREC_BITS,
                     1 << GM_ALPHA_PREC_BITS };
  int32_t min[2][3] = {
    { GM_TRANS_MIN, (1 << GM_ALPHA_PREC_BITS) + GM_ALPHA_MIN, GM_ALPHA_MIN },
    { GM_TRANS_MIN, GM_ALPHA_MIN, (1 << GM_ALPHA_PREC_BITS) + GM_ALPHA_MIN }
  };
  int32_t max[2][3] = {
    { GM_TRANS_MAX, (1 << GM_ALPHA_PREC_BITS) + GM_ALPHA_MAX, GM_ALPHA_MAX },
    { GM_TRANS_MAX, GM_ALPHA_MAX, (1 << GM_ALPHA_PREC_BITS) + GM_ALPHA_MAX }
  };
  int32_t scale[3] = { 1 << GM_TRANS_PREC_DIFF, 1 << GM_ALPHA_PREC_DIFF,
                       1 << GM_ALPHA_PREC_DIFF };

  if (!least_squares_solve_quant(mat[0], mat[0], y[0], y[0], x[0], n, prec,
                                 min[0], max[0], scale)) {
    return false;
  }
  if (!least_squares_solve_quant(mat[1], mat[1], y[1], y[1], x[1], n, prec,
                                 min[1], max[1], scale)) {
    return false;
  }

  // Rearrange least squares result to form output model
  params[0] = x[0][0];
  params[1] = x[1][0];
  params[2] = x[0][1];
  params[3] = x[0][2];
  params[4] = x[1][1];
  params[5] = x[1][2];

  return true;
}

// Return -1 if 'a' is a better motion, 1 if 'b' is better, 0 otherwise.
static int compare_motions(const void *arg_a, const void *arg_b) {
  const RANSAC_MOTION *motion_a = (RANSAC_MOTION *)arg_a;
  const RANSAC_MOTION *motion_b = (RANSAC_MOTION *)arg_b;

  if (motion_a->num_inliers > motion_b->num_inliers) return -1;
  if (motion_a->num_inliers < motion_b->num_inliers) return 1;
  if (motion_a->sse < motion_b->sse) return -1;
  if (motion_a->sse > motion_b->sse) return 1;
  return 0;
}

static bool is_better_motion(const RANSAC_MOTION *motion_a,
                             const RANSAC_MOTION *motion_b) {
  return compare_motions(motion_a, motion_b) < 0;
}

// Returns true on success, false on error
static bool ransac_internal(const Correspondence *matched_points, int npoints,
                            MotionModel *motion_models, int num_desired_motions,
                            const RansacModelInfo *model_info,
                            bool *mem_alloc_failed) {
  assert(npoints >= 0);
  int i = 0;
  int minpts = model_info->minpts;
  bool ret_val = true;

  unsigned int seed = (unsigned int)npoints;

  int indices[MAX_MINPTS] = { 0 };

  // Store information for the num_desired_motions best transformations found
  // and the worst motion among them, as well as the motion currently under
  // consideration.
  RANSAC_MOTION *motions, *worst_kept_motion = NULL;
  RANSAC_MOTION current_motion;

  // Store the parameters and the indices of the inlier points for the motion
  // currently under consideration.
  double params_this_motion[MAX_PARAMDIM];

  // Initialize output models, as a fallback in case we can't find a model
  for (i = 0; i < num_desired_motions; i++) {
    memcpy(motion_models[i].params, kIdentityParams,
           MAX_PARAMDIM * sizeof(*(motion_models[i].params)));
    motion_models[i].num_inliers = 0;
  }

  if (npoints < minpts * MINPTS_MULTIPLIER || npoints == 0) {
    return false;
  }

  int min_inliers = AOMMAX((int)(MIN_INLIER_PROB * npoints), minpts);

  motions =
      (RANSAC_MOTION *)aom_calloc(num_desired_motions, sizeof(RANSAC_MOTION));

  // Allocate one large buffer which will be carved up to store the inlier
  // indices for the current motion plus the num_desired_motions many
  // output models
  // This allows us to keep the allocation/deallocation logic simple, without
  // having to (for example) check that `motions` is non-null before allocating
  // the inlier arrays
  int *inlier_buffer = (int *)aom_malloc(sizeof(*inlier_buffer) * npoints *
                                         (num_desired_motions + 1));

  if (!(motions && inlier_buffer)) {
    ret_val = false;
    *mem_alloc_failed = true;
    goto finish_ransac;
  }

  // Once all our allocations are known-good, we can fill in our structures
  worst_kept_motion = motions;

  for (i = 0; i < num_desired_motions; ++i) {
    motions[i].inlier_indices = inlier_buffer + i * npoints;
  }
  memset(&current_motion, 0, sizeof(current_motion));
  current_motion.inlier_indices = inlier_buffer + num_desired_motions * npoints;

  for (int trial_count = 0; trial_count < NUM_TRIALS; trial_count++) {
    lcg_pick(npoints, minpts, indices, &seed);

    if (!model_info->find_transformation(matched_points, indices, minpts,
                                         params_this_motion)) {
      continue;
    }

    model_info->score_model(params_this_motion, matched_points, npoints,
                            &current_motion);

    if (current_motion.num_inliers < min_inliers) {
      // Reject models with too few inliers
      continue;
    }

    if (is_better_motion(&current_motion, worst_kept_motion)) {
      // This motion is better than the worst currently kept motion. Remember
      // the inlier points and sse. The parameters for each kept motion
      // will be recomputed later using only the inliers.
      worst_kept_motion->num_inliers = current_motion.num_inliers;
      worst_kept_motion->sse = current_motion.sse;

      // Rather than copying the (potentially many) inlier indices from
      // current_motion.inlier_indices to worst_kept_motion->inlier_indices,
      // we can swap the underlying pointers.
      //
      // This is okay because the next time current_motion.inlier_indices
      // is used will be in the next trial, where we ignore its previous
      // contents anyway. And both arrays will be deallocated together at the
      // end of this function, so there are no lifetime issues.
      int *tmp = worst_kept_motion->inlier_indices;
      worst_kept_motion->inlier_indices = current_motion.inlier_indices;
      current_motion.inlier_indices = tmp;

      // Determine the new worst kept motion and its num_inliers and sse.
      for (i = 0; i < num_desired_motions; ++i) {
        if (is_better_motion(worst_kept_motion, &motions[i])) {
          worst_kept_motion = &motions[i];
        }
      }
    }
  }

  // Sort the motions, best first.
  qsort(motions, num_desired_motions, sizeof(RANSAC_MOTION), compare_motions);

  // Refine each of the best N models using iterative estimation.
  //
  // The idea here is loosely based on the iterative method from
  // "Locally Optimized RANSAC" by O. Chum, J. Matas and Josef Kittler:
  // https://cmp.felk.cvut.cz/ftp/articles/matas/chum-dagm03.pdf
  //
  // However, we implement a simpler version than their proposal, and simply
  // refit the model repeatedly until the number of inliers stops increasing,
  // with a cap on the number of iterations to defend against edge cases which
  // only improve very slowly.
  for (i = 0; i < num_desired_motions; ++i) {
    if (motions[i].num_inliers <= 0) {
      // Output model has already been initialized to the identity model,
      // so just skip setup
      continue;
    }

    bool bad_model = false;
    for (int refine_count = 0; refine_count < NUM_REFINES; refine_count++) {
      int num_inliers = motions[i].num_inliers;
      assert(num_inliers >= min_inliers);

      if (!model_info->find_transformation(matched_points,
                                           motions[i].inlier_indices,
                                           num_inliers, params_this_motion)) {
        // In the unlikely event that this model fitting fails, we don't have a
        // good fallback. So leave this model set to the identity model
        bad_model = true;
        break;
      }

      // Score the newly generated model
      model_info->score_model(params_this_motion, matched_points, npoints,
                              &current_motion);

      // At this point, there are three possibilities:
      // 1) If we found more inliers, keep refining.
      // 2) If we found the same number of inliers but a lower SSE, we want to
      //    keep the new model, but further refinement is unlikely to gain much.
      //    So commit to this new model
      // 3) It is possible, but very unlikely, that the new model will have
      //    fewer inliers. If it does happen, we probably just lost a few
      //    borderline inliers. So treat the same as case (2).
      if (current_motion.num_inliers > motions[i].num_inliers) {
        motions[i].num_inliers = current_motion.num_inliers;
        motions[i].sse = current_motion.sse;
        int *tmp = motions[i].inlier_indices;
        motions[i].inlier_indices = current_motion.inlier_indices;
        current_motion.inlier_indices = tmp;
      } else {
        // Refined model is no better, so stop
        // This shouldn't be significantly worse than the previous model,
        // so it's fine to use the parameters in params_this_motion.
        // This saves us from having to cache the previous iteration's params.
        break;
      }
    }

    if (bad_model) continue;

    // Generate a final model, taking into account the quantization of the
    // parameters
    if (!model_info->find_transformation_quantize(
            matched_points, motions[i].inlier_indices, motions[i].num_inliers,
            motion_models[i].params)) {
      // This fit shouldn't fail; if it does, give up and return the identity
      continue;
    }

    // Fill in remaining output fields
    for (int j = 0; j < motions[i].num_inliers; j++) {
      int index = motions[i].inlier_indices[j];
      const Correspondence *corr = &matched_points[index];
      motion_models[i].inliers[2 * j + 0] = (int)rint(corr->x);
      motion_models[i].inliers[2 * j + 1] = (int)rint(corr->y);
    }
    motion_models[i].num_inliers = motions[i].num_inliers;
  }

finish_ransac:
  aom_free(inlier_buffer);
  aom_free(motions);

  return ret_val;
}

static const RansacModelInfo ransac_model_info[TRANS_TYPES] = {
  // IDENTITY
  { NULL, NULL, NULL, 0 },
// TRANSLATION
#if ALLOW_TRANSLATION_MODELS
  { find_translation, find_translation_quantize, score_translation, 1 },
#else
  { NULL, NULL, NULL, 0 },
#endif
  // ROTZOOM
  { find_rotzoom, find_rotzoom_quantize, score_affine, 2 },
  // AFFINE
  { find_affine, find_affine_quantize, score_affine, 3 },
};

// Returns true on success, false on error
bool ransac(const Correspondence *matched_points, int npoints,
            TransformationType type, MotionModel *motion_models,
            int num_desired_motions, bool *mem_alloc_failed) {
#if ALLOW_TRANSLATION_MODELS
  assert(type > IDENTITY && type < TRANS_TYPES);
#else
  assert(type > TRANSLATION && type < TRANS_TYPES);
#endif  // ALLOW_TRANSLATION_MODELS

  return ransac_internal(matched_points, npoints, motion_models,
                         num_desired_motions, &ransac_model_info[type],
                         mem_alloc_failed);
}