#include "DynamixelSerial.h"

//TLC5940NT pin definitions
#define VPRG 2
#define SIN 11
#define SCLK 13
#define XLAT 4
#define BLANK 5
#define DCPRG 6
#define GSCLK 7
uint8_t dataPin  = 8;
uint8_t latchPin = 9;
uint8_t clockPin = 10;

uint8_t val_shift; //Valor del shift register

#define LIMIT  50
#define RES 30 

#define pin_dinamyxel 3
long int VEL = 500;

int Temperature,Voltage,Position; 

long level[16] = {
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; 
  
float Sensor[6] = { 0, 0, 0, 0, 0, 0}; 
  
const int remap[16] = {
  3,4,5,6,7,8,9,10,11,12,0,1,2,13,14,15}; 
  
const int din[7] = {
  0,6,5,4,3,2,1};   
  
float MAX[4] = { 0, 0, 0, 0};
  
#define FREQ 14
#if FREQ > 1
byte int_counter = 0;
#endif


char spi_transfer(volatile byte data)
{
  SPDR = data;                    // Start the transmission
  while (!(SPSR & (1<<SPIF)))     // Wait the end of the transmission
  {
  };
  return SPDR;                    // return the received byte
}

void makemagic(){
  setGreys();       
  feedPorts();
}

void feedPorts() {
  // Clock for TLC5940's PWM
  digitalWrite(BLANK, HIGH);
  digitalWrite(BLANK, LOW);      //=all outputs ON, start PWM cycle
  for (int i=0; i<4096; i++) {
    pulseGSCLK();
  }
}

void pulseGSCLK() {
  //ultra fast pulse trick, using digitalWrite caused flickering
  PORTD |= 0x80 ; // bring pin 7 high, but don't touch any of the other pins in PORTB
  //16 nanosecs is the min pulse width for the 5940, but no pause seems needed here
  PORTD &= 0x7F;  // bring pin 7 low without touching the other pins in PORTB
}

void setGreys() {
  digitalWrite(BLANK, HIGH);
  digitalWrite(XLAT,LOW);
  for(int i = 7; i>=0; i--){
    spi_transfer( (level[2*i+1] & 0x0FF0) >> 4 );
    spi_transfer( ((level[2*i+1] & 0xF) << 4) | ((level[2*i] & 0x0F00) >> 8) );
    spi_transfer( level[2*i] & 0xFF);
  }
  digitalWrite(XLAT,HIGH);
  digitalWrite(XLAT,LOW);
  digitalWrite(BLANK, LOW);
}


void led_rgb(long int red, long int green, long int blue){
  level[remap[0]] = red; 
  level[remap[1]] = green; 
  level[remap[2]] = blue; 
}

void led_white(byte pin, long int intensity){
    if (pin < 7) level[remap[pin + 3 - 1]] = intensity; 
}

void spam_on(){
    shiftWrite(1, HIGH);
}

void spam_off(){
    shiftWrite(1, LOW); 
}

void leds_white_on(){
  led_white(1, 4095);
  led_white(2, 4095);
  led_white(3, 4095);
  led_white(4, 4095);
  led_white(5, 4095);
  led_white(6, 4095);
}

void leds_white_off(){
  led_white(1, 0);
  led_white(2, 0);
  led_white(3, 0);
  led_white(4, 0);
  led_white(5, 0);
  led_white(6, 0);
}

void Dynamixel_stop(){
  Dynamixel.turn(din[1],RIGTH,0); //0 a 1020
  Dynamixel.turn(din[2],RIGTH,0);
  Dynamixel.turn(din[3],RIGTH,0);
  Dynamixel.turn(din[4],RIGTH,0);
  Dynamixel.turn(din[5],RIGTH,0);
  Dynamixel.turn(din[6],RIGTH,0);
}

void Dynamixel_crazy(){
  Dynamixel.turn(din[1],RIGTH,map(Sensor[0], MAX[0] , LIMIT, 300, 1000)); //0 a 1020
  Dynamixel.turn(din[2],LEFT, map(Sensor[0] + Sensor[1], MAX[0] + MAX[1] , 2*LIMIT, 300, 1000));
  Dynamixel.turn(din[3],RIGTH,map(Sensor[1], MAX[1] , LIMIT, 300, 1000));
  Dynamixel.turn(din[4],LEFT, map(Sensor[2], MAX[2] , LIMIT, 300, 1000));
  Dynamixel.turn(din[5],RIGTH,map(Sensor[2] + Sensor[3], MAX[2] + MAX[3] , 2*LIMIT, 300, 1000));
  Dynamixel.turn(din[6],LEFT, map(Sensor[3], MAX[3] , LIMIT, 300, 1000));
}

float measure(int anaPin) {
  shiftWrite(6 - anaPin, HIGH);
  delay(10);
  float average = analogRead(anaPin)/2.;
  shiftWrite(6 - anaPin, LOW);
  delay(10);
  return(average*2.54);  
}

void sensor_update()
  {
    for (int i = 0; i<4; i++) { Sensor[i]=measure(i); /*if ((Sensor[i] > MAX[i])&&(Sensor[i] < 450)) MAX[i] = Sensor[i];*/} 
    Serial.begin(9600);              // Begin Serial Comunication
    for (int i = 0; i<4; i++)
    {
       Serial.print("Sensor");
       Serial.print(i);
       Serial.print(": ");
//       Serial.print(MAX[i]);
//       Serial.print(' ');
       Serial.print(Sensor[i]);
       Serial.print(", ");
    } 
    delay(100);
    Serial.println();
    delay(100);
    Dynamixel.begin(1000000,pin_dinamyxel);  // Inicialize the servo at 1Mbps and Pin Control 2
  }

void resetshift()
{
  val_shift = 0x00;
  digitalWrite(latchPin, LOW);
  shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
  digitalWrite(latchPin, HIGH);
}

void shiftWrite(uint8_t pin, boolean state)
{
  bitWrite(val_shift, pin, state);
  digitalWrite(latchPin, LOW);
  shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
  digitalWrite(latchPin, HIGH);
}

  
//The timer interrupt routine, which periodically interprets the serial commands
ISR(TIMER2_OVF_vect) {
  sei(); //Reenable global interrupts, otherwise serial commands will get dropped
#if FREQ > 1
  if(++int_counter == FREQ){ // Only do this once every FREQ-th interrupt
    int_counter = 0;
#endif //FREQ

    makemagic();
    makemagic();
    makemagic();
    makemagic();

#if FREQ > 1
  }
#endif //FREQ
}

unsigned long time0;
unsigned long time1;
unsigned long time2;
unsigned long time3;
unsigned long time4;
unsigned long time_stop;

unsigned long cnt_color_value = 0;
unsigned long cnt_color_order = 0;
char cnt_step = 1;

void setup(){
  pinMode(VPRG, OUTPUT);
  pinMode(SIN, OUTPUT);
  pinMode(SCLK, OUTPUT);
  pinMode(XLAT, OUTPUT);
  pinMode(BLANK, OUTPUT);
  pinMode(DCPRG, OUTPUT);
  pinMode(GSCLK, OUTPUT);
  pinMode(MISO, INPUT);
  pinMode(SS,OUTPUT);
  pinMode(dataPin, OUTPUT);
  pinMode(latchPin, OUTPUT);  
  pinMode(clockPin, OUTPUT); 
  digitalWrite(SS,HIGH); //disable device
  digitalWrite(SIN, LOW);
  digitalWrite(SCLK, LOW);
  digitalWrite(XLAT, LOW);
  digitalWrite(VPRG, LOW);
  digitalWrite(BLANK, HIGH);
  digitalWrite(GSCLK, HIGH);
  digitalWrite(DCPRG, LOW); // USE EEPROM DC register if LOW
  Dynamixel.begin(1000000,pin_dinamyxel);  // Inicialize the servo at 1Mbps and Pin Control 2
  delay(1000);
  for(int i=1; i<7; i++) Dynamixel.setEndless(i, ON);
  
  TCCR2A = 0;
  TCCR2B = 1<<CS22 | 0<<CS21 | 0<<CS20;

  //Timer2 Overflow Interrupt Enable
  TIMSK2 = 1<<TOIE2;
  delay(10);
  
  //Setup the Hardware SPI registers
  // SPCR = 01010000
  //interrupt disabled,spi enabled,msb 1st,master,clk low when idle,
  //sample on leading edge of clk,system clock/4 (fastest)
  byte clr;
  SPCR = (1<<SPE)|(1<<MSTR);
  clr=SPSR;
  clr=SPDR;
  delay(10);


  led_rgb(0, 4095, 0);
  
  for (int i = 0; i<4; i++)
  {
    MAX[i] = measure(i);
    if (MAX[i]>450) MAX[i] = 450;
  }

  Dynamixel_stop();
  leds_white_off();
  resetshift();
  time0 = millis();
  time1 = millis();
  time2 = millis();
  time3 = millis();
  time4 = millis();
  time_stop = millis();
  delay(2000);
//    Dynamixel.turn(din[1],LEFT,800);
//    Dynamixel.turn(din[2],LEFT,800);
//    Dynamixel.turn(din[3],LEFT,800);
//    Dynamixel.turn(din[4],LEFT,800);
//    Dynamixel.turn(din[5],LEFT,800);
//    Dynamixel.turn(din[6],LEFT,800);
//Serial.begin(9600);
leds_white_off();
spam_off();
delay(1000);
spam_on();
//Dynamixel_crazy();
//
//led_white(6, 4095);
//Dynamixel.turn(din[6],LEFT,800);

}

boolean mode0 = false;
boolean mode1 = false;
boolean mode2 = false;
boolean mode3 = false;
boolean mode4 = false;

//int cnt = 1;


void loop(){

  
  sensor_update();    
  
  if ((Sensor[0]<LIMIT)||(Sensor[1]<LIMIT)||(Sensor[2]<LIMIT)||(Sensor[3]<LIMIT)) { Dynamixel_stop(); leds_white_on(); led_rgb(4095, 4095, 4095); mode0 = false; mode1 = false; mode2 = false; mode3 = false; mode4 = false;}
  else
  {
    /*if ((Sensor[0]<(MAX[0] - RES))&&(Sensor[1]<(MAX[1] - RES))&&(Sensor[2]<(MAX[2] - RES))&&(Sensor[3]<(MAX[3] - RES)))    { mode0 = true;  time0 = millis(); Dynamixel_crazy(); leds_white_on(); led_rgb(4095, 4095, 0);}
    if ((Sensor[0]<(MAX[0] - RES))&&(Sensor[1]>=(MAX[1] - RES))&&(Sensor[2]>=(MAX[2] - RES))&&(Sensor[3]>=(MAX[3] - RES))) { mode1 = true;  time1 = millis(); Dynamixel.turn(din[1,LEFT,map(Sensor[0], MAX[0] , LIMIT, 500, 1020)); led_white(1,4095); led_rgb(0, 0, 4095);}
    if ((Sensor[0]>=(MAX[0] - RES))&&(Sensor[1]<(MAX[1] - RES))&&(Sensor[2]>=(MAX[2] - RES))&&(Sensor[3]>=(MAX[3] - RES))) { mode2 = true;  time2 = millis(); Dynamixel.turn(din[3,LEFT,map(Sensor[1], MAX[1] , LIMIT, 500, 1020)); led_white(3,4095); led_rgb(4095, 0, 4095);}
    if ((Sensor[0]>=(MAX[0] - RES))&&(Sensor[1]>=(MAX[1] - RES))&&(Sensor[2]<(MAX[2] - RES))&&(Sensor[3]>=(MAX[3] - RES))) { mode3 = true;  time3 = millis(); Dynamixel.turn(din[4,LEFT,map(Sensor[2], MAX[2] , LIMIT, 500, 1000)); led_white(4,4095); led_rgb(0, 4095, 4095);}
    if ((Sensor[0]>=(MAX[0] - RES))&&(Sensor[1]>=(MAX[1] - RES))&&(Sensor[2]>=(MAX[2] - RES))&&(Sensor[3]<(MAX[3] - RES))) { mode4 = true;  time4 = millis(); Dynamixel.turn(din[6,LEFT,map(Sensor[3], MAX[3] , LIMIT, 500, 1000)); led_white(6,4095); led_rgb(1000, 4095, 4095);}
    if (((millis()- time0)>500)&&(mode0)) { mode0 = false; leds_white_off(); Dynamixel_stop(); led_rgb(0, 4095, 0);}
    if (((millis()- time1)>500)&&(mode1)) { mode1 = false; led_white(1,0); Dynamixel.turn(din[1,LEFT,0); led_rgb(0, 4095, 0);}
    if (((millis()- time2)>500)&&(mode2)) { mode2 = false; led_white(3,0); Dynamixel.turn(din[3,LEFT,0); led_rgb(0, 4095, 0);}
    if (((millis()- time3)>500)&&(mode3)) { mode3 = false; led_white(4,0); Dynamixel.turn(din[4,LEFT,0); led_rgb(0, 4095, 0);}
    if (((millis()- time4)>500)&&(mode4)) { mode4 = false; led_white(6,0); Dynamixel.turn(din[6,LEFT,0); led_rgb(0, 4095, 0);}*/  
    
    if ((Sensor[0]<(MAX[0] - RES))&&(Sensor[1]<(MAX[1] - RES))&&(Sensor[2]<(MAX[2] - RES))&&(Sensor[3]<(MAX[3] - RES)))    { mode0 = true;  time0 = millis(); Dynamixel_crazy(); leds_white_on(); led_rgb(0, 4095, 0);}
    else{
      if (Sensor[0]<(MAX[0] - RES)) { mode1 = true;  time1 = millis(); Dynamixel.turn(din[1],LEFT,map(Sensor[0], MAX[0] , LIMIT, 500, 1020)); led_white(1,4095); led_rgb(0, 0, 4095);}
      if (Sensor[1]<(MAX[1] - RES)) { mode2 = true;  time2 = millis(); Dynamixel.turn(din[3],LEFT,map(Sensor[1], MAX[1] , LIMIT, 500, 1020)); led_white(3,4095); led_rgb(4095, 0, 4095);}
      if (Sensor[2]<(MAX[2] - RES)) { mode3 = true;  time3 = millis(); Dynamixel.turn(din[4],LEFT,map(Sensor[2], MAX[2] , LIMIT, 500, 1000)); led_white(4,4095); led_rgb(0, 4095, 4095);}
      if (Sensor[3]<(MAX[3] - RES)) { mode4 = true;  time4 = millis(); Dynamixel.turn(din[6],LEFT,map(Sensor[3], MAX[3] , LIMIT, 500, 1000)); led_white(6,4095); led_rgb(1000, 4095, 4095);}
    }  
    
      if (((millis()- time0)>500)&&(mode0)) { mode0 = false; leds_white_off(); Dynamixel_stop(); led_rgb(0, 4095 , 0);}
      if (((millis()- time1)>500)&&(mode1)) { mode1 = false; led_white(1,0); Dynamixel.turn(din[1],LEFT,0); led_rgb(0, 4095, 0);}
      if (((millis()- time2)>500)&&(mode2)) { mode2 = false; led_white(3,0); Dynamixel.turn(din[3],LEFT,0); led_rgb(0, 4095, 0);}
      if (((millis()- time3)>500)&&(mode3)) { mode3 = false; led_white(4,0); Dynamixel.turn(din[4],LEFT,0); led_rgb(0, 4095, 0);}
      if (((millis()- time4)>500)&&(mode4)) { mode4 = false; led_white(6,0); Dynamixel.turn(din[6],LEFT,0); led_rgb(0, 4095, 0);}
    
      
      if(!mode0 && !mode1 && !mode2 && !mode3 && !mode4 && (millis()-time_stop)>1){ // parado sin  usuarios
        leds_white_off();
        time_stop = millis();
        cnt_color_value+=200*cnt_step;
        if(cnt_color_value<0 || cnt_color_value>3895){
            if(cnt_step==-1){
              cnt_color_order++;
              if(cnt_color_order>2) cnt_color_order = 0;
              cnt_color_value = 0;
            }
            if(cnt_color_value>4095) cnt_color_value = 4095; 
            cnt_step*=-1;
        }
        switch(cnt_color_order){
          case 0:
            led_rgb(cnt_color_value,0,0);
            break;  
          case 1:
            led_rgb(0,cnt_color_value,0);
            break; 
          case 2:
            led_rgb(0,0,cnt_color_value);
            break; 
        }
                  
        
      } 
      Serial.begin(9600);              // Begin Serial Comunication

       Serial.print("Colores");
       Serial.print(cnt_color_order);
       Serial.print(": ");
       Serial.print(cnt_color_value);
       //delay(100);
       Serial.println();
    
  }
    
}