#include <Arduino.h>
#include "xtensa/core-macros.h"
#include <ESP32-HUB75-MatrixPanel-I2S-DMA.h>

#include "main.h"
#include "constants.h"
#include "config.h"
#include "overlay.h"
#include "util.h"

#include "visual.h"
#include "blur.h"

#include <vector>


MatrixPanel_I2S_DMA *matrix = nullptr;


struct SlimeAgent {
  int x_position;
  int y_position;
  float heading;
};


int NUM_AGENTS;
int AGENT_MOVE_DISTANCE;
float AGENT_SENSOR_DISTANCE;
float AGENT_MOVE_ANGLE;
float AGENT_SENSOR_ANGLE;

float attractant[PANEL_WIDTH][PANEL_HEIGHT] = {};
int iterations = 0;
std::vector<SlimeAgent> agents;

void init_attractant() {
  memset(attractant, 0.0, sizeof attractant);
  // draw a circle out of attractant
  //for (int p = 0; p < PANEL_WIDTH; p++) {
  //  float angle = ((PI * 2) / PANEL_WIDTH) * p;
  //  int x = PANEL_WIDTH/2 + round(10 * sin(angle));
  //  int y = PANEL_WIDTH/2 + round(10 * cos(angle));
  //  attractant[x][y] = 15.0;
  //}

  // draw a bitmap
  //for(int x = 0; x < PANEL_WIDTH; x++) {
  //  for(int y = 0; y < PANEL_HEIGHT; y++) {
  //    int pixel_num = (y * PANEL_WIDTH) + x;
  //    if(overlay[ pixel_num / 8 ] & (1 << (7 - (pixel_num % 8)) ) ){
  //      attractant[x][y] = 255.0;
  //    }
  //  }
  //}
}

void init_agents() {
  AGENT_MOVE_DISTANCE = 1;
  AGENT_MOVE_ANGLE = 0.175;//random(0, 334) / 1000.0;
  //if (random(0, 2) > 0) {
  //  AGENT_SENSOR_ANGLE = 0.175;
  //} else {
  AGENT_SENSOR_ANGLE = 0.0875;
  //}
  AGENT_SENSOR_DISTANCE = random(AGENT_SENSOR_DISTANCE_MIN * 1000.0, AGENT_SENSOR_DISTANCE_MAX * 1000.0) / 1000.0;
  NUM_AGENTS = random(NUM_AGENTS_MIN, NUM_AGENTS_MAX);

  agents.clear();
  for(int a = 0; a < NUM_AGENTS; a++) {
    float angle = ((PI * 2) / NUM_AGENTS) * a;
    //agents[a].x_position = 31 + round(10*sin(angle));
    //agents[a].y_position = 15 + round(10*cos(angle));
    // agents[a].heading = ((PI * 2) / NUM_AGENTS) * );
    agents.push_back({
        random(SPAWN_MIN_X, SPAWN_MAX_X),
        random(SPAWN_MIN_Y, SPAWN_MAX_Y),
        (random(0, 100) / 100.0) * (PI*2)
    });
  }
}



void setup(){
  Serial.begin(BAUD_RATE);
  //while (!Serial) {
  //  ; // wait for serial port to connect. Needed for native USB
  //}
  pinMode(ONBOARD_LED, OUTPUT);

  // redefine pins if required
  HUB75_I2S_CFG::i2s_pins _pins={R1, G1, BL1, R2, G2, BL2, CH_A, CH_B, CH_C, CH_D, CH_E, LAT, OE, CLK};
  HUB75_I2S_CFG mxconfig(PANEL_WIDTH, PANEL_HEIGHT, PANELS_NUMBER, _pins);

  mxconfig.gpio.e = PIN_E;
  mxconfig.driver = HUB75_I2S_CFG::FM6126A;   // for panels using FM6126A chips

  mxconfig.latch_blanking = 4;
  mxconfig.i2sspeed = HUB75_I2S_CFG::HZ_10M;
  mxconfig.clkphase = false;

  mxconfig.double_buff = true;

  matrix = new MatrixPanel_I2S_DMA(mxconfig);
  matrix->begin();
  matrix->setBrightness8(64);
  matrix->fillScreenRGB888(0, 0, 0);

  init_agents();
  init_attractant();
}

