#include "bhoreal.h"

// Variables for interpreting the serial commands
byte tempR;
byte tempC;
byte lastread;
byte command = 0;
byte ready = true;

// Default draw colour. Each channel can be between 0 and 4095.
int red = 0;
int green = 4095;
int blue = 0;

// Auxiliary analog output definitions
#define ANALOG0 A5 //POTENCIOMETRO
#define ANALOG1 A1
boolean adc[2] = { //On or off state
  0, 0};
byte analogval[2]; //The last reported value
byte tempADC; //Temporary storage for comparison purposes

uint16_t MODEL =  SLIM; //Modelo
uint16_t MAX   =  8; 
uint16_t SPIMAX   =  32; 

  boolean pressed[8][8] = {
    {1,1,1,1,1,1,1,1},
    {1,1,1,1,1,1,1,1},
    {1,1,1,1,1,1,1,1},
    {1,1,1,1,1,1,1,1},
    {1,1,1,1,1,1,1,1},
    {1,1,1,1,1,1,1,1},
    {1,1,1,1,1,1,1,1},
    {1,1,1,1,1,1,1,1}
  };
  
  byte remap[8][8] = {
    {48,49,51,52,      12,13,14,15}, 
    {50,53,54,55,      11,10, 9, 8}, 
    {56,57,58,59,       7, 6, 5, 2}, 
    {63,62,61,60,       4, 3, 1, 0}, 
    {32,33,35,36,      28,29,30,31}, 
    {34,37,38,39,      27,26,25,24}, 
    {40,41,42,43,      23,22,21,18}, 
    {47,46,45,44,      20,19,17,16}, 
  };
  
   const byte remapmini[4][4] = {
    {3, 7, 11, 15}, 
    {2, 6, 10, 14}, 
    {1, 5,  9, 13}, 
    {0, 4,  8, 12}, 
  };
  
   int levelR[64] = {
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0, 
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0};
  int levelG[64] = {
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0, 
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0};
  int levelB[64] = {
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0, 
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0};

#if defined(__AVR_ATmega32U4__)
  //TLC5940NT pin definitions
  #define VPRG      2
  #define SIN       MOSI // MOSI - Hardware SPI, can't be changed
  #define SCLK      SCK // SCK - Hardware SPI, can't be changed
  #define XLAT      A0
  #define BLANK     7
  #define GSCLK     11
  byte row[4]    = {13, 5, 10, 9};
  byte column[4] = {8, 6, 12, 4};
  //byte column[4] = {4, 12, 6, 8};
#elif defined(__AVR_ATmega328P__)|| defined(__AVR_ATmega168__)
  //TLC5940NT pin definitions
  #define VPRG 2
  #define SIN 11 // MOSI - Hardware SPI, can't be changed
  #define SCLK 13 // SCK - Hardware SPI, can't be changed
  #define XLAT 4
  #define BLANK 5
  #define DCPRG 6
  #define GSCLK 7
  // Pin definitions for the 74HC164 SIPO shift register (drives button rows high)
  #define DATAPIN 16 // aka analog pin 2 (what, you didn't know that analog pins 0-5 are also digital pins 14-19? Well, now you do!)
  #define CLOCKPIN 3
  
  // Pin definitions for the 74HC165 PISO shift register (reads button column state)
  #define INDATAPIN 9
  #define INCLOCKPIN 10
  #define INLOADPIN 8 // toggling this tell the 165 to read the value into its memory for reading
#endif


