//Arduino ports
#define clockpin 13 // CI
#define enablepin 10 // EI
#define latchpin 9 // LI
#define datapin 11 // DI

//Matrix dimensions
#define NROWS 4
#define NCOLS 4

//Signals to control the state machine in the Arduino part
#define HEAD_R 0x55
#define HEAD_G 0x56
#define HEAD_B 0x57
#define HEAD_ROW 0x58
#define HEAD_COL 0x59
#define HEAD_ALL 0x60
#define HEAD_DIA1 0x61
#define HEAD_DIA2 0x62
#define HEAD_MATRIX 0x63

int NumLEDs = NROWS * NCOLS;
int _leds[NROWS*NCOLS][3] = {0};
int SB_CommandMode;
int SB_RedCommand;
int SB_GreenCommand;
int SB_BlueCommand;

int _globalC = 0;
int _inc;
int loopC = 0;

//Serial stuff
int state = 0;  //0 nothing, 1 red, 2 green, 3 blue, 4 read mode
int mode = 0;
int matrixIndex = 0;
int matrixItems[16*3];

int red = 0;
int green = 0;
int blue = 0;

int iRow = 0;
int iCol = 0;
int iDia1 = 0;
int iDia2 = 0;

// current values
int iR = 0;
int iG = 0;
int iB = 0;

char incomingByte = 0;	// for incoming serial data

void setup() {

   pinMode(datapin, OUTPUT);
   pinMode(latchpin, OUTPUT);
   pinMode(enablepin, OUTPUT);
   pinMode(clockpin, OUTPUT);
   SPCR = (1<<SPE)|(1<<MSTR)|(0<<SPR1)|(0<<SPR0);
   digitalWrite(latchpin, LOW);
   digitalWrite(enablepin, LOW);

   Serial.begin(115200);	// opens serial port, sets data rate to 9600 bps

}

void loop() {
  
   if (Serial.available()>0) {
    int input = Serial.read();
    switch (state) {
      case 0:
        if (input==HEAD_R) {
          state = 1;
        }
        else if (input==HEAD_G) {
          state = 2;
        }
        else if (input==HEAD_B) {
          state = 3;
        }
        else if (input==HEAD_ROW) {
          state = 4;
        }
        else if (input==HEAD_COL) {
          state = 5;
        }
        else if (input==HEAD_DIA1) {
          state = 7;
        }
        else if (input==HEAD_DIA2) {
          state = 8;
        }else if (input==HEAD_ALL) {
          state = 0;
          updateColors();
          all(0,iR,iG,iB); 
        }else if (input==HEAD_MATRIX) {
          state = 6;
        }
        break;
      case 1:
        red = input;
        state = 0;        
        break;
      case 2:
        green = input;
        state = 0;
        break;
      case 3:
        blue = input;
        state = 0;
        break;
      case 4:
        state = 0;
        iRow = input;
        updateColors();
        all(0,0,0,0);    
        row(iRow,0,iR,iG,iB);
        break;
      case 5:
        state = 0;
        iCol = input;   
        updateColors(); 
        all(0,0,0,0);    
        col(iCol,0,iR,iG,iB);
        break;
      case 6:
        if (matrixIndex<NumLEDs*3){
           matrixItems[matrixIndex] = input;
           matrixIndex++; 
        }
       break;
      case 7:
        state = 0;
        iDia1 = input;   
        updateColors(); 
        all(0,0,0,0);    
        dia1(iDia1,0,iR,iG,iB);
        break;
      case 8:
        state = 0;
        iDia2 = input;   
        updateColors(); 
        all(0,0,0,0);    
        dia2(iDia2,0,iR,iG,iB);
       break;
    }
    if(matrixIndex == NumLEDs*3){ 
         matrixIndex = 0;
         all(0,0,0,0);  
         matrix(0);
         state = 0;
      }    
   }
}

