#include <MicroView.h>
#include <Brain.h>

#include <Wire.h>

#define MK_FREQ 49600L // Set clock to 50kHz (actualy 49.6kHz seems to work better)
#define SOUND (byte)0x52  // Sound controller (device ID 82).  Write to 0xA4, read from 0xA5.
#define BUTTON (byte)0x50 // Button controller (device ID 80). Write to 0xA0, read from 0xA1.
#define MOTOR (byte)0x55  // Motor controller (device ID 85).  Write to 0xAA, read from 0xAB.

class Keepon {
 private:
  struct Command {
    byte device;
    byte command;
    byte arg;
  };
  static const byte qLen = 10;
  static const byte maxAttempts = 50;
  Command queue[qLen];
  byte qHead,qTail;
  byte attempts;

  bool queue_empty() {
    return qHead == qTail;
  }
  bool queue_full() {
    return ((qHead+qLen-1)%qLen) == qTail;
  }
  Command* queue_current() {
    return &queue[qHead];
  }
  void queue_remove() {
    if (queue_empty()) return;
    qHead++;
    qHead %= qLen;
  }
  void queue_add(byte d,byte c, byte a) {
    if (queue_full()) return;
    
    queue[qTail].device=d;
    queue[qTail].command=c;
    queue[qTail].arg=a;

    qTail++;
    qTail %= qLen;
  }

 public:
  Keepon() {
    qHead = 0;
    qTail = 0;
    attempts = 0;
  }
  void begin() {
    pinMode(SDA, OUTPUT); // Data wire on My Keepon
    pinMode(SCL, OUTPUT); // Clock wire on My Keepon
    digitalWrite(SDA, LOW);
    digitalWrite(SCL, LOW);
  }
  void bootup(void(*cb)(bool)) {
    digitalWrite(SDA, LOW);
    digitalWrite(SCL, LOW);
    cb(false);
    while (analogRead(0) < 512); // Wait until we see voltage on A0 pin
    cb(true);
    delay(1000);
    Wire.begin();
    TWBR = ((F_CPU / MK_FREQ) - 16) / 2;
  }
  void send() {
    if (attempts > maxAttempts) {
      attempts = 0;
      queue_remove();
    }
    if (queue_empty()) return;

    Command* current = queue_current();
    Wire.beginTransmission(current->device);
    Wire.write(current->command);
    Wire.write(current->arg);
    int result = (int)Wire.endTransmission();
    if (result == 0) {
      queue_remove();
      attempts = 0;
    }
    else {
      attempts++;
    }
  }

  void pan(byte pos) {
    queue_add(MOTOR,4,pos);
  }
  void tilt(byte pos) {
    queue_add(MOTOR,2,pos);
  }
};

Brain brain(Serial);
Keepon keepon;

void kscan_callback(bool found) {
  if (not found) {
    uView.clear(PAGE);
    uView.setCursor(0,0);uView.println("K?");
    uView.display();
  }
  else {
    uView.clear(PAGE);
    uView.setCursor(0,0);uView.println("K!");
    uView.display();
  }
}

void setup() {
  Serial.begin(9600);

  uView.begin();
  uView.clear(ALL);
  uView.setFontType(0);

  keepon.begin();
  keepon.bootup(&kscan_callback);
}

const int hist_width=3;
const int hist_pad=1;
int samples_delay;

void loop() {
  byte attention, meditation, quality;
  if (brain.update()) {
    uView.clear(PAGE);

    uView.setCursor(0,0); uView.print(quality=brain.readSignalQuality());

    uView.setCursor(0,9); uView.print(attention=brain.readAttention());
    uView.setCursor(18,9); uView.print(meditation=brain.readMeditation());

    const uint32_t* power = brain.readPowerArray();
    uint32_t max_value=0;
    for (int x=0;x<8;++x) {
      if (power[x] > max_value) {
        max_value = power[x];
      }
    }
    
    for (int x=0;x<8;++x) {
      int x0 = (hist_width + hist_pad) * x;
      // we have 30 vertical pixels
      int height = power[x] * 30 / max_value;
      for (int o=0;o<hist_width;++o) {
        uView.lineV(x0+o,47-height,height);
      }
    }
    if (quality < 10 && ++samples_delay > 5) {
      keepon.pan(attention);
      keepon.tilt(meditation);
      samples_delay=0;

      uView.setCursor(5,0);uView.print("sent");
    }
    
    uView.display();
  }
  keepon.send();
}