#include <wiringPi.h>
#include <wiringPiSPI.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <libconfig.h>

#include "LS7366R.h"
#include "influx.h"
#include "ringbuffer.h"
#include "led.h"


const int CTRL_OUT = 16;
const int INTR_IN  = 19;
const int SPI_CHAN = 0;
const int SPI_SPEED = 1000000;

config_t cfg;

const char EPSILON_KEY[] = "epsilon";
const double DEFAULT_EPSILON = 0.01;

uint32_t skipped = 0;


void isr() {
  static uint32_t lastCounter = 0;

  uint32_t currentCounter = ls7366rReadOTR();

  uint32_t diff = currentCounter - lastCounter;
  lastCounter = currentCounter;

  ringbufferPut(diff);
  led(E_GREEN, true);
}

void init() {
  wiringPiSetupGpio();

  wiringPiSPISetup(SPI_CHAN, SPI_SPEED);

  pinMode(CTRL_OUT, OUTPUT);
  digitalWrite(CTRL_OUT, 0);

  pinMode(INTR_IN, INPUT);
}

void readConfig() {
  config_init(&cfg);
  if (! config_read_file(&cfg, "/opt/etc/counter.cfg")) {
    fprintf(stderr, "failed to read config file: %s:%d - %s\n",
            config_error_file(&cfg), config_error_line(&cfg),
            config_error_text(&cfg));
    config_destroy(&cfg);
    exit(-1);
  }
}

void start() {
  wiringPiISR(INTR_IN, INT_EDGE_RISING, isr);
}

int main (void) {

  readConfig();
  init();
  ledInit();
  ls7366rInit(SPI_CHAN);
  influxInit(&cfg);
  start();

  double epsilon;
  if (! config_lookup_float(&cfg, EPSILON_KEY, &epsilon)) {
    epsilon = DEFAULT_EPSILON;
  }
  

  double lastF = 0;
  bool settled = false;
  uint8_t ledTick = 0;  
  while (1) {
    uint32_t diff = ringbufferGet();
  
    double f = 1.0 / (((double) diff) / 1000000.0);

    if (settled && (abs(f - lastF) > epsilon)) {
      fprintf(stderr, "Current f=%f, last f=%f, gradient too large, skipped\n", f, lastF);
      skipped++;
      f = lastF;
    }
    lastF = f;
//    printf("%f\n", f);
    influxAddFrequency(f);

    ledTick++;
    if (ledTick == 50) {
      ledTick = 0;
      led(E_GREEN, false);
      if (! settled) {
        fprintf(stderr, "Now it is settled\n");
        settled = true;
      }
    }

  }

  // will never be reached
  config_destroy(&cfg);
}