#include "liblocky.h"

#include <assert.h>
#include <stdlib.h>
#include <string.h>


void blky_extractor_init(blky_extractor_t* ex) {
  assert(ex->block_num_x > 0);
  assert(ex->block_num_y > 0);
  assert(ex->sensor_num_block_x > 0);
  assert(ex->sensor_num_block_y > 0);
  assert(ex->samples_per_pix > 0);
  assert(ex->utime > 0);
  assert(ex->utime <= BLKY_SENSOR_MAX_DUR);
  assert(ex->pix_stride > 0);
  assert(ex->pix_stride >= ex->samples_per_pix);
  assert(ex->correl_max_var >= 0);
  assert(ex->correl_min_avg >= 0);
  assert(ex->correl_min_avg <= 1);

  // set immutable params
  ex->block_num = ex->block_num_x * ex->block_num_y;

  ex->sensor_num_block =
    ex->sensor_num_block_x * ex->sensor_num_block_y *
    ex->samples_per_pix;

  ex->sensor_num_whole = ex->sensor_num_block * ex->block_num;

  ex->block_w = 1./ex->block_num_x;
  ex->block_h = 1./ex->block_num_y;

  ex->sensor_interval_x = ex->block_w / ex->sensor_num_block_x;
  ex->sensor_interval_y = ex->block_h / ex->sensor_num_block_y;

  // clear states
  ex->time = 0;

  const uint64_t sensors_bytes = sizeof(ex->sensors[0]) * ex->sensor_num_whole;
  const uint64_t probs_bytes   = sizeof(ex->probs[0]) * ex->block_num;

  const uint64_t mem_bytes = sensors_bytes + probs_bytes;
  ex->mem = calloc(mem_bytes, 1);
  assert(ex->mem);

  ex->sensors = (blky_sensor_t*) ex->mem;
  ex->probs   = (double*) (ex->mem + sensors_bytes);
}
void blky_extractor_deinit(blky_extractor_t* ex) {
  free(ex->mem);
}

bool blky_extractor_feed(
    blky_extractor_t* ex,
    const uint8_t* img, uint32_t w, uint32_t h, const double verts[8]) {
  blky_sensor_t* s = ex->sensors;

  for (uint32_t by = 0; by < ex->block_num_y; ++by) {
    const double byf = by*ex->block_h + ex->sensor_interval_y/2.;
    for (uint32_t bx = 0; bx < ex->block_num_x; ++bx) {
      const double bxf = bx*ex->block_w + ex->sensor_interval_x/2.;
      for (uint32_t sy = 0; sy < ex->sensor_num_block_y; ++sy) {
        const double syf = byf + ex->sensor_interval_y*sy;
        for (uint32_t sx = 0; sx < ex->sensor_num_block_x; ++sx) {
          const double sxf = bxf + ex->sensor_interval_x*sx;

          const uint8_t* base = img +
            ex->pix_stride*blky_image_offset(w, h, verts, sxf, syf);
          for (uint8_t off = 0; off < ex->samples_per_pix; ++off) {
            const float v = base[off]*1.f / UINT8_MAX;
            blky_sensor_feed(s++, &v, 1);
          }
        }
      }
    }
  }

  ++ex->time;
  if (ex->time%ex->utime > 0) return false;

  for (uint64_t bi = 0; bi < ex->block_num; ++bi) {
    blky_sensor_t* s = ex->sensors + (bi*ex->sensor_num_block);
    ex->probs[bi] = blky_block_estimate(
        s, ex->sensor_num_block, ex->correl_max_var, ex->correl_min_avg);
    memset(s, 0, ex->sensor_num_block*sizeof(*s));
  }
  return true;
}