filters.cpp

#include "filters.hpp"

#include <cmath>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <vector>

struct Pic {
  int height;
  int width;
  const std::uint8_t *data;
};

static std::vector<float> GaussianKernel(float deviation, int kernelsize){
  const int radius = kernelsize >> 1;
  std::vector<float> kernel(static_cast<std::size_t>(kernelsize));

  // Handle sigma
  if (deviation <= 0.0f){
    // Identity kernel
    std::size_t i = 0;
    while (i < kernel.size()) {
      kernel[i] = 0.0f;
      ++i;
    }
    kernel[static_cast<std::size_t>(radius)] = 1.0f;
    return kernel;
  }

  float dev_sq = deviation * deviation;
  const float inv_two_sigma2 = 1.0f / (2.0f * dev_sq);

  float current_sum = 0.0f;

  int curr = -radius;
  while (curr <= radius) {
    int size_curr = curr*curr;
    const float x = static_cast<float>(size_curr);
    const float value = std::exp(-x * inv_two_sigma2);
    kernel[static_cast<std::size_t>(curr + radius)] = value;
    current_sum += value;
    ++curr;
  }

  // Normalize
  float inv_sum;
  if(current_sum <= 0.0f){
    inv_sum = 1.0f;
  }
  else{
    inv_sum = 1.0f /current_sum;
  }

  std::size_t k_idx = 0;
  while (k_idx < kernel.size()) {
    kernel[k_idx] *=inv_sum;
    ++k_idx;
  }
  return kernel;
}

static inline void gaussian_horizontal_left(const uint8_t *main_image,float *output, const float *kernel,int radius, int Width);
static inline void gaussian_horizontal_center(const uint8_t *main_image,float *output, const float *kernel, int radius,int kernelsize, int Width);
static inline void gaussian_horizontal_right(const uint8_t *main_image,float *output, const float *kernel,int radius, int Width);
static inline void gaussian_vertical(int Height, int Width, const float *kernel,int radius, const float *middle,uint8_t *output_image);

static void gaussian_horizontal(const Pic &image,const std::vector<float> &kernel,int kernelsize, std::vector<float> &middle){
  const int Height = image.height, Width = image.width;
  const int radius = kernelsize >> 1;

  const float *kernel_data = kernel.data();
  int y = 0;
  while (y <Height) {
    const int row_shift = y * Width;
    const uint8_t *main_image = &image.data[row_shift];
    float *output = &middle[row_shift];

    gaussian_horizontal_left(main_image, output, kernel_data, radius, Width);
    gaussian_horizontal_center(main_image, output, kernel_data, radius,kernelsize, Width);
    gaussian_horizontal_right(main_image, output, kernel_data, radius, Width);
    ++y;
  }
}

static inline void gaussian_horizontal_left(const uint8_t *main_image,float *output, const float *kernel,int radius, int Width) {
  int x = 0;
  while (x < radius) {
    float current_sum = 0.0f;
    int k = -radius;
    while (k <= radius) {
      int range = x + k;
      if (range < 0)
        range = 0;
      else if (range >= Width)
        range = Width - 1;

      const int index_kernel = k + radius;
      current_sum +=
          kernel[index_kernel] * static_cast<float>(main_image[range]);
      ++k;
    }

    output[x] = current_sum;
    ++x;
  }
}

static inline void gaussian_horizontal_center(const uint8_t *main_image,float *output, const float *kernel, int radius,int kernelsize, int Width) {
  int x = radius;
  const int end = Width - radius;

  while (x < end) {
    const uint8_t *sptr = main_image + (x - radius);
    const float *kernel_ptr = kernel;

    float acc = 0.0f;
    int i = 0;
    while (i < kernelsize) {
      acc += (*kernel_ptr) * static_cast<float>(*sptr);
      ++kernel_ptr;
      ++sptr;
      ++i;
    }
    output[x] = acc;
    ++x;
  }
}

