/// \file algorithm/distancepath.cpp
/// \copyright 2022 Sascha Nitsch
/// Licenced under MIT license
/// \brief implementation of the DistancePath class


// system includes
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <utility>

// own includes
#include "distancepath.h"

namespace Algorithm {

DistancePath::DistancePath(int argc, const char* argv[]) {
  if (argc < 6) {
    printf("DistancePath needs min 6 arguments <weight for traversalable pixel> <weight for blocked pixel> <Threshhold to mark as traversable> <max Distance> <StartX> <StartY> [<StartX> <StartY>] ...");
  } else {
    m_weightPath = atof(argv[0]);
    m_weightBlock = atof(argv[1]);
    m_threshhold = atoi(argv[2]);
    m_maxDistance = atof(argv[3]);
    for (uint8_t i = 4; i < argc; i+=2) {
      m_queue1.push_back((uint32_t)(atoi(argv[i]) << 16) + atoi(argv[i + 1]));
    }
  }
}

DistancePath::~DistancePath() {
}

inline void DistancePath::processPixel(uint8_t* inRAW, float* outRow, uint16_t x, uint16_t y, float lastValue, uint16_t width, std::list<uint32_t>* next, uint16_t* outPixels, uint16_t iteration) {
  float *oR = &outRow[x];
  uint32_t offset = (y * width + x) * 3;
  float avg = (inRAW[offset] + inRAW[offset + 1] + inRAW[offset + 2]) / 765.0;
  float newValue = lastValue + (1.0 - avg) * (m_weightBlock - m_weightPath) + m_weightPath;
  if (newValue > m_maxDistance) newValue = m_maxDistance;
  if (newValue < *oR) {
    next->push_back((x << 16) + y);
    outPixels[offset + 1] = iteration;
    *oR = newValue;
  }
}
inline void DistancePath::processRow(uint8_t* inRAW, float* outRAW, uint16_t x, uint16_t y, float lastValue, uint16_t width, std::list<uint32_t>* next, uint16_t* outPixels, uint16_t iteration) {
  float* outRow = &outRAW[y * width];
  if (x > 0) {
    processPixel(inRAW, outRow, x - 1, y, lastValue, width, next, outPixels, iteration);
  }
  processPixel(inRAW, outRow, x, y, lastValue, width, next, outPixels, iteration);
  if (x < width - 1) {
    processPixel(inRAW, outRow, x + 1, y, lastValue, width, next, outPixels, iteration);
  }
}

int8_t DistancePath::process(Image* input, Image* output) {
  std::list<uint32_t> queue2;
  std::list<uint32_t>* last = &m_queue1;
  std::list<uint32_t>* next = &queue2;
  uint16_t width = input->getWidth();
  uint16_t height = input->getHeight();
  uint8_t* inRAW = input->getPixels8();
  uint32_t end = width * height;
  // allocate output storage and set to m_maxDistance
  float* outRAW = reinterpret_cast<float*>(malloc(width * height * sizeof(float)));
  for (uint32_t i = 0; i < end; ++i) {
    outRAW[i] = m_maxDistance;
  }
  // set start pixels to distance of 0
  uint16_t* outPixels = output->getPixels16();
  for (uint32_t coordinate : m_queue1) {
    uint16_t x = coordinate >> 16;
    uint16_t y = coordinate & 0xFFFF;
    outRAW[(width * y + x)] = 0;
  }
  uint16_t iteration = 1;
  do {
    // process pixel from last time
    for (uint32_t coordinate : *last) {
      uint16_t x = coordinate >> 16;
      uint16_t y = coordinate & 0xFFFF;
      float lastValue = outRAW[y * width + x];
      if (y > 0) {  // process row on top
        processRow(inRAW, outRAW, x, y-1, lastValue, width, next, outPixels, iteration);
      }
      processRow(inRAW, outRAW, x, y, lastValue, width, next, outPixels, iteration);
      if (y < height - 1) {  // process row on bottom
        processRow(inRAW, outRAW, x, y+1, lastValue, width, next, outPixels, iteration);
      }
    }
    ++iteration;
    // clear out old list
    last->clear();
    // swap list to get new input queue
    std::swap(last, next);
    if (iteration > 10000) break;
  }  while (last->size() > 0);

  printf("iterations: %i\n", iteration);
  // find max value
  float max = 0;
  uint16_t threshhold = m_threshhold * 3;  // multiply by 3 here saves many division from sum to average in the loops below

  // could be optimized, but it is only run once for every pixel
  for (uint32_t i = 0; i < end; ++i) {
    uint32_t offset = i * 3;
    uint16_t sum = (inRAW[offset] + inRAW[offset + 1] + inRAW[offset + 2]);
    // only update max if it is larger than previous, but still smaller than our max distance and larger as the threshhold (visible path cheat)
    if (outRAW[i] > max && outRAW[i] < m_maxDistance && (sum > threshhold)) max = outRAW[i];
  }
  float iterMult = 65535.0 / iteration;  // multiplicator to map iteration from 0 to 65535
  max = 65535.0 / max;  // results in 0 ... 65535 range
  // write result to output
  uint32_t offset = 0;
  for (uint32_t i = 0; i < end; ++i) {
    uint16_t sum = (inRAW[offset] + inRAW[offset + 1] + inRAW[offset + 2]);
    if (sum > threshhold) {
      outPixels[offset] = outRAW[i] * max;  // red = distance
      outPixels[offset + 1] = iterMult * outPixels[offset + 1];  // green = iteration
      outPixels[offset + 2] = 65535;  // blue= a pathable pixel
    } else {
      outPixels[offset] = 0;
      outPixels[offset + 1] = 0;
      outPixels[offset + 2] = 0;
    }
    offset += 3;
  }
  free(outRAW);
  return 0;
}
}  // namespace Algorithm