/* 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


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

  ShiftPWM.SetAmountOfRegisters(numRegisters);
  ShiftPWM.Start(pwmFrequency, maxBrightness);
}

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

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

  //testOutput();

  digitalWrite(ShiftRegisterClear, HIGH);
  RSSI = retrieveRSSI(data,64);
  Serial.print("MAX RSSI value is ---> ");
  Serial.println(RSSI);


//  if(RSSI == 4)
//    Serial.println("The RSSI is not 4! Stop lying you stupid sMMMMhit!!");
//  else{
    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);
  }

}