#include <TimerThree.h>
#include <SoftwareSerial.h>
#include <SPI.h>
#include <Arduino.h>
#include "LSM6DS3.h"
#include "Wire.h"
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"
#include "Constants.h"

#define CE_PIN     22
#define CSN_PIN    13

#define DEBUG      0
#define RFID_TEST  0

#define RED_PIN    10
#define GREEN_PIN  9
#define BLUE_PIN   11

#define CTS        4
#define POWER_VCC  5
#define BOT_PROG   23

#define BAT        A2

unsigned long time_interrup1_isr3 = 0;
uint32_t time_prog;
uint32_t time_tag;
uint32_t time_sleep;
uint32_t time_low_bat;
byte RED_COLOR, GREEN_COLOR, BLUE_COLOR;

boolean flag = true;

int idpin[4]  = { A0, A1, 12, 6};

int i = 0;

/*-----( Declare objects )-----*/
RF24 radio(CE_PIN, CSN_PIN); // Create a Radio

// NOTE: the "LL" at the end of the constant is "LongLong" type
const uint64_t pipe = 0xE8E8F0F0E1LL; // Define the transmit pipe
byte data[32];                           //Data buffer for testing data transfer speeds

static boolean mode = false;
SoftwareSerial rfid(8, 11); // RX, TX NO SE USA
#define RST_RFID A3

void setup() {
  ini_spk();
}

void loop()
{
  char tagString[14] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,'\0'};
  int index = 0;
  boolean reading = false;
  if (mode==true)
  {
    if (Serial.available())
    {
      Serial1.write(Serial.read());
      if (flag)
      {
        digitalWrite(CTS, LOW);
        delay(10);
        digitalWrite(CTS, HIGH);
        flag = false;
      }
      time_prog = millis();
    }
    if (Serial1.available())
      Serial.write(Serial1.read());
    if ((millis()- time_prog)>=1000) flag = true;  
  }
  else
  {
    #if DEBUG
        if(Serial1.available()) Serial.write(Serial1.read());
        if(Serial.available()) Serial1.write(Serial.read());
    #endif 
    
    if (!to_sleep())
      {
       if ((millis()- time_tag)>=150)
        {
           //Serial.println(level_bat());
           while(rfid.available()){
        
            int readByte = rfid.read(); //read next available byte
        
            if(readByte == 2) reading = true; //begining of tag
            if(readByte == 3) reading = false; //end of tag
        
            if(reading && readByte != 2 && readByte != 10 && readByte != 13){
              //store the tag
              tagString[index] = readByte;
              index ++;
            }
           tagString[12] = accelRead();
          }
          checkTag(tagString); //Check if it is a match
          clearTag(tagString); //Clear the char of all value
          resetReader(); //eset the RFID reader
          time_tag=millis();
        }
        else if (millis()< time_tag) time_tag = millis();
        while (Serial1.available())
          {
            if (Serial1.read()=='S') 
              {
                RGBwrite(0, 0, 0);
                data[3] = 0;
                radio.writeFast(&data,32); 
              }
          }
      }
     if ((level_bat()<=3400)&&((millis()-time_low_bat)>=1000))
      {
        byte tempRED = RED_COLOR;
        byte tempGREEN = GREEN_COLOR;
        byte tempBLUE = BLUE_COLOR;
        RGBwrite(255, 0, 0);
        delay(100);
        RGBwrite(tempRED, tempGREEN, tempBLUE);
        time_low_bat = millis();
      }
     
     else if (millis()< time_low_bat) time_low_bat = millis();
  }

}

byte id()
{
  int id = 0;
    for (byte i = 0; i < 4; i++) bitWrite(id, i, digitalRead(idpin[i]));
  return id;
}

void RGBwrite(byte RED, byte GREEN, byte BLUE)
{
  RED_COLOR = RED;
  GREEN_COLOR = GREEN;
  BLUE_COLOR = BLUE;
  analogWrite(RED_PIN, 255 - RED);
  analogWrite(GREEN_PIN, 255- GREEN);
  analogWrite(BLUE_PIN, 255 - BLUE);
}

