#include "bhoreal.h"
#include "Adafruit_NeoPixel.h"

// Variables for interpreting the serial commands
byte tempR;
byte tempC;
byte lastread;
byte command = 0;
boolean ready = true;
boolean refresh_ok = false;
uint16_t IntensityMAX = 255;

// Default draw colour. Each channel can be between 0 and 4095.
int red = 0;
int green = IntensityMAX;
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 =  MINISLIM; //Modelo
uint16_t MAX   =  4; 

int NUM_LEDS   =  16; 

#define PIN 11
Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_LEDS, PIN, NEO_GRB + NEO_KHZ800);
        
  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, 4, 11, 12}, 
    {2, 5, 10, 13}, 
    {1, 6,  9, 14}, 
    {0, 7,  8, 15}, 
  };
  
   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};

  byte row[4]    = {13, 5, 10, 9};
  byte column[4] = {8, 6, 12, 4};


void Bhoreal::begin(uint16_t DEVICE, uint32_t BAUD)
  {
    for(byte x = 0; x < MAX; ++x){ 
           for(byte y = 0; y <MAX; ++y)
           {
             remap[x][y] = remapmini[x][y];
           }
          }
    for(byte i = 0; i<4; i++) 
      {
        pinMode(column[i], INPUT);
        pinMode(row[i], OUTPUT);
        digitalWrite(row[i], LOW);
      }
    // Start the serial port
    Serial.begin(BAUD);
    /* Setup the timer interrupt*/
    strip.begin();
    strip.show();
    PORTE |= B01000000;
    DDRE  |= B01000000;
    //digitalWrite(7, LOW);
    timer1Initialize();
    timer3Initialize();
  }

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);
}

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);
  } 
}


  
unsigned long time = 0;

void Bhoreal::refresh(){ 
  if (refresh_ok) 
    {
      strip.show();
      refresh_ok=false;
    }
// if ((millis() - time)>=100) 
//   {
//     strip.show();
//     time = millis();
//   }
 
}
  
// 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]] = IntensityMAX;
         levelB[remap[x][y]] = IntensityMAX;
         levelG[remap[x][y]] = IntensityMAX;
       }
      }
      for(int x = 0; x < NUM_LEDS; ++x) strip.setPixelColor(x, levelR[x], levelG[x], levelB[x]);
      strip.show();
}

float average(int anaPin) {
  int lecturas = 100;
  long total = 0;
  float average = 0;
  for(int i=0; i<lecturas; i++)
  {
    //delay(1);
    total = total + analogRead(anaPin);
  }
  average = (float)total / lecturas;  
  return(average);
}

unsigned int val = 0;
unsigned int val_ant = 0;

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.
  
//  Serial.println((int)((pow((analogRead(ANALOG0)/1023.),10)-1)*1023));
  val = average(ANALOG0);
  if ((val>=(val_ant + 5))||(val<=(val_ant - 5)))
  {
    val_ant =val;
    Serial.println(val);
     for(byte x = 0; x < MAX; ++x){ 
         for(byte y = 0; y < MAX; ++y)
         {
           if (val<=(1023/3))
           {
             levelR[remap[x][y]] = map(val,0,1023/3,0, IntensityMAX);
             levelG[remap[x][y]] = 0;
             levelB[remap[x][y]] = 0;
           }
           else if (val<=(1023*2/3))
             {
               levelR[remap[x][y]] = IntensityMAX;
               levelG[remap[x][y]] = map(val,1023/3, 1023*2/3, 0, IntensityMAX);
               levelB[remap[x][y]] = 0;
             }
           else
           {
             levelR[remap[x][y]] = IntensityMAX;
             levelG[remap[x][y]] = IntensityMAX;
             levelB[remap[x][y]] = map(val,1023*2/3, 1023, 0, IntensityMAX);
           }
          }
      }
      for(int x = 0; x < NUM_LEDS; ++x) strip.setPixelColor(x, levelR[x], levelG[x], levelB[x]);
      strip.show();
  }  
//  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]);
//    }
//  }
}


void Bhoreal::hueADC(){
  // For all of the ADC's which are activated, check if the analog value has changed,
  // and send a message if it has.
  
//  Serial.println((int)((pow((analogRead(ANALOG0)/1023.),10)-1)*1023));
  val = average(ANALOG0);
  if ((val>=(val_ant + 1))||(val<=(val_ant - 1)))
  {
    val_ant =val;
    //Serial.println(val);
     for(byte x = 0; x < MAX; ++x){ 
         for(byte y = 0; y < MAX; ++y)
         {
            uint32_t c = hue2rgb(val/8); // 128 HUE steps 
            uint8_t
              r = (uint8_t)(c >> 16),
              g = (uint8_t)(c >>  8),
              b = (uint8_t)c;  
         }
      }
      for(int x = 0; x < NUM_LEDS; ++x) strip.setPixelColor(x, levelR[x], levelG[x], levelB[x]);
      strip.show();
  }  

}




///////////////////////////////////////////////////////////////
//////////////////////     HUE -> RGB    //////////////////////
///////////////////////////////////////////////////////////////