void Bhoreal::begin(uint16_t DEVICE, uint32_t BAUD)
  {
    MODEL = DEVICE;
    if (MODEL == MINI)
      {
        for(byte x = 0; x < MAX; ++x){ 
           for(byte y = 0; y <MAX; ++y)
           {
             remap[x][y] = remapmini[x][y];
           }
          }
         MAX = 4;
         SPIMAX = 8;
      }
    else 
      {
        MAX = 8;
        SPIMAX = 32;
      }
    //Setup data directions, and set everything to the correct initial levels,
    // For TLC5940
    #if defined(__AVR_ATmega32U4__)
        PORTF |= B00110010;  
        DDRF  |= B00110010;
    #elif defined(__AVR_ATmega328P__)|| defined(__AVR_ATmega168__)
        PORTC |= B00100011;  
        DDRC |= B00100011;
    #endif
    pinMode(VPRG, OUTPUT);
    pinMode(SIN, OUTPUT);
    pinMode(SCLK, OUTPUT);
    pinMode(XLAT, OUTPUT);
    pinMode(BLANK, OUTPUT);
    pinMode(GSCLK, OUTPUT);
    pinMode(MISO, INPUT);
    pinMode(SS,OUTPUT);
    digitalWrite(SS,HIGH); //disable device
    digitalWrite(SIN, LOW);
    digitalWrite(SCLK, LOW);
    digitalWrite(XLAT, LOW);
    digitalWrite(VPRG, LOW);
    digitalWrite(BLANK, HIGH);
    digitalWrite(GSCLK, HIGH);
    
    //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)|(0<<SPR1)|(0<<SPR0);
    clr=SPSR;
    clr=SPDR;
    delay(10);
    
    #if defined(__AVR_ATmega32U4__)
    for(byte i = 0; i<4; i++) 
      {
        pinMode(column[i], INPUT);
        pinMode(row[i], OUTPUT);
        digitalWrite(row[i], LOW);
      }
    #elif defined(__AVR_ATmega328P__)|| defined(__AVR_ATmega168__)
       // 165 Setup
        pinMode(INDATAPIN, INPUT);
        pinMode(INCLOCKPIN, OUTPUT);
        pinMode(INLOADPIN, OUTPUT);
      
        // 164 Setup
        pinMode(DATAPIN, OUTPUT);
        pinMode(CLOCKPIN, OUTPUT);
    #endif
    // Start the serial port
    Serial.begin(BAUD);
    /* Setup the timer interrupt*/
    Timer1.initialize(400); // set a timer of length 1000000 microseconds (or 1 sec - or 1Hz)
    //Timer1.attachInterrupt( timerIsr ); // attach the service routine here  
    delay(10);
  }

void Bhoreal::on_press(byte r, byte c){
  Serial.write( 1);
  Serial.write( (r << 4) | c);
}

void Bhoreal::on_release(byte r, byte c){
  Serial.write( byte(0) );
  Serial.write( (r << 4) | c);
}

#if defined(__AVR_ATmega32U4__)
  void Bhoreal::checkButtons(){
    for(byte c = 0; c < MAX; c++)
     { 
      digitalWrite(row[c],HIGH);
      for(int r= MAX - 1; r >= 0; r--)
       {
        if(pressed[c][r] != digitalRead(column[r]))
          { // read the state
            pressed[c][r] = digitalRead(column[r]);
            if(pressed[c][r]) on_press(c, r);
            else on_release(c, r);
          }
       }
      digitalWrite(row[c],LOW);
    } 
  }
#elif defined(__AVR_ATmega328P__)|| defined(__AVR_ATmega168__)
  void Bhoreal::checkButtons(){
   
  digitalWrite(CLOCKPIN,LOW);
  digitalWrite(DATAPIN, HIGH);
  for(byte c = 0; c < MAX; c++){
    digitalWrite(CLOCKPIN, HIGH);

    digitalWrite(INLOADPIN, LOW); // read into register
    digitalWrite(INLOADPIN, HIGH); // done reading into register, ready for us to read

    for(int r= MAX - 1; r >= 0; r--){ // read each of the 165's 8 inputs (or its snapshot of it rather)

      // tell the 165 to send the first inputs pin state
      digitalWrite(INCLOCKPIN, LOW);
      // read the current output
      //int tempvalue = digitalRead(INDATAPIN);
      //Serial.print(tempvalue,DEC);

      if(pressed[r][c] != digitalRead(INDATAPIN)){ // read the state
        pressed[r][c] = digitalRead(INDATAPIN);
        if(!pressed[r][c]){
          on_press(r, c);
        }
        else {
          on_release(r, c);
        }
      }
      // tell the 165 we are done reading the state, the next inclockpin=0 will output the next input value
      digitalWrite(INCLOCKPIN, 1);

    }

    //Serial.println();

    digitalWrite(CLOCKPIN, LOW);
    digitalWrite(DATAPIN, LOW);
    

   }
  }
