#include <Arduino.h>
#include "constants.h"
#include "config.h"
#include "util.h"

void setupGaussianKernel(float* kernel, int width, float sigma) {
  float sum = 0.0;
  for(int i = 0; i < width; i++) {
    //kernel[width+i] = exp( -(i*i) / (2 * sigma * sigma)) / (PI * 2 * sigma * sigma);
    kernel[i] = exp(-0.5f * (i * i) / (sigma * sigma));
    sum += kernel[width];
  }
  for(int i; i < width; i++) {
    kernel[i] = sum/width;
  }
}

//float first_pass[PANEL_WIDTH][PANEL_HEIGHT];
void gaussian_blur(float *field[PANEL_WIDTH][PANEL_HEIGHT]) {
  float kernel[GAUSS_WIDTH];
  float first_pass[PANEL_WIDTH][PANEL_HEIGHT];
  memset(first_pass, 0.0, sizeof first_pass);
  setupGaussianKernel(kernel, GAUSS_WIDTH, GAUSS_SIGMA);
  // horizontal pass
  for (int x = 0; x < PANEL_WIDTH; x++) {
    for(int y = 0; y < PANEL_HEIGHT; y++) {
      float sum = *field[x][y] * DECAY_FACTOR * kernel[0]; //0.0;
      int additions = 1;
      for (int x_offset = 1; x_offset < GAUSS_WIDTH; x_offset++) {
        if (is_in_bounds(x+x_offset, y)) {
          sum += *field[x + x_offset][y] * kernel[x_offset];
          additions++;
        }
        if (is_in_bounds(x-x_offset, y)) {
          sum += *field[x - x_offset][y] * kernel[x_offset];
          additions++;
        }
      }
      first_pass[x][y] = sum/GAUSS_WIDTH;
    }
  }
  // vertical pass
  for (int x = 0; x < PANEL_WIDTH; x++) {
    for(int y = 0; y < PANEL_HEIGHT; y++) {
      float sum = first_pass[x][y] * kernel[0]; //0.0;
      int additions = 1;
      for (int y_offset = 1; y_offset < GAUSS_WIDTH; y_offset++) {
        if (is_in_bounds(x, y + y_offset)) {
          sum += first_pass[x][y + y_offset] * kernel[y_offset];
          additions++;
        }
        if (is_in_bounds(x, y - y_offset)) {
          sum += first_pass[x][y - y_offset] * kernel[y_offset];
          additions++;
        }
      }
      *field[x][y] = sum/GAUSS_WIDTH;
    } 
  }
}

void box_blur(float field[PANEL_WIDTH][PANEL_HEIGHT]) {
  auto first_pass = new float[PANEL_WIDTH][PANEL_HEIGHT]();
  // horizontal pass
  for (int x = 0; x < PANEL_WIDTH; x++) {
    for(int y = 0; y < PANEL_HEIGHT; y++) {
      float sum;
      sum = field[x][y] * DECAY_FACTOR; //0.0;
      int additions = 1;
      for (int x_offset = 1; x_offset < BLUR_KERNEL_SIZE; x_offset++) {
        if (is_in_bounds(x+x_offset, y)) {
          sum += field[x + x_offset][y];
          additions++;
        }
        if (is_in_bounds(x-x_offset, y)) {
          sum += field[x - x_offset][y];
          additions++;
        }
      }
      first_pass[x][y] = (sum/additions);
    }
  }
  // vertical pass
  for (int x = 0; x < PANEL_WIDTH; x++) {
    for(int y = 0; y < PANEL_HEIGHT; y++) {
      float sum = first_pass[x][y]; //0.0;
      int additions = 1;
      for (int y_offset = 1; y_offset < BLUR_KERNEL_SIZE; y_offset++) {
        if (is_in_bounds(x, y + y_offset)) {
          sum += first_pass[x][y + y_offset];
          additions++;
        }
        if (is_in_bounds(x, y - y_offset)) {
          sum += first_pass[x][y - y_offset];
          additions++;
        }
      }

      float result = (sum/additions); // * DECAY_FACTOR;
      //attractant[x][y] = (((8.0*attractant[x][y]) + result) / 9.0);
      field[x][y] = result;
    } 
  }
  delete[] first_pass;
}