uint32_t Bhoreal::hue2rgb(uint16_t hueValue)
{
  
  uint8_t r;
  uint8_t g;
  uint8_t b;
  hueValue<<= 3;  // 128 midi steps -> 1024 hue steps
  
  if (hueValue < 341)  { // Lowest third of the potentiometer's range (0-340)
    hueValue = (hueValue * 3) / 4; // Normalize to 0-255

    r = 255 - hueValue;  // Red from full to off
    g = hueValue;        // Green from off to full
    b = 1;               // Blue off
  }
  else if (hueValue < 682) { // Middle third of potentiometer's range (341-681)
    hueValue = ( (hueValue-341) * 3) / 4; // Normalize to 0-255

    r = 1;              // Red off
    g = 255 - hueValue; // Green from full to off
    b = hueValue;       // Blue from off to full
  }
  else  { // Upper third of potentiometer"s range (682-1023)
    hueValue = ( (hueValue-683) * 3) / 4; // Normalize to 0-255

    r = hueValue;       // Red from off to full
    g = 1;              // Green off
    b = 255 - hueValue; // Blue from full to off
  }
  
  return ((uint32_t)r << 16) | ((uint32_t)g <<  8) | b;

}



boolean flag = true;

/*TIMER*/
ISR(TIMER3_OVF_vect)
{
  cli();
    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();
            green = Serial.read();
            blue = Serial.read();
            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;
              strip.setPixelColor(remap[tempC][tempR], red, green, blue);
               refresh_ok=true;
             }
            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;
              strip.setPixelColor(remap[tempC][tempR], 0, 0, 0);
              refresh_ok=true;
             }
            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;
                    strip.setPixelColor(remap[tempR][tempC], red, green, blue);
                  }
                  else {
                    levelR[remap[tempR][tempC]] = 0;
                    levelG[remap[tempR][tempC]] = 0;
                    levelB[remap[tempR][tempC]] = 0;
                    strip.setPixelColor(remap[tempR][tempC], 0, 0, 0);
                  }
                }
             }
              refresh_ok=true;
            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;
                    strip.setPixelColor(remap[tempR][tempC], red, green, blue);
                  }
                  else {
                    levelR[remap[tempR][tempC]] = 0;
                    levelG[remap[tempR][tempC]] = 0;
                    levelB[remap[tempR][tempC]] = 0;
                    strip.setPixelColor(remap[tempR][tempC], 0, 0, 0);
                  }
                }
             }
              refresh_ok=true;
            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;
                  strip.setPixelColor(remap[tempR][tempC], red, green, blue);
                }
                else {
                  levelR[remap[tempR][tempC]] = 0;
                  levelG[remap[tempR][tempC]] = 0;
                  levelB[remap[tempR][tempC]] = 0;
                  strip.setPixelColor(remap[tempR][tempC], 0, 0, 0);
                }
              }
            }
             refresh_ok=true;
            ready = true;
          }
          break;
        case 9: //clear
          if(command & 1){
            byte TEMPMAX = MAX*MAX;
            for(int x = 0; x< TEMPMAX;++x){
              levelR[x] = red;
              levelG[x] = green;
              levelB[x] = blue;
              strip.setPixelColor(x, red, green, blue);
            }
          }
          else{
            byte TEMPMAX = MAX*MAX;
            for(int x = 0; x< TEMPMAX;++x){
              levelR[x] = 0;
              levelG[x] = 0;
              levelB[x] = 0;
              strip.setPixelColor(x, 0, 0, 0);
            }
          }
          refresh_ok=true;
          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);
    sei();
}

#define RESOLUTION 65536    // Timer1 is 16 bit
unsigned int pwmPeriod;
    unsigned char clockSelectBits;
	char oldSREG;					// To hold Status 

void setPeriodTimer1(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 Bhoreal::timer1Initialize()
{
    TCCR1A = 0;                 // clear control register A 
    TCCR1B = _BV(WGM13);        // set mode 8: phase and frequency correct pwm, stop the timer
    setPeriodTimer1(5); 
    TIMSK1 = _BV(TOIE1);                                     
}

ISR(TIMER1_OVF_vect)
{

     if (flag) { PORTE |= B01000000; flag=0;}
     else if (!flag) { PORTE &= B10111111; flag=1;}
     
}

void setPeriodTimer3(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(CS30);              // no prescale, full xtal
  else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS31);              // prescale by /8
  else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS31) | _BV(CS30);  // prescale by /64
  else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS32);              // prescale by /256
  else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS32) | _BV(CS30);  // prescale by /1024
  else        cycles = RESOLUTION - 1, clockSelectBits = _BV(CS32) | _BV(CS30);  // request was out of bounds, set as maximum
  
  oldSREG = SREG;				
  cli();							// Disable interrupts for 16 bit register access
  ICR3 = pwmPeriod = cycles;                                          // ICR1 is TOP in p & f correct pwm mode
  SREG = oldSREG;
  
  TCCR3B &= ~(_BV(CS30) | _BV(CS31) | _BV(CS32));
  TCCR3B |= clockSelectBits;                                          // reset clock select register, and starts the clock
}

void Bhoreal::timer3Initialize()
{
    TCCR3A = 0;                 // clear control register A 
    TCCR3B = _BV(WGM33);        // set mode 8: phase and frequency correct pwm, stop the timer
    setPeriodTimer3(5000); 
    TIMSK3 = _BV(TOIE3);    
  
//    TCCR3A = 0;
//    TCCR3B = 0<<CS32 | 0<<CS31 | 1<<CS30;
//    //Timer1 Overflow Interrupt Enable
//    TIMSK3 = 1<<TOIE3;
}