static inline void gaussian_horizontal_right(const uint8_t *main_image,float *output, const float *kernel,int radius, int Width) {
  int x = Width - radius;
  while (x < Width) {
    float current_sum = 0.0f;

    int k = -radius;
    while (k <= radius) {
      int range = x + k;
      if (range < 0)
        range = 0;
      else if (range >= Width)
        range = Width - 1;

      const int index_kernel = k + radius;
      current_sum +=
          kernel[index_kernel] * static_cast<float>(main_image[range]);
      ++k;
    }

    output[x] = current_sum;
    ++x;
  }
}

static inline void gaussian_vertical(int Height, int Width, const float *kernel,int radius, const float *middle,uint8_t *output_image) {
  const int kernelsize = (radius << 1) + 1;

  if (Height < 3 || Width < 1) {
    int x = 0;
    while (x < Width) {
      int y = 0;
      while (y < Height) {
        float sum = 0.0f;
        int k = -radius;
        while (k <= radius) {
          int yy = y + k;
          if (yy < 0)
            yy = 0;
          else if (yy >= Height)
            yy = Height - 1;

          sum += kernel[k + radius] * middle[yy * Width + x];
          ++k;
        }
        output_image[y * Width + x] = static_cast<uint8_t>(sum);
        ++y;
      }
      ++x;
    }
    return;
  }

  int y = 0;
  while (y < radius && y < Height) {
    const int out_row = y * Width;
    int x = 0;
    while (x < Width) {
      float sum = 0.0f;
      int k = -radius;
      while (k <= radius) {
        int yy = y + k;
        if (yy < 0)
          yy = 0;
        else if (yy >= Height)
          yy = Height - 1;
        sum += kernel[k + radius] * middle[yy * Width + x];
        ++k;
      }
      output_image[out_row + x] = static_cast<uint8_t>(sum);
      ++x;
    }
    ++y;
  }

  const int y_end_center = Height - radius;
  while (y < y_end_center) {
    const int out_row = y * Width;
    const int top_row = (y - radius) * Width;

    int x = 0;
    while (x < Width) {
      float sum = 0.0f;
      const float *sp = middle + top_row + x;
      const float *kp = kernel;
      int i = 0;
      while (i < kernelsize) {
        sum += (*kp) * (*sp);
        ++kp;
        sp += Width;
        ++i;
      }
      output_image[out_row + x] = static_cast<uint8_t>(sum);
      ++x;
    }
    ++y;
  }

  while (y < Height) {
    const int out_row = y * Width;
    int x = 0;
    while (x < Width) {
      float sum = 0.0f;
      int k = -radius;
      while (k <= radius) {
        int yy = y + k;
        if (yy < 0)
          yy = 0;
        else if (yy >= Height)
          yy = Height - 1;
        sum += kernel[k + radius] * middle[yy * Width + x];
        ++k;
      }
      output_image[out_row + x] = static_cast<uint8_t>(sum);
      ++x;
    }
    ++y;
  }
}

// 8-neighborhood in fixed order:
// 0:N 1:NE 2:E 3:SE 4:S 5:SW 6:W 7:NW
static constexpr int kDy8[8] = {-1, -1, 0, 1, 1, 1, 0, -1};
static constexpr int kDx8[8] = {0, 1, 1, 1, 0, -1, -1, -1};

// Map (dy,dx) in {-1,0,1}^2 \ {(0,0)} to direction index in [0,7].
static inline int dir_from_delta(int dy, int dx) {
  // Keep it branch-friendly and very explicit.
  switch (dy) {
  case -1:
    switch (dx) {
    case 0:
      return 0;
    case 1:
      return 1;
    case -1:
      return 7;
    default:
      return -1;
    }
  case 0:
    switch (dx) {
    case 1:
      return 2;
    case -1:
      return 6;
    default:
      return -1;
    }
  case 1:
    switch (dx) {
    case 1:
      return 3;
    case 0:
      return 4;
    case -1:
      return 5;
    default:
      return -1;
    }
  default:
    return -1;
  }
}

static inline bool scan_ring(const int *img, int w, int cy, int cx,int start_dir, int step, int &ny, int &nx) {
  const int inc = (step >= 0) ? 1 : 7;
  int dir = start_dir;
  int t = 0;
  while (t < 8) {
    dir = (dir + inc) & 7;
    const int ty = cy + kDy8[dir];
    const int tx = cx + kDx8[dir];
    if (img[ty * w + tx] != 0) {
      ny = ty;
      nx = tx;
      return true;
    }
    ++t;
  }
  return false;
}