void button(void)
{
  if ((millis()-time_interrup1_isr3)>500)
    {
      if (!digitalRead(BOT_PROG)) 
        {
          mode=!mode;
          if (mode==true)
            {
              digitalWrite(POWER_VCC, LOW);
              RGBwrite(0, 0, 255);
              digitalWrite(CTS, LOW);
              delay(10);
              digitalWrite(CTS, HIGH);
            }
          else
            {
              RGBwrite(0, 0, 0);
            }
        }
      time_interrup1_isr3 = millis();
    }
  else if (millis()< time_interrup1_isr3) time_interrup1_isr3 = millis();
}

char tagString_previous[14] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, '\0'};
char tag[13] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, '\0'};

void checkTag(char tag_read[]){
///////////////////////////////////
////Check the read tag against known tags
/////////////////////////////////////
//
  if(strlen(tag_read) == 0)
    {
      //Serial.println("STOP"); 
      clearTag(tagString_previous); //Clear the char of all value
      #if RFID_TEST
        RGBwrite(0, 0, 0);
      #endif 
      return; //empty, no need to continue
    }
  else if ((compareTag(tag_read, tagString_previous))&&(tag_read[12]==tagString_previous[12]))
    {
      //Serial.println("EQUAL");
      return;
    }
  for (int i=0; i<177; i++)
    { 
//      Serial.print(i);
//      Serial.print(" ");
//      Serial.println(tag[i]);
      strcpy_P(tag, (char*)pgm_read_word(&(rfid_table[i])));
      if (compareTag(tag_read, tag)) 
        {
          data[0] = 'D';
          data[3] = 0;
          radio.writeFast(&data,32);
          delay(10);
          boolean accel_state = false;
          if (tag_read[12]=='1') accel_state = true;
          if (accel_state) RGBwrite(255, 0, 0);
          else RGBwrite(255, 90, 90);
          uint16_t temp_data = i+ accel_state*200;
          Serial1.println(temp_data);

          data[0] = 'D';
          data[1] = temp_data&0x00FF;
          data[2] = temp_data>>8;
          data[3] = 1;
          radio.writeFast(&data,32);   
          Serial.print("Playing file ");
          Serial.print(temp_data);
          Serial.println(".wav...");
          copyTag(tag_read);
          return;
        }
    }
    #if RFID_TEST
      Serial.println(tag_read); //read out any unknown tag
      data[0] = 'R';
      for(int i = 0; i < 13; i++){
         data[i+1] = tag_read[i];
       } 
      radio.writeFast(&data,32);
      RGBwrite(0, 255, 0);
    #endif 
}

void resetReader(){
///////////////////////////////////
//Reset the RFID reader to read again.
///////////////////////////////////
  digitalWrite(RST_RFID, LOW);
  digitalWrite(RST_RFID, HIGH);
  //delay(150);
}

void clearTag(char one[]){
///////////////////////////////////
//clear the char array by filling with null - ASCII 0
//Will think same tag has been read otherwise
///////////////////////////////////
  for(unsigned int i = 0; i < strlen(one); i++){
    one[i] = 0;
  }
}

boolean compareTag(char one[], char two[]){
///////////////////////////////////
//compare two value to see if same,
//strcmp not working 100% so we do this
///////////////////////////////////

  if(strlen(one) == 0) return false; //empty

  for(int i = 0; i < 12; i++){
    if(one[i] != two[i]) return false;
  }

  return true; //no mismatches
}


void copyTag(char one[]){
  if(strlen(one) == 0) return; //empty
  for(int i = 0; i < 13; i++){
    tagString_previous[i] = one[i];
  }
}

int accel_sleepY = 0;
bool state_sleep = false;
#define TIME_TO_SLEEP 30000