#endif

// Transfer a character out over hardware SPI
char Bhoreal::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 Bhoreal::setGreysR() {
  digitalWrite(BLANK, HIGH);
  digitalWrite(XLAT,LOW);
  for(int i = SPIMAX; i>=0; i--){
    spi_transfer( (levelR[2*i+1] & 0x0FF0) >> 4 );
    spi_transfer( ((levelR[2*i+1] & 0xF) << 4) | ((levelR[2*i] & 0x0F00) >> 8) );
    spi_transfer( levelR[2*i] & 0xFF);
  }
  digitalWrite(XLAT,HIGH);
  digitalWrite(XLAT,LOW);
  digitalWrite(BLANK, LOW);
}

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

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

#if defined(__AVR_ATmega32U4__)
  void Bhoreal::pulseGSCLK() {
    //ultra fast pulse trick, using digitalWrite caused flickering
    PORTB |= 0x80 ; // bring pin 11 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
    PORTB &= 0x7F;  // bring pin 11 low without touching the other pins in PORTB
  }
#elif defined(__AVR_ATmega328P__)|| defined(__AVR_ATmega168__)
  void Bhoreal::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
  }
#endif

void Bhoreal::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();
  }
}

#if defined(__AVR_ATmega32U4__)
  void Bhoreal::makemagic(){
    /* Clear the transistors, set the TLC5940 to the correct channel value with setGreys,
    turn the transistor on to give voltage to the LEDs, and then pulse the TLC5940's clock.
  
    */
    PORTF |= B00110010;
    setGreysB();
    PORTF &= B11011111;         
    feedPorts();
    
    PORTF |= B00110010;
    setGreysG();
    PORTF &= B11101111;         
    feedPorts();
    
    PORTF |= B00110010;
    setGreysR();
    PORTF &= B11111101;         
    feedPorts();
    
  }
#elif defined(__AVR_ATmega328P__)|| defined(__AVR_ATmega168__)
  void Bhoreal::makemagic(){
    /* Clear the transistors, set the TLC5940 to the correct channel value with setGreys,
    turn the transistor on to give voltage to the LEDs, and then pulse the TLC5940's clock.
  
    */
    PORTC |= B00100011;
    setGreysR();
    PORTC &= B11011111;         
    feedPorts();
    
    PORTC |= B00100011;
    setGreysG();
    PORTC &= B11111101;         
    feedPorts();
    
    PORTC |= B00100011;
    setGreysB();
    PORTC &= B11111110;         
    feedPorts();
  }
#endif

// Run this animation once at startup. Currently unfinished.
void Bhoreal::startup(){
  for(byte x = 0; x < MAX; ++x){ 
   for(byte y = 0; y <MAX; ++y)
   {
     //levelR[remap[x][y]] = 0;
     levelR[remap[x][0]] = 4095;
     makemagic();
     makemagic();
     makemagic();
     //levelB[remap[x][y]] = 0;
     levelB[remap[0][y]] = 4095;
     makemagic();
     makemagic();
     makemagic();
     levelG[remap[x][y]] = 4095;
     makemagic();
     makemagic();
     makemagic();
   }
  }
}