static inline bool first_neighbor_cw(const int *img, int w, int cy, int cx,int &ny, int &nx, int sy, int sx) {
  const int start_dir = dir_from_delta(sy - cy, sx - cx);
  if (start_dir < 0)
    return false;
  return scan_ring(img, w, cy, cx, start_dir, +1, ny, nx);
}

static inline bool next_neighbor_ccw(const int *img, int w, int cy, int cx,int &ny, int &nx, int sy, int sx) {
  const int start_dir = dir_from_delta(sy - cy, sx - cx);
  if (start_dir < 0)
    return false;
  return scan_ring(img, w, cy, cx, start_dir, -1, ny, nx);
}

// Small helpers to keep the main algorithm readable & structurally different.
static inline std::vector<int> make_padded_binary(const std::uint8_t *src,int h, int w, int &outH,int &outW) {
  outH = h + 2;
  outW = w + 2;
  std::vector<int> grid(static_cast<std::size_t>(outH) * static_cast<std::size_t>(outW), 0);
  int y = 0;
  while (y < h) {
    const int in_row = y * w;
    const int out_row = (y + 1) * outW;
    int x = 0;
    while (x < w) {
      if (src[in_row + x] > 0)
        grid[out_row + (x + 1)] = 1;
      ++x;
    }
    ++y;
  }
  return grid;
}

// Hierarchy layout in flat vector (stride 4):
// [0]: parent
// [1]: first_child
// [2]: next_sibling
// [3]: prev_sibling
static const int H_PARENT = 0;
static const int H_CHILD = 1;
static const int H_NEXT = 2;
static const int H_PREV = 3;

static inline void join_parchild(std::vector<int> &H, int parent_idx,int self_idx) {
  // Set parent of self
  H[self_idx * 4 + H_PARENT] = parent_idx;

  // Access parent's first child
  if (H[parent_idx * 4 + H_CHILD] == -1) {
    H[parent_idx * 4 + H_CHILD] = self_idx;
    return;
  }

  // Traverse to last sibling
  int s = H[parent_idx * 4 + H_CHILD];
  while (H[s * 4 + H_NEXT] != -1) {
    s = H[s * 4 + H_NEXT];
  }

  // Link self as next sibling of s
  H[s * 4 + H_NEXT] = self_idx;
  // Link s as prev sibling of self
  H[self_idx * 4 + H_PREV] = s;
}

struct TracerState {
  int cy, cx; // Current position
  int py, px; // Previous position
  int y0, x0; // Start position
  int ny, nx; // Next neighbor position
};

static inline void run_contour_tracer(std::vector<int> &grid, int width,int start_y, int start_x, int search_y,int search_x, int label,std::vector<int> &out_poly) {
  TracerState S;
  S.y0 = start_y;
  S.x0 = start_x;
  S.cy = start_y;
  S.cx = start_x;
  int found_y = -1, found_x = -1;
  if (!first_neighbor_cw(grid.data(), width, S.y0, S.x0, found_y, found_x,search_y, search_x)) {
    grid[S.y0 * width + S.x0] = -label;
    out_poly.push_back(S.x0 - 1);
    out_poly.push_back(S.y0 - 1);
    return;
  }

  // Setup loop state
  S.ny = found_y;
  S.nx = found_x;
  S.py = S.ny;
  S.px = S.nx;

  // Step 2: Traverse the contour
  bool active = true;
  do {
    // Find next move (Counter-Clockwise scan)
    int move_y, move_x;
    if (!next_neighbor_ccw(grid.data(), width, S.cy, S.cx, move_y, move_x, S.py,S.px)) {
      active = false; // Dead end (shouldn't happen in closed loops)
    } else {
      // Apply labeling logic (transformed form)
      // We check pixel relative to motion
      const int idx_curr = S.cy * width + S.cx;
      const int idx_right = idx_curr + 1;

      if (grid[idx_right] == 0) {
        grid[idx_curr] = -label;
      } else if (grid[idx_curr] == 1) {
        grid[idx_curr] = label;
      }

      // Store vertex
      out_poly.push_back(S.cx - 1);
      out_poly.push_back(S.cy - 1);

      // Check loop closure condition
      // (Current is Start) AND (Next is Initial Neighbor)
      if (move_y == S.y0 && move_x == S.x0 && S.cy == S.ny && S.cx == S.nx) {
        active = false;
      }

      // Update state for next iteration
      S.py = S.cy;
      S.px = S.cx;
      S.cy = move_y;
      S.cx = move_x;
    }
  } while (active);
}

