#include <TimerThree.h>

/* Shift Register Test with ShiftPWM */

#include <SPI.h>                     //SPI used for faster hardware-based serial communication

//-------------SET PINS---------------------------
//Data pin is MOSI (UNO: pin 11, MEGA: pin 51)
const int ShiftPWM_latchPin = 8;          //Latch
//Clock pin is SCK(UNO: pin 13, MEGA: pin 52)
const int ShiftRegisterClear = 7;      //Clear
const bool ShiftPWM_invertOutputs = 0;  //if 1, outputs active low

#include <ShiftPWM.h>                //checks for ShiftPWM prefixed values

//-------------SET PARAMETERS---------------------------
unsigned char maxBrightness = 255;
unsigned char pwmFrequency = 75;    //In what units?
int numRegisters = 6;        //set this
int numLEDs = 8 * numRegisters;
int maxRSSI = 40;           //set this
int previousTopLED = 0;      //where Top LED is the topmost illuminated LED

//init timer 3
TimerThree t3;

//data buffer
char data[128];
int RSSI;

void getWifiData(){
  RSSI = retrieveRSSI(data,64);
  Serial.print("MAX RSSI value is ---> ");
  Serial.println(RSSI);
}

void setup(){

  pinMode(ShiftPWM_latchPin, OUTPUT);
  SPI.setBitOrder(MSBFIRST);
  SPI.setClockDivider(SPI_CLOCK_DIV4);    //What does this do?
  pinMode(ShiftRegisterClear, OUTPUT);
  SPI.begin();                             //What does this do?
  Serial.begin(9600);
  Serial1.begin(9600);
  Serial.println("Wi-Fi shield version 1");
  RSSI = 0;
  t3.initialize(2000000);
  t3.attachInterrupt(getWifiData());
  ShiftPWM.SetAmountOfRegisters(numRegisters);
  ShiftPWM.Start(pwmFrequency, maxBrightness);
}


void loop(){

  digitalWrite(ShiftRegisterClear, HIGH);
  if(RSSI > 0){
    RSSIToLEDOutput(RSSI);
    delay(2000);
  }
}

/*RSSI to LED Output*/
void RSSIToLEDOutput(int RSSI){
  if (RSSI >= 0 && RSSI <=maxRSSI){

    //normalize RSSI to a scale appropriate to numLEDs
    int topLED = numLEDs * RSSI / maxRSSI;    //this truncates topLED.
    int topLEDBrightness = modf(maxBrightness * (double)RSSI / (double)maxRSSI, NULL) ;

    Serial.print("Output topmost LED: ");

    //if RSSI increases, illuminate LEDs
    if (topLED > previousTopLED)
      for (int i = previousTopLED; i< topLED; i++){
        for (int j = 0; j < maxBrightness; j++){
          ShiftPWM.SetOne(i, j);
          j = 5 + j;
          delay(3);
          //if (interrupt != 0)
          //break;
        }
        //        if (interrupt != 0)
        //          break;
      }

    //if RSSI decreases, turn off LEDs
    if (topLED < previousTopLED)
      for (int i = previousTopLED-1; i>= topLED; i--){
        for (int j = maxBrightness; j > 0; j--){
          ShiftPWM.SetOne(i, j);
          j = j - 5;
          delay(3);
          //          if (interrupt != 0)
          //            break;
        }
        //        if (interrupt != 0)
        //          break;
      }

    ShiftPWM.SetOne(topLED, topLEDBrightness);


    previousTopLED = topLED;          //set up for next round

    Serial.println(topLED);
  }
  else {
    Serial.println("Output failed.");
  }
}