//The timer interrupt routine, which periodically interprets the serial commands
void timerIsr() {
  sei(); //Reenable global interrupts, otherwise serial commands will get dropped
    do{ // This do ensures that the data is always parsed at least once per cycle
      if(Serial.available()){
        if(ready){ // If the last command has finished executing, read in the next command and reset the command flag
          command = Serial.read();
          ready = false;
        }
        switch (command >> 4) { //Execute the appropriate command, but only if we have received enough bytes to complete it. We might one day add "partial completion" for long command strings.
        case 1: // set colour
          if( Serial.available() > 2 ) {
            red = Serial.read() * 16;
            green = Serial.read() * 16;
            blue = Serial.read() * 16;
            ready=true;
          }
          break;
        case 2: // led_on
          if( Serial.available() ) {
            lastread = Serial.read();
            tempR = lastread >> 4;
            tempC = lastread & B1111;
            if ((tempR < MAX)&&(tempC < MAX))
             {
              levelR[remap[tempC][tempR]] = red;
              levelG[remap[tempC][tempR]] = green;
              levelB[remap[tempC][tempR]] = blue;
             }
            ready = true;
          }

          break;
        case 3: // led_off
          if( Serial.available() ) {
            lastread = Serial.read();
            tempR = lastread >> 4;
            tempC = lastread & B1111;
            if ((tempR < MAX)&&(tempC < MAX))
             {
              levelR[remap[tempC][tempR]] = 0;
              levelG[remap[tempC][tempR]] = 0;
              levelB[remap[tempC][tempR]] = 0;
             }
            ready = true;
          }
          break;
        case 4: // led_row1
          if( Serial.available() ) {
            tempR = command & B1111;
            lastread = Serial.read();
            if (tempR < MAX)
             {
                for(tempC = 0; tempC < MAX; ++tempC){
                  if(lastread & (1 << tempC) ){
                    levelR[remap[tempR][tempC]] = red;
                    levelG[remap[tempR][tempC]] = green;
                    levelB[remap[tempR][tempC]] = blue;
                  }
                  else {
                    levelR[remap[tempR][tempC]] = 0;
                    levelG[remap[tempR][tempC]] = 0;
                    levelB[remap[tempR][tempC]] = 0;
                  }
                }
             }
            ready = true;
          }
          break;
        case 5: // led_col1
          if( Serial.available() ) {
            tempC = command & B1111;
            lastread = Serial.read();
            if (tempC < MAX)
             {
                for(tempR = 0; tempR < MAX; ++tempR){
                  if(lastread & (1 << tempR) ){
                    levelR[remap[tempR][tempC]] = red;
                    levelG[remap[tempR][tempC]] = green;
                    levelB[remap[tempR][tempC]] = blue;
                  }
                  else {
                    levelR[remap[tempR][tempC]] = 0;
                    levelG[remap[tempR][tempC]] = 0;
                    levelB[remap[tempR][tempC]] = 0;
                  }
                }
             }
            ready = true;
          }
          break;
        case 8: //frame
          if( Serial.available() > 7 ) {
            
            for(tempR=0; tempR < MAX; ++tempR){
              lastread = Serial.read();
              for(tempC = 0; tempC < MAX; ++tempC){
                if(lastread & (1 << tempC) ){
                  levelR[remap[tempR][tempC]] = red;
                  levelG[remap[tempR][tempC]] = green;
                  levelB[remap[tempR][tempC]] = blue;
                }
                else {
                  levelR[remap[tempR][tempC]] = 0;
                  levelG[remap[tempR][tempC]] = 0;
                  levelB[remap[tempR][tempC]] = 0;
                }
              }
            }
            ready = true;
          }
          break;
        case 9: //clear
          if(command & 1){
            for(int x = 0; x< MODEL;++x){
              levelR[x] = red;
              levelG[x] = green;
              levelB[x] = blue;
            }
          }
          else{
            for(int x = 0; x<MODEL;++x){
              levelR[x] = 0;
              levelG[x] = 0;
              levelB[x] = 0;
            }
          }
          ready = true;
          break;
        case 12:
          switch(command & 15){
          case 0:
            adc[0] = true;
            analogval[0] = (analogRead(ANALOG0) >> 2);
            Serial.write(14 << 4);
            Serial.write(analogval[0]);
            break;
          case 1:
            adc[1] = true;
            analogval[1] = (analogRead(ANALOG1) >> 2);
            Serial.write(14 << 4 | 1);
            Serial.write(analogval[1]);
            break;
          default:
            break;
          }
          ready = true;
          break;
        case 13:
          adc[command & 15] = false;
          ready = true;
          break;
        default:
          break;
        }
      }
    }
    // If the serial buffer is getting too close to full, keep executing the parsing until it falls below a given level
    // This might cause flicker, or even dropped messages, but it should prevent a crash.
    while (Serial.available() > TOOFULL);
}