extern "C" void gaussian_blurs(const std::uint8_t *input, std::uint8_t *output,int height, int width, int kernel_size,float sigma) {
  const int Height = height;
  const int Width = width;

  // If sigma <= 0, treat as copy (fast path)
  if (sigma <= 0.0f || kernel_size <= 1) {
    const std::size_t bytes =
        static_cast<std::size_t>(Height) * static_cast<std::size_t>(Width);
    std::memcpy(output, input, bytes);
    return;
  }

  Pic image{Height, Width, input};
  auto kernel = GaussianKernel(sigma, kernel_size);

  std::vector<float> middle(static_cast<std::size_t>(Height) * static_cast<std::size_t>(Width));

  gaussian_horizontal(image, kernel, kernel_size, middle);

  const int radius = kernel_size >> 1;
  gaussian_vertical(Height, Width, kernel.data(), radius, middle.data(),output);
}

ContourResult get_contours(const std::uint8_t *img_ptr, int h_raw, int w_raw) {
  int H = 0, W = 0;
  std::vector<int> work_buf = make_padded_binary(img_ptr, h_raw, w_raw, H, W);

  ContourResult output;
  output.hierarchy.reserve(400);
  output.contours.reserve(100);

  // Root node (background) -- 4 ints [-1, -1, -1, -1]
  output.hierarchy.push_back(-1); // parent
  output.hierarchy.push_back(-1); // first_child
  output.hierarchy.push_back(-1); // next_sibling
  output.hierarchy.push_back(-1); // prev_sibling

  int label_gen = 1; // Start labels from 1 (background)

  // Linear scan of the image
  int y = 1;
  const int y_end = H - 1;

  while (y < y_end) {
    int last_known_label = 1;
    int x = 1;
    const int x_end = W - 1;

    while (x < x_end) {
      const int idx = y * W + x;
      int val = work_buf[idx];

      if (val == 0) { // Background skip
        x++;
        continue;
      }

      // Determine explicit topology type
      // Type A: Outer boundary start
      // Type B: Hole boundary start
      bool type_a = (val == 1 && work_buf[idx - 1] == 0);
      bool type_b = (val >= 1 && work_buf[idx + 1] == 0);

      if (type_a || type_b) {
        // We found a new component
        label_gen++;
        int active_label = label_gen;

        // Parent determination
        int parent_id = 0;
        if (type_a == true) {
          parent_id = last_known_label;
        } else {
          if (val > 1){
            parent_id = val;
          }else{
            parent_id = last_known_label;
          }
        }

    
        output.hierarchy.push_back(-1);
        output.hierarchy.push_back(-1);
        output.hierarchy.push_back(-1);
        output.hierarchy.push_back(-1);

        int my_node_idx = static_cast<int>(output.hierarchy.size() / 4) - 1;
        int parent_node_idx = parent_id - 1;

        join_parchild(output.hierarchy, parent_node_idx, my_node_idx);

        std::vector<int> poly;
        poly.reserve(256);
        int sx = (type_a) ? (x - 1) : (x + 1);
        int sy = y;

        run_contour_tracer(work_buf, W, y, x, sy, sx, active_label, poly);
        output.contours.push_back(std::move(poly));

        val = work_buf[idx];
        if (val != 1)
          last_known_label = std::abs(val);

      } else {
        // Just internal pixel, update history
        if (val != 1)
          last_known_label = std::abs(val);
      }
      x++;
    }
    y++;
  }

  return output;
}