// Sets the global values of the colors
void updateColors(){
  float fR = ((float)red/(float)255)*1023;
  float fG = ((float)green/(float)255)*1023;
  float fB = ((float)blue/(float)255)*1023;
  iR = (int)fR;
  iG = (int)fG;
  iB = (int)fB;
}

// Function belonging to the library
//------------------------------------------------------

// 0 - Wipe
void all(int d,int r,int g,int b){
  
  for (int i=0;i<NumLEDs;i++){
    _leds[i][0] = r;
    _leds[i][1] = g;
    _leds[i][2] = b;
  }

  WriteLEDArray();
  
  delay(d);
  
}

// 0 - One
void one(int i,int d,int r,int g,int b){
  
  _leds[i][0] = r;
  _leds[i][1] = g;
  _leds[i][2] = b;

  WriteLEDArray();
  
  delay(d);  
}

// 1 - Row
void row(int i,int d,int r,int g,int b){
  
  for (int j=0;j<NROWS;j++){
    _leds[i*4+j][0] = r;
    _leds[i*4+j][1] = g;
    _leds[i*4+j][2] = b;
  }

  WriteLEDArray();
  
  delay(d);  
}

// 2 - Col
void col(int i,int d,int r,int g,int b){
  
  int count = 0;
  
  for (int j=0;j<NROWS/2;j++){
    
    int first = i + j*NROWS*2;
    int second = count + (NROWS*2) - (1+i);
    
    _leds[first][0] = r;
    _leds[first][1] = g;
    _leds[first][2] = b;
    
    _leds[second][0] = r;
    _leds[second][1] = g;
    _leds[second][2] = b;
    
    count+=NROWS*2;
  }

  WriteLEDArray();
  
  delay(d);  
}

//Hardcoded method, this should be modified 
//depending on the size of the array
void dia1(int i,int d,int r,int g,int b){
     
  if (i==0){    
    _leds[i][0] = r;
    _leds[i][1] = g;
    _leds[i][2] = b;
  }else if (i==1){
    _leds[1][0] = r;
    _leds[1][1] = g;
    _leds[1][2] = b;
   
    _leds[7][0] = r;
    _leds[7][1] = g;
    _leds[7][2] = b; 
  }else if (i==2){
    _leds[2][0] = r;
    _leds[2][1] = g;
    _leds[2][2] = b;
   
    _leds[6][0] = r;
    _leds[6][1] = g;
    _leds[6][2] = b; 
    
    _leds[8][0] = r;
    _leds[8][1] = g;
    _leds[8][2] = b;
  }else if (i==3){
    _leds[3][0] = r;
    _leds[3][1] = g;
    _leds[3][2] = b;
   
    _leds[5][0] = r;
    _leds[5][1] = g;
    _leds[5][2] = b; 
    
    _leds[9][0] = r;
    _leds[9][1] = g;
    _leds[9][2] = b;
    
    _leds[15][0] = r;
    _leds[15][1] = g;
    _leds[15][2] = b; 
  }else if (i==4){
    _leds[4][0] = r;
    _leds[4][1] = g;
    _leds[4][2] = b;
   
    _leds[10][0] = r;
    _leds[10][1] = g;
    _leds[10][2] = b; 
    
    _leds[14][0] = r;
    _leds[14][1] = g;
    _leds[14][2] = b;
  }else if (i==5){
    _leds[11][0] = r;
    _leds[11][1] = g;
    _leds[11][2] = b;
   
    _leds[13][0] = r;
    _leds[13][1] = g;
    _leds[13][2] = b; 
  }else if (i==6){
    _leds[12][0] = r;
    _leds[12][1] = g;
    _leds[12][2] = b;
  }
  
  WriteLEDArray();
  
  delay(d);  
}

