#include "inout.h"
#include <EEPROM.h>

// external connections
const int buttonCalib = 2;
const int ledChecksumError = 8;
const int ledSequenceError = 7;
const int ledHeading1 = 9;
const int ledHeading2 = 10;
const int pgaData = 11;
const int pgaClock = 12;
const int pgaSelect = 13;

// configuration constants
const int baudRate = 19200;
const int buttonCalibDelay = 1000;
const int ledErrorDelay = 300;
const int maxLevel = 127;
const int openAngle = 36; // half of opening angle w/o xfade
const int maxLevelLedPwm = 192;

// equal power xfade attenuation lookup table in pga2311 steps (0.5db)
// 2 * 20 * log10(cos(0.5 * M_PI * (N / 100))) for 100 >= N >= 0
const int xfade[101] = { -649, -72, -60, -53, -48, -44, -41, -38, -36, -34,
  -32, -31, -29, -28, -26, -25, -24, -23, -22, -21,
  -20, -20, -19, -18, -17, -17, -16, -15, -15, -14,
  -14, -13, -13, -12, -12, -11, -11, -10, -10, -10,
  -9, -9, -9, -8, -8, -7, -7, -7, -7, -6,
  -6, -6, -5, -5, -5, -5, -5, -4, -4, -4,
  -4, -3, -3, -3, -3, -3, -3, -2, -2, -2,
  -2, -2, -2, -2, -1, -1, -1, -1, -1, -1,
  -1, -1, -1, -1, -1, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

// global vars
angle_t heading;
angle_t headingCorr;
int head, headAbs;
sequence_t seq;
unsigned char checksum;
unsigned short int lastSeq = 0;
unsigned int lastButtonCalib = 0;
unsigned int lastChecksumError = 0;
unsigned int lastSequenceError = 0;


void setup () {
  
  int i;
  byte check = 0xff;

  // init pins
  pinMode(buttonCalib, INPUT);
  digitalWrite(buttonCalib, HIGH); // enable pull-up resistor
  pinMode(ledChecksumError, OUTPUT);
  digitalWrite(ledChecksumError, HIGH); // start enabled for power up ctrl
  pinMode(ledSequenceError, OUTPUT);
  digitalWrite(ledSequenceError, HIGH); // start enabled for power up ctrl
  pinMode(ledHeading1, OUTPUT);
  digitalWrite(ledHeading1, LOW);
  pinMode(ledHeading2, OUTPUT);
  digitalWrite(ledHeading2, LOW);
  pinMode(pgaData, OUTPUT);
  digitalWrite(pgaData, HIGH);
  pinMode(pgaClock, OUTPUT);
  digitalWrite(pgaClock, HIGH);
  pinMode(pgaSelect, OUTPUT);
  digitalWrite(pgaSelect, HIGH);

  // init level
  // start with channels 1-2 open in case tracking does not work
  pgaSetLevel(maxLevel, maxLevel, 0, 0);  
  
  // read correction heading
  for (i = 0; i < 4; ++i) {
    headingCorr.bytes[i] = EEPROM.read(i);
    check &= headingCorr.bytes[i];    
  }
  if (check == 0xff) {
    headingCorr.f = 0.0f;
  }

  Serial.begin(baudRate);
} // setup


void loop () {

  int i;
  int now = millis();
  int fadePos1, fadePos2;
  byte level1, level2, level3, level4;
  byte ledHeading1Pwm, ledHeading2Pwm;
  
  // check for timeouts
  if (now - lastChecksumError > ledErrorDelay) {
    digitalWrite(ledChecksumError, LOW);
  }
  if (now - lastSequenceError > ledErrorDelay) {
    digitalWrite(ledSequenceError, LOW);
  }
  
  // check for front calibration button
  if (digitalRead(buttonCalib) == LOW && now - lastButtonCalib > buttonCalibDelay) {
    lastButtonCalib = now;
    // write current heading as correction value to EEPROM
    headingCorr.f = heading.f;
    for (i = 0; i < 4; ++i) {
      EEPROM.write(i, headingCorr.bytes[i]);
    }
  }
  
  // receive tracking data
  if (Serial.available() >= 19) {
    // check for correct preamble, message and format (data size)
    if (Serial.read() == 250 && Serial.read() == 255 && Serial.read() == 50 && Serial.read() == 14) {
      checksum = 63; // starting point: 255 + 50 + 14 & 0xff
      // read unused x and y rotations
      for (i = 7; i >= 0; --i) {
        checksum += Serial.read();
      }
      // read heading angle
      for (i = 3; i >= 0; --i) {
        heading.bytes[i] = Serial.read();
        checksum += heading.bytes[i];
      }
      // read sequence number
      for (i = 1; i >= 0; --i) {
        seq.bytes[i] = Serial.read();
        checksum += seq.bytes[i];
      }
      // sequence test
      if (seq.i != lastSeq) {
        digitalWrite(ledSequenceError, HIGH);
        lastSequenceError = now;
        lastSeq = seq.i;        
      }
      ++lastSeq; // should wrap automatically (16-bit)
      // checksum test
      if (checksum += Serial.read()) {
        digitalWrite(ledChecksumError, HIGH);
        lastChecksumError = now;
      } else {
        // message received correctly

        // condition values
        head = (int)roundf(heading.f - headingCorr.f);
        if (head < -180) {
          head += 360;
        } else if (head > 180) {
          head -= 360;
        }
        headAbs = abs(head);

        if (headAbs < openAngle) { // front within openAngle
          level1 = maxLevel;
          level2 = maxLevel;
          level3 = 0;
          level4 = 0;
          ledHeading1Pwm = maxLevelLedPwm;
          ledHeading2Pwm = 0;
        } else if (headAbs > 180 - openAngle) { // back within openAngle
          level1 = 0;
          level2 = 0;
          level3 = maxLevel;
          level4 = maxLevel;
          ledHeading1Pwm = 0;
          ledHeading2Pwm = maxLevelLedPwm;
        } else { // outside openAngle
          // special fade
          // fade such that ear facing the source fades in earlier / fades out later
          if (head < 0) { // right of middle axis
            fadePos1 = map(headAbs, openAngle, 180 - openAngle, 0, 100);
            fadePos2 = min(100, map(headAbs, openAngle, 90, 0, 100));
          } else { // head > 0, left of middle axis
            fadePos1 = min(100, map(headAbs, openAngle, 90, 0, 100));
            fadePos2 = map(headAbs, openAngle, 180 - openAngle, 0, 100);
          }
          level1 = max(0, maxLevel + xfade[100 - fadePos1]);
          level2 = max(0, maxLevel + xfade[100 - fadePos2]);
          level3 = max(0, maxLevel + xfade[fadePos1]);
          level4 = max(0, maxLevel + xfade[fadePos2]);
          ledHeading1Pwm = map(headAbs, openAngle, 180 - openAngle, maxLevelLedPwm, 0);
          ledHeading2Pwm = maxLevelLedPwm - ledHeading1Pwm;          
        }

        // set levels and indicator LEDs
        pgaSetLevel(level1, level2, level3, level4);
        analogWrite(ledHeading1, ledHeading1Pwm);
        analogWrite(ledHeading2, ledHeading2Pwm);
      }   
    } else {
      // no/wrong message start: signal like sequence error
        digitalWrite(ledSequenceError, HIGH);
        lastSequenceError = now;
    }
  }
} // loop


void pgaSetLevel (byte level1, byte level2, byte level3, byte level4) {

  digitalWrite(pgaSelect, LOW);
  pgaSpiWrite(level1);
  pgaSpiWrite(level2);
  pgaSpiWrite(level3);
  pgaSpiWrite(level4);
  digitalWrite(pgaSelect, HIGH);
} // pgaSetLevel


void pgaSpiWrite (byte value) {

  int i;
  
  for (i = 8; i > 0; --i) {
    value <<= 1;
    digitalWrite(pgaClock, LOW);
    digitalWrite(pgaData, 0x80 & value);
    digitalWrite(pgaClock, HIGH);
  }
} // pgaSpiWrite

// EOF