/*Test LED Output*/
void testLEDOutput(){
  ShiftPWM.Set8Pins(0,0b01100011);
  delay(4000);

  // Fade in and fade out all outputs one by one fast
  ShiftPWM.OneByOneFast();

  // Fade in all outputs
  for(int j=0;j<maxBrightness;j++){
    ShiftPWM.SetAll(j);
    delay(20);
  }
  // Fade out all outputs
  for(int j=maxBrightness;j>=0;j--){
    ShiftPWM.SetAll(j);
    delay(20);
  }

  // Fade in and fade out all outputs slowly
  ShiftPWM.OneByOneSlow();
}

/*
* Extract rssi from:
 * SSID_BSSID_Channel_RSSI_Security
 * <Sxxxx_xxxxxxxxxxxx_xx_xx_x>
 */

int extractRSSI(char* data, int buffer_length){

  int last_;
  char RSSI[2];
  int RSSIValue;

  for(int k = 0;k < buffer_length; k++){
    if(data[k] == '_'){
      last_ = k;
    }
  }

  //clear buffer
  RSSI[0] = 0;
  RSSI[1] = 0;
  int x, j;
  for(j = last_- 1,x = 1; data[j] != '_' && x >= 0; j--, x-- ){
    RSSI[x] = data[j];
  }

  RSSIValue = atoi(RSSI);
  //Serial.print("RSSI value is ---> ");
  //Serial.print(RSSIValue);
  //Serial.println(" things");

  return RSSIValue;
}

void clearDataBuffer(char* data, int buffer_length){
  //clear data buffer
  for(int c = 0; c < buffer_length; c++){
    data[c] = 0;
  }
}

/*
* sends command out on UART1 & reads response
 *
 */
int retrieveRSSI(char* data, int buffer_length){

  char inByte;
  int readBytes = 0;
  int i;
  int maxRSSI = 0;
  boolean commandStatus;
  String command = "<DI>";


  //Serial.println("no bytes in rx buffer sending command "+command+" to shield");
  Serial1.print(command);
  while(Serial1.available()==0) {

  }
  //set data buffer pointer to beginning of data buffer
  i=0;

  //Serial.println("reading from shield");
  while(1){

    inByte = Serial1.read();
    Serial.print(inByte);
    readBytes++;

    if(inByte == '<'){
      // next byte is status of command
      commandStatus = true;
      while (Serial1.available()==0) {
        //do nothing
        delayMicroseconds(30);
      }
      continue;
    }
    if (commandStatus == true) {
      //inByte = Serial1.read();
      //Serial.print(inByte);
      if(inByte == 'S'){
        Serial.println(command+" command was successfull !\nOutput:\n");
        continue;
      }
      else if(inByte == 'F'){
        Serial.println("command failed :( pmop pmon ...");
        continue;
      }
      commandStatus = false;
    }

    if(inByte == ':'){
      Serial.print('\n');
      int t = extractRSSI(data, buffer_length);
      if(t > maxRSSI){
        maxRSSI = t;
      }
      clearDataBuffer(data, buffer_length);
      i=0; // reset buffer pointer
    }
    else{
      if(i<buffer_length){
        data[i] = inByte;
        i++;
      }
    }

    if(inByte == '>'){
      Serial.println("\n end of output");
      break;
    }
    while (Serial1.available()==0) {
      //do nothing
      delayMicroseconds(30);
    }
  }

  if(readBytes > 0){
    Serial.print("\nfinished reading : ");
    Serial.print(readBytes);
    Serial.println(" Bytes\n");
    readBytes = 0;
  }

  return maxRSSI;
}

void testOutput(){
  // Print information about the interrupt frequency, duration and load on your program
  ShiftPWM.SetAll(0);
  ShiftPWM.PrintInterruptLoad();

  // Fade in and fade out all outputs one by one fast. Usefull for testing your circuit
  ShiftPWM.OneByOneFast();

  // Fade in all outputs
  for(int j=0;j<maxBrightness;j++){
    ShiftPWM.SetAll(j);
    delay(20);
  }
  // Fade out all outputs
  for(int j=maxBrightness;j>=0;j--){
    ShiftPWM.SetAll(j);
    delay(20);
  }

}