void loop() {
  matrix ->flipDMABuffer();
  delay(10);
  matrix->fillScreenRGB888(15, 0, 10);

  // draw background and organism
  draw_bg(matrix);
  draw_attractant(matrix, attractant);

  for(int a = 0; a < NUM_AGENTS; a++) {
    //matrix->drawPixel(agents[a].x_position, agents[a].y_position, matrix->color565(0, 0, attractant[agents[a].x_position][agents[a].y_position]));
    // motor step: check if agent can move forward
    // if yes, do so, if not, turn randomly
    float target_x = agents[a].x_position + cos(agents[a].heading) * AGENT_MOVE_DISTANCE;
    float target_y = agents[a].y_position + sin(agents[a].heading) * AGENT_MOVE_DISTANCE;
    int move_x = round(target_x);
    int move_y = round(target_y);
    if (is_in_bounds(move_x, move_y)) {
      agents[a].x_position = move_x;
      agents[a].y_position = move_y;
      if (attractant[move_x][move_y] < 255.0 - AGENT_DROP_AMOUNT) {
        attractant[move_x][move_y] += AGENT_DROP_AMOUNT;
      }
    } else {
      agents[a].heading = (random(0, 100)/100.0) * (PI*2);
    }
  }
  // sample step - check the 3 positions
  for(int a = 0; a < NUM_AGENTS; a++) {
    //matrix->drawPixel(agents[a].x_position, agents[a].y_position, matrix->color565(0, 0, attractant[agents[a].x_position][agents[a].y_position]));
    int front_x = round(agents[a].x_position + cos(agents[a].heading) * AGENT_SENSOR_DISTANCE);
    int front_y = round(agents[a].y_position + sin(agents[a].heading) * AGENT_SENSOR_DISTANCE);
    int left_x = round(agents[a].x_position + cos(agents[a].heading - (AGENT_SENSOR_ANGLE * (PI * 2))) * AGENT_SENSOR_DISTANCE);
    int left_y = round(agents[a].y_position + sin(agents[a].heading - (AGENT_SENSOR_ANGLE * (PI * 2))) * AGENT_SENSOR_DISTANCE);
    int right_x = round(agents[a].x_position + cos(agents[a].heading + (AGENT_SENSOR_ANGLE * (PI * 2))) * AGENT_SENSOR_DISTANCE);
    int right_y = round(agents[a].y_position + sin(agents[a].heading + (AGENT_SENSOR_ANGLE * (PI * 2))) * AGENT_SENSOR_DISTANCE);
    float front_value = attractant[front_x][front_y];
    float left_value = attractant[left_x][left_y];
    float right_value = attractant[right_x][right_y];
    if (front_value > right_value and front_value > left_value) {
      ;
    } else if (front_value < right_value and front_value < left_value) {
      agents[a].heading += (random(0, 2) * 2 - 1) * (AGENT_MOVE_ANGLE / 100.0) * (PI*2) ;
    } else if (left_value > right_value) {
      agents[a].heading -= AGENT_MOVE_ANGLE * (PI * 2);
    } else {
      agents[a].heading += AGENT_MOVE_ANGLE * (PI * 2);
    }
  }
  iterations++;
  if (iterations % 2 == 0) { //64 && iterations % 64 == 0) { // (( == 0) {
    box_blur(attractant);
    //gaussian_blur();
    ;
  }
  if (iterations >= NUM_ITERATIONS) {
    //cleanup();
    if (cleanup(matrix, &iterations)) {
      init_attractant();
      init_agents();
    }
  }
}

