const byte ShiftEnable = 4;             // Every time you use variable instead of constant and int instead of byte, God kills a kitten and wastes 2 B of memory.
const byte ShiftSerIn = 2;
const byte ShiftClear = 3;
const byte ShiftClock = 5;
const byte ShiftCommit = 6;

unsigned long CurrTime = 0;            // Hmm, my first 4 B of memory I've allocated out of that 1 KiB.
unsigned long NextEnvelopeSwitch = 0;
boolean EnvelopeStatus = false;        // Starting with motor down.
byte CurrMotor = 0;
boolean ExternalControl = false;       // Let's assume this is primarily a compass.
unsigned int PauseDuration = 500;
unsigned int DutyDuration = 100;       // Some defaults if we fail to read config from EEPROM…
/* Well, why this is actually NOT an array with separate config for each motor?
 During some experiments on our volunteers (well, actually it was an experiment on me),
 I found out, an user's brain get used very quickly (in about a minute) on a periodic
 signal and when you change the period (even a bit - could be 20 ms or around 3 %),
 the feeling is really weird.
 Of course it's cool to test it in a laboratory (I suggest testing with ECG attached;
 I bet there would be at least 5/min rise), where you control the anklet from a computer.
 But it's absolutely impossible to go with it in the real life. Yes, you could be
 (and probably are) more psychically resistent than me, but I think it would be
 strange on everybody.
 And, of course, we save 28 B of our little memory ;-)
 */
byte MotorIntensity[] = {
  60, 60, 60, 60, 60, 60, 60, 60};
/* It's good time to mentoin physical construction.
 Motors are counted from 0 to 7 or from North (N) to NorthWest (NW) clockwise.
 
 And about the intensity.
 0 should be off (to conserver power).
 1 should be the lowest perceptable boundary.
 255 should be the boundary where the motors start burning.
 But currently this is broken, we have to do some measurements on volunteers
 (again me…) and real velcro paw and I don't have required hardware at home as of
 I'm writing this code.
 Finally, there should be a curve (and it could be non-linear) stored in EEPROM
 describing this.
 */
byte MotorMinPWM = 150; // When they start burning
byte MotorMaxPWM = 220; // Unfeelable
byte pwm;
int bearing = 0;   // I've totally resignated on saving 1 B per angle and computing it from byte.
int MotorStart[] = {
  360-22.5, 22.5, 3*22.5, 5*22.5, 7*22.5, 9*22.5, 11*22.5, 13*22.5};
// Again, some defaults, should be stored in EEPROM.
// Notice one motor at a time.

boolean KeyboardControl = false;
boolean BinaryControl = false;

byte packet = 0;  // Packet to send to the shift register.
byte ManualIntensity = 60;

char c;
String cmd;

void setup() {
  // TODO set compass to 5Hz mode
  pinMode(ShiftEnable, OUTPUT);
  pinMode(ShiftSerIn, OUTPUT);
  pinMode(ShiftClear, OUTPUT);
  pinMode(ShiftClock, OUTPUT);
  pinMode(ShiftCommit, OUTPUT);
  // TODO read config from EEPROM

  digitalWrite(ShiftEnable, HIGH);      // disable shift register output
  digitalWrite(ShiftClear, LOW);        // zero the buffer
  digitalWrite(ShiftCommit, LOW);
  digitalWrite(ShiftClock, LOW);        // the TPIC6 clocks work on a rising edge, so make sure they're low to start.
  digitalWrite(ShiftClear, HIGH);       // stop clearing the buffer

  Serial.begin(9600);
  Serial.println("Console ready, send command or ? for help.");
}

void loop() {
  // TODO ReadCompass
  // TODO FadeIntensity
  ChangeMotor();
  ManageEnvelope();
  ServeConsole();
}


void ManageEnvelope() {
  /* Yo we herd you like rectangle wawes, so we put rectangle wave into a rectangle envelope,
   so u can generate rectangle wave while u are generating rectangle wave! */

  CurrTime = millis();
  if (NextEnvelopeSwitch <= CurrTime) {
    // Yea, we have to switch the state!
    if (EnvelopeStatus) {
      // Motor is ON, so we will turn it OFF
      NextEnvelopeSwitch = CurrTime + PauseDuration;
    } 
    else {
      // Motor is OFF, so we will turn it ON, what's the problem?! (--Moss, The IT Crowd)
      NextEnvelopeSwitch = CurrTime + DutyDuration;
    }
    EnvelopeStatus = !EnvelopeStatus;
    if (EnvelopeStatus) {
      if(!ExternalControl) {
        pwm = ComputePWM(MotorIntensity[CurrMotor]);
      } 
      else {
        pwm = ComputePWM(ManualIntensity);
      }
      analogWrite(ShiftEnable, pwm);
    } 
    else {
      analogWrite(ShiftEnable, 255);    // Yes, really, 255 is off. Some documentation is going to be written about this - maybe.
    }
  }
}

