#include <Wire.h>

#ifndef _HELPER_H_
#include "helper.h"
#endif

#include "SparkFun_Qwiic_Scale_NAU7802_Arduino_Library.h"

#define SAMPLES 5
#define IGNORE_READINGS 5

NAU7802 myScale; //Create instance of the NAU7802 class

byte debug_level;

byte interruptPin = A0;

void SetScalesDebugLevel(byte dbg_level)
{
  debug_level = dbg_level;
}

bool SetupScales(byte dbg_level)
{
  debug_level = dbg_level;
  if (debug_level > 0) {
    gCatena.SafePrintf("%010d - SetupScales start\n", millis());
  }
  pinMode(interruptPin, INPUT);
  Wire.begin();

  if (!myScale.begin())
  {
    gCatena.SafePrintf("Scale not detected. Please check wiring. Freezing...\n");
    return false;
  }
  gCatena.SafePrintf("Scale detected!\n");

  myScale.setIntPolarityHigh();

  myScale.clearBit(NAU7802_PGA_PWR_PGA_CAP_EN, NAU7802_PGA_PWR);
  myScale.setSampleRate(NAU7802_SPS_80);
  myScale.setLDO(NAU7802_LDO_3V3);
  myScale.setGain(NAU7802_GAIN_128);

  if (debug_level > 0) {
    gCatena.SafePrintf("%010d - SetupScales done\n", millis());
  }

  return true;
}

long ReadScale(char channel)
{
  long res;

  if (debug_level > 0) {
    gCatena.SafePrintf("%010d - ReadScale Start\n", millis());
  }

  uint8_t channelNumber;
  if (channel == 'B') {
    channelNumber = NAU7802_CHANNEL_1;
  } else {
    channelNumber = NAU7802_CHANNEL_2;
  }
  long startTime = millis();
  myScale.setChannel(channelNumber);
  bool calibrate_success = myScale.calibrateAFE();
  if (! calibrate_success) {
    if (debug_level > 0) {
      gCatena.SafePrintf("Error: Calibration not successful!\n");
    }
  }

  int32_t dummy;
  int const ignore_readings = IGNORE_READINGS; // number of first <n> readings to ignore
  int const num_scale_readings = SAMPLES; // number of instantaneous scale readings to calculate the median

  for (int i = 0; i < ignore_readings; i++) {
    //while (digitalRead(interruptPin) == LOW) {
    while (! myScale.available()) {
      if ((millis() - startTime) > 60000) {
        if (debug_level > 0) {
          gCatena.SafePrintf("Timeout while reading scale (dummy values)...\n");
        }
        return 0;
      }
      delay(1);
    }
    dummy = myScale.getReading();
  }

  // we use the median, not the average, see https://community.particle.io/t/boron-gpio-provides-less-current-than-electrons-gpio/46647/13
  startTime = millis();
  long readings[num_scale_readings];  // create arry to hold readings
  for (int i = 0; i < num_scale_readings; i++) {
    //while (digitalRead(interruptPin) == LOW) {
    while (! myScale.available()) {
      //while(! myScale.available()) {
      // we set a timeout of 60 seconds for the measurement...
      if ((millis() - startTime) > 60000) {
        if (debug_level > 0) {
          gCatena.SafePrintf("Timeout while reading scale...\n");
        }
        return 0;
      }
      delay(1);
    }
    int32_t reading = myScale.getReading();
    delay(10);
    if (debug_level > 0) {
      gCatena.SafePrintf("Reading int32_t: %d\n", reading);
    }
    readings[i] = long(reading);  // fill the array with instantaneous readings from the scale
  }

  long duration = millis() - startTime;
  res = median(readings, num_scale_readings); // calculate median

  if (debug_level > 0) {
    gCatena.SafePrintf("Median of %d samples: %d\n", num_scale_readings, res);
    float sdev;
    sdev = stddev(readings, num_scale_readings);
    float sdev_proc;
    sdev_proc = 100 * (sdev / float(res));
    gCatena.SafePrintf("Measurements: [");
    for (int i = 0; i < num_scale_readings; i++) {
      gCatena.SafePrintf("%d", readings[i]);
      if (i < (SAMPLES - 1)) {
        gCatena.SafePrintf(",");
      }

    }
    gCatena.SafePrintf("]\n");

    gCatena.SafePrintf("Standard Deviation: %d.%03d\n", (int)sdev, (int)abs(sdev * 1000) % 1000);
    gCatena.SafePrintf("Standard Deviation / Median (Percent): %d.%03d\n", (int)sdev_proc, (int)abs(sdev_proc * 1000) % 1000);
    gCatena.SafePrintf("Duration (ms): %d\n", duration);
  }

  if (debug_level > 0) {
    gCatena.SafePrintf("%010d - ReadScale Done\n", millis());
  }

  return res;
}

void PowerdownScale()
{
  if (debug_level > 0) {
    gCatena.SafePrintf("%010d - PowerdownScale Start\n", millis());
  }
  myScale.powerDown();
  if (debug_level > 0) {
    gCatena.SafePrintf("%010d - PowerdownScale Done\n", millis());
  }
}

void PowerupScale()
{
  if (debug_level > 0) {
    gCatena.SafePrintf("%010d - PowerupScale Start\n", millis());
  }
  myScale.powerUp(); //Power up scale. This scale takes ~600ms to boot and take reading.

  // we wait 100 ms to give it time to stabilze
  delay(100);

  if (debug_level > 0) {
    gCatena.SafePrintf("%010d - PowerupScale Done\n", millis());
  }
}