//Hardcoded method, this should be modified 
//depending on the size of the array
void dia2(int i,int d,int r,int g,int b){
     
  if (i==0){    
    _leds[3][0] = r;
    _leds[3][1] = g;
    _leds[3][2] = b;
  }else if (i==1){
    _leds[4][0] = r;
    _leds[4][1] = g;
    _leds[4][2] = b;
   
    _leds[2][0] = r;
    _leds[2][1] = g;
    _leds[2][2] = b; 
  }else if (i==2){
    _leds[1][0] = r;
    _leds[1][1] = g;
    _leds[1][2] = b;
   
    _leds[5][0] = r;
    _leds[5][1] = g;
    _leds[5][2] = b; 
    
    _leds[11][0] = r;
    _leds[11][1] = g;
    _leds[11][2] = b;
  }else if (i==3){
    _leds[0][0] = r;
    _leds[0][1] = g;
    _leds[0][2] = b;
   
    _leds[6][0] = r;
    _leds[6][1] = g;
    _leds[6][2] = b; 
    
    _leds[10][0] = r;
    _leds[10][1] = g;
    _leds[10][2] = b;
    
    _leds[12][0] = r;
    _leds[12][1] = g;
    _leds[12][2] = b; 
  }else if (i==4){
    _leds[7][0] = r;
    _leds[7][1] = g;
    _leds[7][2] = b;
   
    _leds[9][0] = r;
    _leds[9][1] = g;
    _leds[9][2] = b; 
    
    _leds[13][0] = r;
    _leds[13][1] = g;
    _leds[13][2] = b;
  }else if (i==5){
    _leds[8][0] = r;
    _leds[8][1] = g;
    _leds[8][2] = b;
   
    _leds[14][0] = r;
    _leds[14][1] = g;
    _leds[14][2] = b; 
  }else if (i==6){
    _leds[15][0] = r;
    _leds[15][1] = g;
    _leds[15][2] = b;
  }
  
  WriteLEDArray();
  
  delay(d);  
}

// 0 - All the matrix
void matrix(int d){
  
  for (int i=0;i<NumLEDs*3;i++){
    if (i%3 == 0)
      _leds[i/3][0] = matrixItems[i];
    else if (i%3 == 1)
      _leds[i/3][1] = matrixItems[i];
    else if (i%3 == 2)
      _leds[i/3][2] = matrixItems[i];
  }

  WriteLEDArray();
  
  delay(d);
  
}

//Shiftbrite specific functions
//------------------------------------------------------

void SB_SendPacket() {

    if (SB_CommandMode == B01) {
     SB_RedCommand = 120;
     SB_GreenCommand = 100;
     SB_BlueCommand = 100;
    }

    SPDR = SB_CommandMode << 6 | SB_BlueCommand>>4;
    while(!(SPSR & (1<<SPIF)));
    SPDR = SB_BlueCommand<<4 | SB_RedCommand>>6;
    while(!(SPSR & (1<<SPIF)));
    SPDR = SB_RedCommand << 2 | SB_GreenCommand>>8;
    while(!(SPSR & (1<<SPIF)));
    SPDR = SB_GreenCommand;
    while(!(SPSR & (1<<SPIF)));

}

void WriteLEDArray() {

    SB_CommandMode = B00; // Write to PWM control registers
    for (int h = 0;h<NumLEDs;h++) {
	  SB_RedCommand = _leds[h][0];
	  SB_GreenCommand = _leds[h][1];
	  SB_BlueCommand = _leds[h][2];
	  SB_SendPacket();
    }

    delayMicroseconds(15);
    digitalWrite(latchpin,HIGH); // latch data into registers
    delayMicroseconds(15);
    digitalWrite(latchpin,LOW);

    SB_CommandMode = B01; // Write to current control registers
    for (int z = 0; z < NumLEDs; z++) SB_SendPacket();
    delayMicroseconds(15);
    digitalWrite(latchpin,HIGH); // latch data into registers
    delayMicroseconds(15);
    digitalWrite(latchpin,LOW);

}