/*
 * Copyright Francesco PotortAZ 2014
 *
 * This software is distributed under the GPLv3 licence
 */

#include <avr/wdt.h>
#include "Streaming.h"
#include "SchedulerARMAVR.h"
//#include <WiFi.h>
#undef PROGMEM				// avoid warnings in the F() macro
#define PROGMEM __attribute__((section(".progmem.data")))



/****************** Constants ******************/

const bool reactiveSensorUsed = true;	// whether we have two blink sensors
const bool usingWatchdogTimer = true;	// set a watchdog timer for reliability

// These three constants define how the correlator works
const int readingsPerPowerPeriod = 8;	// sensor readings per power period
const int powerFrequency = 50;		// power line frequency
const int blinkLengthMs = 20;		// blink length is 20 ms

// Pins usage
const int internalLED = 9;		// WiFi built-in yellow miniled
const int resetPin = A5;
const int debugLED = 2;

/************* Internal constants ***************/

// Basic constants for readings: interval from one reading to the next and
// length of the energy meter blink in number of readings
const int readingIntvUs = (1000000l / readingsPerPowerPeriod) / powerFrequency;
const int blinkWidth = (1000l * blinkLengthMs) / readingIntvUs;

const int maxint = 0x7FFF;		// maximum positive value for int
const unsigned maxuint = 0xFFFF;	// maximum positive value for uint

/********************* Data types **************/

typedef unsigned long millis_t;		// for storing milliseconds
typedef unsigned long time_t;		// for storing Unix time

enum FifoSize { singleton=1, pair=2, quartet=4, octet=8, normal=16, large=32, big=64, huge=128 };
template<typename T, FifoSize N> class Fifo
{
 public:
  Fifo() { flush(); }
  ~Fifo() { delete data; }
  bool put(T const &tp) {
    if (count == N) return false;
    byte front = (tail + count++) & (N-1); // idx of youngest element
    data[front] = tp;
    return true;
  }
  bool get(T &tp) {
    if (count == 0) return false;
    tp = data[tail];
    tail = (tail + 1) & (N-1), count -= 1; // idx of oldest element
    return true;
  }
  const byte& numel() const { return count; }
  void flush() { tail = count = 0; }
 private:
  const T& peek() const { return data[tail]; }
  byte count;				// number of elements stored
  byte tail;				// index to oldest element
  T data[N];				// the FIFO ring
};

class AnalogSensor
{
 public:
  AnalogSensor (byte pin);
  virtual void reset () {}
  virtual void prepareRead ();
  virtual void readSensor (byte prescaler = 7);
  virtual void publishReading () {}
  virtual void debug (char command = 'n', int arg = 0);

  const byte pin;

 //protected:
  int val;					// read value
};

class InternalTempSensor : public AnalogSensor
{
public:
  InternalTempSensor (byte pin) : AnalogSensor(8) {}
  void debug (char command = 'n', int arg = 0);
};

class BlinkSensor : public AnalogSensor
{
 public:
  BlinkSensor (byte pin);
  ~BlinkSensor ();
  void reset ();
  void prepareRead ();
  void readSensor (byte prescaler = 3);
  void publishReading ();
  void debug (char command = 'n', int arg = 0);

 private:
  byte readVal;					// read value
  unsigned valRSum;				// running sum of readings
  unsigned long valRSumSq;			// running sum of squared readings
  static const int corLen = 90;			// assert(corLen < 256-corLen/2-blinkWidth/2)
  byte corWind[corLen];				// correlation window
  byte corIdx;					// corIdx wraps around at corLen
  byte cor;					// 256 * square correlation
  unsigned var;					// 32 * running variance / lowvar
  static const int protoNum = 3;		// number of blink prototypes
  static unsigned long protoMult[protoNum]; // corLen divided by squared sum of prototypes
  static byte *proto;				// blink prototypes, each terminated by 0
  static const signed char lowBlink = -1;			    // prototype floor
  static const signed char highMinusLowBlink = corLen / blinkWidth; // prototype ceil
  void initProto ();						    // init prototypes
  int covariance (byte);					    // compute covariance

