#pragma once

#include <Wire.h>

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

#include "SparkFun_Qwiic_Scale_NAU7802_Arduino_Library.h"

#define SAMPLES 5


byte debug_level;

//byte interruptPin = A0;

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

bool InitializeScales()
{
  bool result;
  result = myScale.reset(); //Reset all registers
  result &= myScale.powerUp(); //Power on analog and digital sections of the scale

  result &= myScale.setLDO(NAU7802_LDO_3V3); //Set LDO to 3.3V
  result &= myScale.setGain(NAU7802_GAIN_128); //Set gain to 128
  result &= myScale.setSampleRate(NAU7802_SPS_40); //Set samples per second to 40
  result &= myScale.setRegister(NAU7802_ADC, 0x30); //Turn off CLK_CHP. From 9.1 power on sequencing.

  result &= myScale.calibrateAFE(); //Re-cal analog front end when we change gain, sample rate, or channel

  return result;
}

bool SetupScales(byte dbg_level)
{
  debug_level = dbg_level;
  if (debug_level > 0) {
    gCatena.SafePrintf("SetupScales start\n");
  }
  //  pinMode(interruptPin, INPUT);

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

  bool result = InitializeScales();
  if (debug_level > 0) {
    gCatena.SafePrintf("SetupScales done, result: %d\n", result);
  }

  return result;
}

long ReadScale(char channel)
{
  long res;

  if (debug_level > 0) {
    gCatena.SafePrintf("ReadScale Start, Channel %c\n",  channel);
  }

  uint8_t channelNumber;
  if (channel == 'B') {
    channelNumber = NAU7802_CHANNEL_1;
  } else {
    channelNumber = NAU7802_CHANNEL_2;
  }
  unsigned 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");
    }
  }

  if (myScale.available()) {
    long dummy = myScale.getReading();
  }
  int const num_scale_readings = SAMPLES; // number of instantaneous scale readings to calculate the median

  // we use the median, not the average, see https://community.particle.io/t/boron-gpio-provides-less-current-than-electrons-gpio/46647/13
  long readings[num_scale_readings];  // create array to hold readings
  for (int i = 0; i < num_scale_readings; i++) {
    //while (digitalRead(interruptPin) == LOW) {
    unsigned long mytimer = millis();
    int timeouts = 0;
    while (! myScale.available() && (timeouts < 3)) {
      // we set a timeout of 10 seconds for the measurement...
      if ((millis() - mytimer) > 10000) {
        timeouts = timeouts + 1;
        // Timeout reading scale...
        Wire.endTransmission(true);
        delay(50);
        InitializeScales();
        if (debug_level > 0) {
          gCatena.SafePrintf("Timeout while reading scale...\n");
        }
      }
      delay(50);
    }
    long reading;
    if (myScale.available()) {
      reading = myScale.getReading();
      readings[i] = reading;
    }
    if (debug_level > 0) {
      gCatena.SafePrintf("Reading: %d\n", reading);
    }
    delay(50);
  }

  unsigned 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("ReadScale Done\n");
  }

  return res;
}

void PowerdownScale()
{
  if (debug_level > 0) {
    gCatena.SafePrintf("PowerdownScale Start\n");
  }
  myScale.powerDown();
  if (debug_level > 0) {
    gCatena.SafePrintf("PowerdownScale Done\n");
  }
}

void PowerupScale()
{
  if (debug_level > 0) {
    gCatena.SafePrintf("PowerupScale Start\n");
  }

  InitializeScales();

  if (debug_level > 0) {
    gCatena.SafePrintf("PowerupScale Done\n");
  }
}