// clang-format off
#include <stdlib.h>
#include "fast.h"


#define Compare(X, Y) ((X)>=(Y))

xy* aom_nonmax_suppression(const xy* corners, const int* scores, int num_corners,
                           int** ret_scores, int* ret_num_nonmax)
{
  int num_nonmax=0;
  int last_row;
  int* row_start;
  int i, j;
  xy* ret_nonmax;
  int* nonmax_scores;
  const int sz = (int)num_corners;

  /*Point above points (roughly) to the pixel above the one of interest, if there
    is a feature there.*/
  int point_above = 0;
  int point_below = 0;

  *ret_num_nonmax = 0;
  if(!(corners && scores) || num_corners < 1)
  {
    return 0;
  }

  ret_nonmax = (xy*)malloc(num_corners * sizeof(xy));
  if (! ret_nonmax)
  {
    return 0;
  }

  nonmax_scores = (int*)malloc(num_corners * sizeof(*nonmax_scores));
  if (! nonmax_scores)
  {
    free(ret_nonmax);
    return 0;
  }

  /* Find where each row begins
     (the corners are output in raster scan order). A beginning of -1 signifies
     that there are no corners on that row. */
  last_row = corners[num_corners-1].y;
  row_start = (int*)malloc((last_row+1)*sizeof(int));
  if(! row_start)
  {
    free(ret_nonmax);
    free(nonmax_scores);
    return 0;
  }

  for(i=0; i < last_row+1; i++)
    row_start[i] = -1;

  {
    int prev_row = -1;
    for(i=0; i< num_corners; i++)
      if(corners[i].y != prev_row)
      {
        row_start[corners[i].y] = i;
        prev_row = corners[i].y;
      }
  }


  for(i=0; i < sz; i++)
  {
    int score = scores[i];
    xy pos = corners[i];

    /*Check left */
    if(i > 0)
      if(corners[i-1].x == pos.x-1 && corners[i-1].y == pos.y && Compare(scores[i-1], score))
        continue;

    /*Check right*/
    if(i < (sz - 1))
      if(corners[i+1].x == pos.x+1 && corners[i+1].y == pos.y && Compare(scores[i+1], score))
        continue;

    /*Check above (if there is a valid row above)*/
    if(pos.y > 0)
      if (row_start[pos.y - 1] != -1)
      {
        /*Make sure that current point_above is one
          row above.*/
        if(corners[point_above].y < pos.y - 1)
          point_above = row_start[pos.y-1];

        /*Make point_above point to the first of the pixels above the current point,
          if it exists.*/
        for(; corners[point_above].y < pos.y && corners[point_above].x < pos.x - 1; point_above++)
        {}


        for(j=point_above; corners[j].y < pos.y && corners[j].x <= pos.x + 1; j++)
        {
          int x = corners[j].x;
          if( (x == pos.x - 1 || x ==pos.x || x == pos.x+1) && Compare(scores[j], score))
            goto cont;
        }

      }

    /*Check below (if there is anything below)*/
    if(pos.y >= 0)
      if (pos.y != last_row && row_start[pos.y + 1] != -1 && point_below < sz) /*Nothing below*/
      {
        if(corners[point_below].y < pos.y + 1)
          point_below = row_start[pos.y+1];

        /* Make point below point to one of the pixels belowthe current point, if it
           exists.*/
        for(; point_below < sz && corners[point_below].y == pos.y+1 && corners[point_below].x < pos.x - 1; point_below++)
        {}

        for(j=point_below; j < sz && corners[j].y == pos.y+1 && corners[j].x <= pos.x + 1; j++)
        {
          int x = corners[j].x;
          if( (x == pos.x - 1 || x ==pos.x || x == pos.x+1) && Compare(scores[j],score))
            goto cont;
        }
      }

    ret_nonmax[num_nonmax] = corners[i];
    nonmax_scores[num_nonmax] = scores[i];
    num_nonmax++;
cont:
    ;
  }

  free(row_start);
  *ret_scores = nonmax_scores;
  *ret_num_nonmax = num_nonmax;
  return ret_nonmax;
}

// clang-format on