  byte blinkCor;
  unsigned blinkVar;
  unsigned blinkVarEnvelope;
  millis_t blinkTimeMs, prevBlinkTimeMs;
};

template <byte bufsz>
struct buffer
{
  char len[4];
  byte buf[bufsz+7];
  byte bufidx;
  byte tlen;
  bool chunked;
  void putCh (Print& stream, byte b)
  {
    buf[bufidx] = b;
    if (++bufidx == bufsz)
      push(stream);
  }
  void push (Print& stream)
  {
    if (chunked)
      {
	itoa(bufidx, len, 16);
	byte *trail = buf+bufidx;
	trail[0] = trail[3] = trail[5] = len[2] = '\r';
	trail[1] = trail[4] = trail[6] = len[3] = '\n';
	trail[2] = '0';
	stream.write((byte *)len, 4+bufidx+tlen);
      }
    else
      stream.write(buf, bufidx);
    yield();
    bufidx = 0;
  }
};

/////////////////////// Allocate buffer for Stream buffered output //////////////
buffer<64> b;

inline Print& operator+ (Print& stream, char c)
{
  b.putCh(stream, c);
  return stream;
}

Print& operator+ (Print& stream, char *cp)
{
  while (byte c = *cp++)
    b.putCh(stream, c);
  return stream;
}

inline Print& operator+ (Print& stream, const char *ccp)
{
  return stream + const_cast<char *>(ccp);
}

Print& operator+ (Print& stream, const __FlashStringHelper *flashp)
{
  char *p = (char *)flashp;
  while (byte c = pgm_read_byte(p++))
    b.putCh(stream, c);
  return stream;
}

inline Print& operator+ (Print& stream, int i)
{
  char a[7];
  return stream + itoa(i, a, 10);
}

inline Print& operator+ (Print& stream, unsigned long ul)
{
  char a[11];
  return stream + ultoa(ul, a, 10);
}

// Hex, it's faster
inline Print& operator+ (Print& stream, unsigned long long ull)
{
  byte *ullp = (byte *)&ull;
  byte *bp = ullp + sizeof(ull) - 1;
  while (*bp == 0 && bp > ullp)
    bp -= 1;
  stream + '0' + 'x';
  do
    for (int s = 4; s >= 0; s -= 4)
      {
	byte n = (*bp >> s) & 0x0f;
	if (n < 10)
	  stream + char('0' + n);
	else
	  stream + char('A' + n - 10);
      }
  while (bp-- > ullp);
  return stream;
}

#ifndef ARDUINO_STREAMING
enum _EndLineCode { endl };
#endif
inline Print& operator+ (Print &stream, _EndLineCode arg)
{
  return stream + "\r\n";
}

enum _Push { chunk, push, clear };
inline Print& operator+ (Print &stream, _Push arg)
{
  if (arg == clear)
    b.bufidx = 0;
  else
    {
      b.tlen = 7;
      b.push(stream);
      b.tlen = 2;
      b.chunked = (arg == chunk);
    }
  return stream;
}


// Local Variables:
// fill-column: 100
// c-macro-cppflags: "-include Arduino.h -MMD -mmcu=atmega328p -DF_CPU=16000000L -DARDUINO=154 -I. -I../arduino-1.5.4/hardware/arduino/cores/arduino -I../arduino-1.5.4/hardware/arduino/variants/standard -I../arduino-1.5.4/libraries/SPI -I../arduino-1.5.4/libraries/WiFi -I../arduino-1.5.4/libraries/SD -I../arduino-1.5.4/libraries/SPI -I../arduino-1.5.4/libraries/WiFi/utility -I../arduino-1.5.4/libraries/SD/utility -I/home/pot/src/arduino/libraries/Streaming -I/home/pot/src/arduino/libraries/SchedulerARMAVR"
// End: