#include <Servo.h> 

Servo servos[14];
uint8_t pinModes[32];

unsigned int crc;
unsigned int command;
unsigned int channel;
unsigned int streamAnalog;
unsigned int value;
unsigned long lvalue;
unsigned long streamDigital;
unsigned long lastDigital;
unsigned int lastAnalog[16];
unsigned long streamEncoder;
unsigned long lastLeftEncoder;
unsigned long lastRightEncoder;
int lastEncoderSend=0;
unsigned int heartBeat=0;
unsigned int defaultServo[14];

#define ENCODER_2_A 18
#define ENCODER_2_B 19
#define ENCODER_1_A 20
#define ENCODER_1_B 21
#define ENC_PORT PIND

#define ARDUINO_GET_ID 0
#define ARDUINO_SET_SERVO 1
#define ARDUINO_SET_DIGITAL_STREAM 2
#define ARDUINO_SET_DIGITAL_HIGH 3
#define ARDUINO_SET_DIGITAL_LOW 4
#define ARDUINO_SET_ANALOG_STREAM 5
#define ARDUINO_DIGITAL_STREAM 6
#define ARDUINO_ANALOG_STREAM 7
#define ARDUINO_SET_ENCODER_STREAM 8
#define ARDUINO_ENCODER_STREAM 9
#define ARDUINO_HEARTBEAT 10
#define ARDUINO_SET_ANALOG 11
#define ARDUINO_SET_SERVO_DEFAULT 12

volatile unsigned int encoder1Counter = 0;
volatile unsigned int encoder2Counter = 0;

void initialize()
{
  int i;
  for (i=0;i<32;i++) pinModes[i]=-1;
  streamDigital=0;
  streamAnalog=0;
  streamEncoder=0;
  lastLeftEncoder=0;
  lastRightEncoder=0;
  lastDigital=-1;
  for (i=0;i<16;i++) lastAnalog[i]=-1;
  for (i=0;i<14;i++) defaultServo[i]=1500;
}