boolean to_sleep()
  {
    int accel_tempY = (accelerometer.getConvertedYAxis()*20);
//    Serial.println(String(accel_tempY) + ", " + String(accel_sleepY));
    if (((accel_sleepY == accel_tempY)&&((millis()-time_sleep)>=TIME_TO_SLEEP))&&(!state_sleep))
      {
        accel_sleepY = accel_tempY;
        time_sleep = millis();
        RGBwrite(0, 0, 0);
        digitalWrite(RST_RFID, LOW);
        Serial1.print('S');
        delay(100);
        Serial.println("SLEEP");
        if (level_bat()>=4150) RGBwrite(0, 90, 0);
        else RGBwrite(255, 50, 0);
        state_sleep = true;
      }
    else if (accel_sleepY != accel_tempY)
      {
        accel_sleepY = accel_tempY;
        time_sleep = millis();
        if (state_sleep) 
          {
            RGBwrite(0, 0, 0);
            //digitalWrite(POWER_VCC, LOW);
            digitalWrite(CTS, LOW);
            delay(10);
            digitalWrite(CTS, HIGH);
            delay(1500);
            digitalWrite(RST_RFID, HIGH);
            clearTag(tagString_previous); //Clear the char of all value 
            Serial.println("AWAKE");
            state_sleep = false;
          }
        
      }
    else if (millis()< time_sleep) time_sleep = millis();  
    return state_sleep;
  }
  
char state_accel = '0';
char accelRead()
  {  
    if (accelerometer.getConvertedYAxis()>=0.8) 
        state_accel = '1';
    else if (accelerometer.getConvertedYAxis()<=-0.8)
        state_accel = '0';
    return state_accel;
    
//    Serial.print("Accelerometer Values ");
//    Serial.print("Raw  X = ");
//    Serial.print(accelerometer.getRawXAxis());
//    Serial.print(" Y = ");
//    Serial.print(accelerometer.getRawYAxis());
//    Serial.print(" Z = ");
//    Serial.print(accelerometer.getRawZAxis());
//    Serial.print(" X = ");
//    Serial.print(accelerometer.getConvertedXAxis(), 2);
//    Serial.print("g  Y = ");
//    Serial.println(accelerometer.getConvertedYAxis(), 2);
//    Serial.print("g  Z = ");
//    Serial.print(accelerometer.getConvertedZAxis(), 2);
//    Serial.println("g");
  }

int level_bat()
  {
    return average(BAT)*2*3300/1023.;
  }
  
float average(int anaPin) {
  int lecturas = 4;
  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);
}

void ini_spk()
  {
   //delay(4000);
   for (int i = 0; i < 4; i++)
    {
      pinMode(idpin[i], INPUT);
    }
    pinMode(CTS, OUTPUT);
    pinMode(POWER_VCC, OUTPUT);
    pinMode(RST_RFID, OUTPUT);
    pinMode(BOT_PROG, INPUT);
    digitalWrite(POWER_VCC, LOW);
    digitalWrite(RST_RFID, HIGH);
    digitalWrite(CTS, LOW);
    delay(100);
    digitalWrite(CTS, HIGH);
    delay(1500);
    Serial.begin(57600);
    Serial1.begin(57600);
    accelerometer.begin();
    rfid.begin(9600);
    Timer3.initialize(150000);
    Timer3.attachInterrupt(button);
    if(accelerometer.isActive()){
      #if DEBUG
        Serial.println("Accelerometer already active");
      #endif 
    }else{
      if(accelerometer.powerOn()){
        #if DEBUG
          Serial.println("Accelerometer Power ON");
        #endif 
      }else{
        #if DEBUG
          Serial.println("Accelerometer Not Powered On");
        #endif 
      }
    }
    ini_nrf();
    clearTag(tagString_previous); //Clear the char of all value
    time_sleep = millis();
    time_low_bat = millis();
  }

  void ini_nrf()
    {
    radio.begin();
    printf_begin();
    radio.setDataRate( RF24_250KBPS );
    radio.setPALevel( RF24_PA_MAX );
    radio.setAutoAck(0);
    radio.setPayloadSize(16);
    radio.setChannel(100); // canal para sala developpers. Por defecto es el canal 76 (showroom)
    radio.openWritingPipe(pipe);
    
    #if DEBUG
      radio.printDetails();
      Serial.println();
      Serial.println("Nrf24L01 Receiver Starting");
      Serial.print("Address: ");
      Serial.println(id());
    #endif 
    radio.powerUp();                        //Power up the radio
    }