#include "helper.h"

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

  for(int i = 0; i < 3; i++)
    pinMode(outputPins[i], OUTPUT);
  for(int i = 0; i < 4; i++)
    pinMode(ledPins[i], OUTPUT);

  if(askState == askBatteryLife)
    displayBatteryLife();
}

void loop() {
  if(askState == askPacketEcho) {
    zeroLeds();
    setLed(1, Serial.available() > 0 ? 1 : 0); // red when receiving data
  }
  while(Serial.available() > 0) {    
    packet[packetPosition] = Serial.read();
    packetPosition = (packetPosition + 1) % packetSize;
    sync();
    if(packetPosition == 0)
      processPacket();
  }
  updateOutputs();
}

void sync() {
  if(
  packet[0] == nullPacket[2] &&
    packet[1] == nullPacket[0] &&
    packet[2] == nullPacket[1]) {
    resetPacket();
    packetPosition = (packetPosition + 2) % packetSize;
    noteSync(false);
    if(askState == askPacketEcho)
      setLed(3, 1);
  } 
  else if(
  packet[0] == nullPacket[1] &&
    packet[1] == nullPacket[2] &&
    packet[2] == nullPacket[0]) {
    resetPacket();
    packetPosition = (packetPosition + 1) % packetSize;
    noteSync(false);
    if(askState == askPacketEcho)
      setLed(3, 1);
  } 
  else if(isNullPacket()) {
    noteSync(true);
    if(askState == askPacketEcho)
      setLed(3, 0);
  }
}

void noteSync(boolean inSync) {
  if(qualityNote[qualityPosition])
    quality--;
  qualityNote[qualityPosition] = inSync;
  if(inSync)
    quality++;
  qualityPosition = (qualityPosition + 1) % qualityWindow;
}

void resetPacket() {
  packet[0] = nullPacket[0];
  packet[1] = nullPacket[1];
  packet[2] = nullPacket[2];
}

boolean isNullPacket() {
  return
    packet[0] == nullPacket[0] &&
    packet[1] == nullPacket[1] &&
    packet[2] == nullPacket[2];
}

void processPacket() {
  if(askState == askSignalQuality)
    displayLedBar(quality == 0 ? 0 : (float) quality / (float) qualityWindow);
  if(!isNullPacket() && (packet[0] == 255 || packet[0] == address || inGroup(254 - packet[0]))) {
    if(askState == askPacketEcho)
      setLed(2, 1); // green when a packet is received for this vibrobyte
    int mode = (packet[1] & modeMask) >> modeShift;
    switch(mode) {
    case outputMode: 
      {
        int wave = (packet[1] & waveMask) >> waveShift;
        float amplitude = grab(packet[1], amplitudeMask, amplitudeShift, amplitudeRange, lowAmplitude, highAmplitude);
        float sustain = grab(packet[1], sustainMask, sustainShift, sustainRange, lowSustain, highSustain);
        float frequency = grab(packet[2], frequencyMask, frequencyShift, frequencyRange, lowFrequency, highFrequency);
        int outputs = select(packet[2], outputsMask, outputsShift);
        for(int i = 0; i < 3; i++) {
          if(outputs & (B100 >> i)) {
            waves[i] = wave;
            amplitudes[i] = amplitude;
            frequencies[i] = frequency;
            sustains[i] = sustain;
            if(outputModeState == outputSingle)
              counters[i] = 0;
          }
        }
      }
      break;

    case ledMode: 
      {
        leds[0] = grab(packet[1], irMask, irShift, irMax);
        leds[1] = grab(packet[1], redMask, redShift, rgbMax);
        leds[2] = grab(packet[2], greenMask, greenShift, rgbMax);
        leds[3] = grab(packet[2], blueMask, blueShift, rgbMax);
        updateLeds();
      }
      break;

    case askMode: 
      {
        switch(packet[2]) {
        case askBatteryLife:  
          {
            askState = askBatteryLife;
            displayBatteryLife();
          }
          break;

        case askSignalQuality:  
          {
            askState = askSignalQuality;
          }
          break;

        case askPacketEcho: 
          {
            askState = askPacketEcho;
          }
          break;

        case askOutputEcho: 
          {
            askState = askOutputEcho;
          }
          break;
        }
      }
      break;

    case reprogramMode: 
      {
        int key = select(packet[1], keyMask, keyShift);
        int value = packet[2];
        switch(key) {
        case reOutputMode: 
          {
            outputModeState = value;
          }
          break;

        case reAddGroup:
          groups = groups | (1 << value);
          break;

        case reRemoveGroup:
          groups = groups & ~(1 << value);
          break;

        case reResetGroup:
          groups = 0;
          break;
        }
      }
      break;
    }
  } 
  else {
    if(askState == askPacketEcho)
      setLed(2, 0);
  }
}

boolean inGroup(int group) {
  return groups & (1 << group);
}

void displayBatteryLife() {
  displayLedBar((float) analogRead(1) / 1024.);
}

// output functions

float lastTime = 0;
void updateOutputs() {
  float time = (float) millis() / 1000.;
  float timeDiff = time - lastTime;
  float wave;
  for(int i = 0; i < 3; i++) {
    if(outputModeState == outputOff) {
      digitalWrite(outputPins[i], LOW);
    } 
    else {
      counters[i] += timeDiff * frequencies[i];
      if(outputModeState == outputRepeat)
        counters[i] = mod(counters[i], 1.);
      float value = amplitudes[i] * getWave(waves[i], counters[i], sustains[i]);
      analogWrite(outputPins[i], 255. * value);
      if(askState == askOutputEcho)
        setLed(i + 1, value);
    }
  }
  lastTime = time;
}

float getWave(int wave, float c, float s) {
  if(c < s) {
    switch(wave) {
    case 0: 
      return 1; // square
    case 1: 
      return c / s; // saw ascending
    case 2: 
      return 1 - (c / s); // saw descending
    default: 
      return sin(PI * (c / s)); // absolute sine
    }
  } 
  else {
    return 0;
  }
}

// led functions

/*
  takes a float from 0-1 and displays it on the LEDs
 1/3 will light red
 5/6 will light red and green and half-light blue
 */
void displayLedBar(float value) {
  zeroLeds();
  for(int i = 0; i < 3; i++) {
    if(value > oneThird) {
      leds[i + 1] = 1;
      value -= oneThird;
    } 
    else {
      leds[i + 1] = value * 3;
      break;
    }
  }
  updateLeds();
}

void zeroLeds() {
  for(int i = 0; i < allLeds; i++)
    leds[i] = 0;
}

void updateLeds() {
  digitalWrite(ledPins[0], leds[0]);
  for(int i = 1; i < allLeds; i++)
    setLed(i, leds[i]);
}

void setLed(int led, float pwm) {
  if(pwm == 0)
    digitalWrite(ledPins[led], LOW);
  else
    analogWrite(ledPins[led], pwm * ledIntensity[led]);
}

// aux

float mod(float x, float y) {
  return x - ((int) (x / y) * y);
}

float grab(int packet, int mask, int shift, float max) {
  return ((float) select(packet, mask, shift)) / max;
}

float grab(int packet, int mask, int shift, float range, float low, float high) {
  return map((float) select(packet, mask, shift) / range, low, high);
}

int select(int packet, int mask, int shift) {
  return (packet & mask) >> shift;
}

float map(float input, float low, float high) {
  return input * (high - low) + low;
}