/* returns change in encoder state (-1,0,1) Based on http://www.circuitsathome.com/mcu/programming/reading-rotary-encoder-on-arduino */
void doEncoder1()
{
  static int8_t enc_states[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
  static uint8_t old_AB = 0;
  int new_AB = ENC_PORT;

  old_AB <<= 2;
  //old_AB |= ( ENC_PORT & 0x12 ); //add current state

  old_AB |= ( new_AB & 0x03 ); //add current state
  encoder1Counter += ( enc_states[( old_AB & 0x0f )]);
}

void doEncoder2()
{
  static int8_t enc_states[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
  static uint8_t old_AB = 0;
  int new_AB = ENC_PORT;

  old_AB <<= 2;
  //old_AB |= ( ENC_PORT & 0x12 ); //add current state

  old_AB |= ( (new_AB & 12)>>2 ); //add current state
  encoder2Counter += ( enc_states[( old_AB & 0x0f )]);
}

void setupEncoders()
{
  /* Setup encoder pins as inputs */
  pinMode(ENCODER_1_A, INPUT);
  digitalWrite(ENCODER_1_A, HIGH);
  pinMode(ENCODER_1_B, INPUT);
  digitalWrite(ENCODER_1_B, HIGH);

  pinMode(ENCODER_2_A, INPUT);
  digitalWrite(ENCODER_2_A, HIGH);
  pinMode(ENCODER_2_B, INPUT);
  digitalWrite(ENCODER_2_B, HIGH);

  // encoder pin on interrupt 2 (pin 21)
  attachInterrupt(2, doEncoder1, CHANGE);
  // encoder pin on interrupt 3 (pin 20)
  attachInterrupt(3, doEncoder1, CHANGE);

  // encoder pin on interrupt 4 (pin 18)
  attachInterrupt(4, doEncoder2, CHANGE);
  // encoder pin on interrupt 5 (pin 19)
  attachInterrupt(5, doEncoder2, CHANGE);
}

void setdownEncoders()
{
  detachInterrupt(2);
  detachInterrupt(3);
  detachInterrupt(4);
  detachInterrupt(5);
}

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

  initialize();
}

void writePacket()
{
  unsigned char buffer[2];  
  buffer[0]=command|128;
  buffer[1]=channel;
  Serial.write(buffer, 2);
}

void writeValuePacket(int value)
{
  unsigned char buffer[5];  
  
  buffer[0]=command|128;
  buffer[1]=channel;
  buffer[2]=value&127;
  buffer[3]=(value>>7)&127;
  buffer[4]=(buffer[0]^buffer[1]^buffer[2]^buffer[3])&127;
  
  Serial.write(buffer, 5);
}

void writeTriValuePacket(unsigned long value)
{
  unsigned char buffer[6];  
  
  buffer[0]=command|128;
  buffer[1]=channel;
  buffer[2]=value&127;
  buffer[3]=(value>>7)&127;
  buffer[4]=(value>>14)&127;
  buffer[5]=(buffer[0]^buffer[1]^buffer[2]^buffer[3]^buffer[4])&127;
  
  Serial.write(buffer, 6);
}

void writeQuadValuePacket(unsigned long value)
{
  unsigned char buffer[8];  
  
  buffer[0]=command|128;
  buffer[1]=channel;
  buffer[2]=value&127;
  buffer[3]=(value>>7)&127;
  buffer[4]=(value>>14)&127;
  buffer[5]=(value>>21)&127;
  buffer[6]=(value>>28)&127;
  buffer[7]=(buffer[0]^buffer[1]^buffer[2]^buffer[3]^buffer[4]^buffer[5]^buffer[6])&127;
  
  Serial.write(buffer, 8);
}

void readPacket()
{
  // get header byte
  // 128 (bit 8) flag indicates a new command packet .. that 
  // means the value bytes can never have 128 set!
  // next 7 bits are the command 0-7
  // next byte is the channel 0-32
  do
  {
    while (Serial.available() <= 0) continue; 
    command = Serial.read();
  }
  while ((command&128)==0);
  
  command^=128;

  while (Serial.available() <= 0) continue; 
  channel = Serial.read();
}

int readValuePacket()
{
  unsigned int valueLow;
  unsigned int valueHigh;
  
  // wait for value low byte    
  while (Serial.available() <= 0) continue; 
  valueLow = Serial.read();
  if (valueLow&128) return 0;

  // wait for value high byte    
  while (Serial.available() <= 0) continue; 
  valueHigh = Serial.read();
  if (valueHigh&128) return 0;
    
  // wait for crc byte    
  while (Serial.available() <= 0) continue; 
  crc = Serial.read();
  if (crc&128) return 0;
  
  if (crc!=(((128|command)^channel^valueLow^valueHigh)&127)) return 0;

  value = valueLow|(valueHigh<<7);
  
  return 1;
}

int readTriValuePacket()
{
  unsigned long valueLow;
  unsigned long valueMed;
  unsigned long valueHigh;
  
  // wait for value low byte    
  while (Serial.available() <= 0) continue; 
  valueLow = Serial.read();
  if (valueLow&128) return 0;

  // wait for value med byte    
  while (Serial.available() <= 0) continue; 
  valueMed = Serial.read();
  if (valueMed&128) return 0;

  // wait for value high byte    
  while (Serial.available() <= 0) continue; 
  valueHigh = Serial.read();
  if (valueHigh&128) return 0;
    
  // wait for crc byte    
  while (Serial.available() <= 0) continue; 
  crc = Serial.read();
  if (crc&128) return 0;
  
  if (crc!=(((128|command)^channel^valueLow^valueMed^valueHigh)&127)) return 0;

  lvalue = valueLow|(valueMed<<7)|(valueHigh<<14);
  
  return 1;
}

int readQuadValuePacket()
{
  unsigned long value1;
  unsigned long value2;
  unsigned long value3;
  unsigned long value4;
  unsigned long value5;
  
  // wait for value 1 byte    
  while (Serial.available() <= 0) continue; 
  value1 = Serial.read();
  if (value1&128) return 0;

  // wait for value 2 byte    
  while (Serial.available() <= 0) continue; 
  value2 = Serial.read();
  if (value2&128) return 0;
    
  // wait for value 3 byte    
  while (Serial.available() <= 0) continue; 
  value3 = Serial.read();
  if (value3&128) return 0;
    
  // wait for value 4 byte    
  while (Serial.available() <= 0) continue; 
  value4 = Serial.read();
  if (value4&128) return 0;
    
  // wait for value 5 byte    
  while (Serial.available() <= 0) continue; 
  value5 = Serial.read();
  if (value5&128) return 0;
    
  // wait for crc byte    
  while (Serial.available() <= 0) continue; 
  crc = Serial.read();
  if (crc&128) return 0;
  
  if (crc!=(((128|command)^channel^value1^value2^value3^value4^value5)&127)) return 0;

  lvalue = value1|(value2<<7)|(value3<<14)|(value4<<21)|(value5<<28);
  
  return 1;
}

void loop() 
{
/*  
  Serial.print(encoder1Counter);
  Serial.print(":");
  Serial.println(encoder2Counter);
  return;
*/
  while (Serial.available()>0)
  {
    readPacket();
    
    heartBeat=0;
    
    switch (command)
    {
      // init
      case  ARDUINO_GET_ID:
        initialize();
        Serial.print("ARDU");
      break;
      // servo
      case  ARDUINO_SET_SERVO:
        if ((channel>=0)&&(channel<14))
        {
          if (readValuePacket())
          {
            if (pinModes[channel]!=1)
            {
              servos[channel].attach(channel);
              pinModes[channel]=1;
            }
            servos[channel].writeMicroseconds(value);
            writeValuePacket(value);
          }
        }
      break;
      //digital stream
      case  ARDUINO_SET_DIGITAL_STREAM:
        if (readQuadValuePacket())
        {
          streamDigital = lvalue;
          writeQuadValuePacket(lvalue);
          lastDigital=-1;
        }
      break;
      //set digital high
      case  ARDUINO_SET_DIGITAL_HIGH:
        if ((channel>=0)&&(channel<32))
        {
          if (pinModes[channel]!=2)
          {
            if (pinModes[channel]==1)
              servos[channel].detach();

            pinMode(channel+22, OUTPUT);
            pinModes[channel]=2;
            if (streamDigital&((unsigned long)1<<channel))
              streamDigital^=(unsigned long)1<<channel;
          }
          
          digitalWrite(channel+22, HIGH);
          writePacket();
        }
      break;
      //set digital low
      case  ARDUINO_SET_DIGITAL_LOW:
        if ((channel>=0)&&(channel<32))
        {
          if (pinModes[channel]!=2)
          {
            if (pinModes[channel]==1)
              servos[channel].detach();

            pinMode(channel+22, OUTPUT);
            pinModes[channel]=2;
            if (streamDigital&((unsigned long)1<<channel))
              streamDigital^=(unsigned long)1<<channel;
          }
          digitalWrite(channel+22, LOW);
          writePacket();
        }
      break;
      //analog stream
      case  ARDUINO_SET_ANALOG_STREAM:
        if (readTriValuePacket())
        {
          streamAnalog = lvalue;
          writeTriValuePacket(lvalue);
          for (channel=0;channel<16;channel++) lastAnalog[channel]=-1;
        }
      break;
      case ARDUINO_SET_ENCODER_STREAM:
        streamEncoder = channel;
        if (streamEncoder)
          setupEncoders();
        else
          setdownEncoders();
        writePacket();
      break;
      case  ARDUINO_SET_ANALOG:
        if (readValuePacket())
        {
					if ((channel>=2)&&(channel<=14))
					{
						if (pinModes[channel]!=2)
						{
							if (pinModes[channel]==1)
								servos[channel].detach();

							pinMode(channel, OUTPUT);
							pinModes[channel]=2;
						}
						analogWrite(channel, value);
						writeValuePacket(value);
					}
        }
      break;
      case ARDUINO_HEARTBEAT:
      break;
      case ARDUINO_SET_SERVO_DEFAULT:
        if ((channel>=0)&&(channel<14))
        {
          if (readValuePacket())
          {
            defaultServo[channel] = value;
						writeValuePacket(value);
          }
        }
      break;
    }
  }
  
  if ((heartBeat++)>30000)
  {
    int i;
    for (i=0;i<14;i++)
    {
      if (pinModes[i]==1) 
        servos[i].writeMicroseconds(defaultServo[i]);
      else
      if (pinModes[i]==2) 
        analogWrite(i, 0);
    }
  }
  
  for (channel=0;channel<16;channel++)
  {
    if (streamAnalog&(1<<channel))
    {
      value = analogRead(channel);
      // only send value if it has changed
      if (value!=lastAnalog[channel])
      {
        command = ARDUINO_ANALOG_STREAM;
        writeValuePacket(value);
        
        lastAnalog[channel]=value;
      }
    }
  }

  if (streamEncoder)
  {
    if ((lastLeftEncoder!=(encoder1Counter>>8))||(lastRightEncoder!=(encoder2Counter>>8))||(lastEncoderSend--<0))
    {
      lastLeftEncoder=encoder1Counter;
      lastRightEncoder=encoder2Counter;

      command = ARDUINO_ENCODER_STREAM;
      channel = 0;
      writeQuadValuePacket((lastRightEncoder<<16)|lastLeftEncoder);
      
      lastLeftEncoder>>=8;
      lastRightEncoder>>=8;
      lastEncoderSend=500;
    }
  }

  if (streamDigital)
  {
    unsigned long digitalValue=0;
    for (channel=0;channel<32;channel++)
    {
      if (streamDigital&((unsigned long)1<<channel))
      {
        if (pinModes[channel]!=3)
        {
          if (pinModes[channel]==1)
            servos[channel].detach();
            
          pinMode(channel+22, INPUT);
          pinModes[channel]=3;
          // pullup
          digitalWrite(channel+22, HIGH); 
        }
        
        digitalValue |= ((unsigned long)digitalRead(channel+22))<<channel;
      }
    }

    // only send value if it has changed
    if (digitalValue!=lastDigital)
    {    
      command = ARDUINO_DIGITAL_STREAM;
      channel = 0;
      writeQuadValuePacket(digitalValue);
      lastDigital=digitalValue;
    }
  }
}