void ChangeMotor() {
  if(!ExternalControl) {
    byte i=0;
    for (i=0; i<8; i++) {  // Select motor.
      if (MotorStart[i] < bearing && MotorStart[i+1] > bearing) {
        CurrMotor = i;
      }
    }
    // Compute binary code
    packet = 0;
    bitWrite(packet, CurrMotor, 1);
  }
  // Pipe it into buffer
  shiftOut(ShiftSerIn, ShiftClock, LSBFIRST, packet);
  // And commit
  delayMicroseconds(100);
  digitalWrite(ShiftCommit, HIGH);
  delayMicroseconds(100);
  digitalWrite(ShiftCommit, LOW);  // 10 kHz ought to be enough…
}

byte ComputePWM(byte Intensity) {
  /*byte DutyCycle = PWMInversed;               // Yup, it's inversed. At least here.
  DutyCycle = PWMInversed - Intensity;          // Well, this is really crap.*/
  byte DutyCycle = 255;
  DutyCycle = map(Intensity, 0, 255, MotorMaxPWM, MotorMinPWM);                           
  return DutyCycle;                             // Duh.
}

void ServeConsole() {
  while(Serial.available()) {                     // Read chars from buffer.
    c = Serial.read();
    if (KeyboardControl) {
      KeyboardSwitch(c);
      return;
    }
    if (c == 10 || c == 13 || c==64) {
      ExecCommand(cmd);
      cmd="";
      return;
    }
    if (c == '-') {
      cmd="";
      Serial.println("Aborted.");
      return;
    }
    cmd+=c;
  }
  return;
}

void ExecCommand(String s) {
  char command;
  char param[5];
  byte space = s.indexOf(' ');
  command = s.charAt(0);
  
  if (space) {
    // Command with param
    String work = s.substring(2,6);
    work.toCharArray(param, sizeof(param)-1);
  } 
  else {
    byte i;
    for(i=0; i < sizeof(param); i++) {
      param[i] = 0;
    }
  }
  switch (command) {
  case 'm':
    ExternalControl = true;
    KeyboardControl = true;
    Serial.println("Entering keyboard control mode.");
    break;
  case 'b':
    ExternalControl = true;
    BinaryControl = true;
    Serial.println("Entering binary control mode. (NYI)");
    break;
  case 'B':
    bearing = atoi(param);
    Serial.print("Set bearing to ");
    Serial.println(bearing, DEC);
  default:
    Serial.println("WTF OMG?");
    break;
  }
  return;
}


void KeyboardSwitch(byte c) {
  switch (c) {
  case '-':
    KeyboardControl = false;
    ExternalControl = false;
    Serial.println("Exit.");
    break;
  case 'a':
    ManualIntensity+=5;
    break;
  case 'y':
    ManualIntensity-=5;
    break;
  case 's':
    PauseDuration+=10;
    break;
  case 'x':
    PauseDuration-=10;
    break;
  case 'd':
    DutyDuration+=10;
    break;
  case 'c':
    DutyDuration-=10;
    break;
  case 'q':
    SwitchBit(0);
    break;
  case 'w':
    SwitchBit(1);
    break;
  case 'e':
    SwitchBit(2);
    break;
  case 'r':
    SwitchBit(3);
    break;
  case 't':
    SwitchBit(4);
    break;
  case 'z':
    SwitchBit(5);
    break;
  case 'u':
    SwitchBit(6);
    break;
  case 'i':
    SwitchBit(7);
    break;
  default:
    Serial.println("WTF?");
    break;
  }
  Serial.print("Running at intensity ");
  Serial.print(ManualIntensity, DEC);
  Serial.print(" (PWM ");
  Serial.print(pwm, DEC);
  Serial.print("), pause ");
  Serial.print(PauseDuration, DEC);
  Serial.print(" ms, pulse ");
  Serial.print(DutyDuration, DEC);
  Serial.print(" ms, motors ");
  Serial.println(packet, BIN);
}

void SwitchBit (byte pos) {
  if(bitRead(packet, pos)) {
    bitWrite(packet, pos, 0);
  } 
  else {
    bitWrite(packet, pos, 1);
  }
}