void Bhoreal::checkADC(){
  // For all of the ADC's which are activated, check if the analog value has changed,
  // and send a message if it has.
  if(adc[0]){
    tempADC = (analogRead(ANALOG0) >> 2);
    if(abs((int)analogval[0] - (int)tempADC) > 3 ){
      analogval[0] = tempADC;
      Serial.write(14 << 4);
      Serial.write(analogval[0]);
    }
  }
  if(adc[1]){
    if(analogval[1] != (analogRead(ANALOG1) >> 2)){
      analogval[1] = (analogRead(ANALOG1) >> 2);
      Serial.write(14 << 4 | 1);
      Serial.write(analogval[1]);
    }
  }
}


/*TIMER*/

TimerOne Timer1;              // preinstatiate

ISR(TIMER1_OVF_vect)          // interrupt service routine that wraps a user defined function supplied by attachInterrupt
{
  timerIsr();
}

void TimerOne::initialize(long microseconds)
{
  TCCR1A = 0;                 // clear control register A 
  TCCR1B = _BV(WGM13);        // set mode 8: phase and frequency correct pwm, stop the timer
  setPeriod(microseconds);
  TIMSK1 = _BV(TOIE1);                                     // sets the timer overflow interrupt enable bit
  // AR - remove sei() - might be running with interrupts disabled (eg inside an ISR), so leave unchanged
  //  sei();                                                   // ensures that interrupts are globally enabled
    resume();
}

#define RESOLUTION 65536    // Timer1 is 16 bit

void TimerOne::setPeriod(long microseconds)		// AR modified for atomic access
{
  
  long cycles = (F_CPU / 2000000) * microseconds;                                // the counter runs backwards after TOP, interrupt is at BOTTOM so divide microseconds by 2
  if(cycles < RESOLUTION)              clockSelectBits = _BV(CS10);              // no prescale, full xtal
  else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11);              // prescale by /8
  else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11) | _BV(CS10);  // prescale by /64
  else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS12);              // prescale by /256
  else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS12) | _BV(CS10);  // prescale by /1024
  else        cycles = RESOLUTION - 1, clockSelectBits = _BV(CS12) | _BV(CS10);  // request was out of bounds, set as maximum
  
  oldSREG = SREG;				
  cli();							// Disable interrupts for 16 bit register access
  ICR1 = pwmPeriod = cycles;                                          // ICR1 is TOP in p & f correct pwm mode
  SREG = oldSREG;
  
  TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
  TCCR1B |= clockSelectBits;                                          // reset clock select register, and starts the clock
}


void TimerOne::resume()				// AR suggested
{ 
  TCCR1B |= clockSelectBits;
}

void TimerOne::start()	// AR addition, renamed by Lex to reflect it's actual role
{
  unsigned int tcnt1;
  
  TIMSK1 &= ~_BV(TOIE1);        // AR added 
  GTCCR |= _BV(PSRSYNC);   		// AR added - reset prescaler (NB: shared with all 16 bit timers);

  oldSREG = SREG;				// AR - save status register
  cli();						// AR - Disable interrupts
  TCNT1 = 0;                	
  SREG = oldSREG;          		// AR - Restore status register

  do {	// Nothing -- wait until timer moved on from zero - otherwise get a phantom interrupt
	oldSREG = SREG;
	cli();
	tcnt1 = TCNT1;
	SREG = oldSREG;
  } while (tcnt1==0); 
 
//  TIFR1 = 0xff;              		// AR - Clear interrupt flags
//  TIMSK1 = _BV(TOIE1);              // sets the timer overflow interrupt enable bit
}

void TimerOne::stop()
{
  TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));          // clears all clock selects bits
}