/****************************************************************
Hardware Connections:

IMPORTANT: The APDS-9960 can only accept 3.3V!
 
 Arduino Pin  APDS-9960 Board  Function
 
 3.3V         VCC              Power
 GND          GND              Ground
 A4           SDA              I2C Data
 A5           SCL              I2C Clock
 2            INT              Interrupt
****************************************************************/

#include <Wire.h>
#include <SparkFun_APDS9960.h>

// Pins
#define APDS9960_INT    2 // Needs to be an interrupt pin
const int pinRelayA = 8;
const int pinRelayB = 9;
bool pos=0;

// Constants

// Global Variables
SparkFun_APDS9960 apds = SparkFun_APDS9960();
int isr_flag = 0;
int getup_overflow=0;
int todrink_overflow=0;

#define REDPIN 5
#define GREENPIN 6
#define BLUEPIN 3

ISR(TIMER2_OVF_vect)
{
  getup_overflow++; // variable which counts the overflows
  todrink_overflow++;
}

void setup() {
  TCCR2B |= (1 << CS02); // timer prescaler = 256
  TCNT2 = 0; // Counter initialize
  TIMSK2 |= (1 << TOIE2); // Enables interruption by overflow
  sei(); // Enables global interrupts
  getup_overflow = 0;
  todrink_overflow = 0;

  pinMode(REDPIN, OUTPUT);
  pinMode(GREENPIN, OUTPUT);
  pinMode(BLUEPIN, OUTPUT);
  // Set interrupt pin as input
  pinMode(APDS9960_INT, INPUT);

  // Initialize Serial port
  Serial.begin(9600);
  Serial.println();
  Serial.println(F("--------------------------------"));
  Serial.println(F("SparkFun APDS-9960 - GestureTest"));
  Serial.println(F("--------------------------------"));
  
  // Initialize interrupt service routine
  attachInterrupt(0, interruptRoutine, FALLING);

  // Initialize APDS-9960 (configure I2C and initial values)
  if ( apds.init() ) {
    Serial.println(F("APDS-9960 initialization complete"));
  } else {
    Serial.println(F("Something went wrong during APDS-9960 init!"));
  }
  
  // Start running the APDS-9960 gesture sensor engine
  if ( apds.enableGestureSensor(true) ) {
    Serial.println(F("Gesture sensor is now running"));
  } else {
    Serial.println(F("Something went wrong during gesture sensor init!"));
  }

  // initialize relay pins
   pinMode(pinRelayA, OUTPUT);
   pinMode(pinRelayB, OUTPUT);
 
   // preset relays to LOW
   digitalWrite(pinRelayA, LOW);
   digitalWrite(pinRelayB, LOW);
}

void loop() {
  int r, g, b;
 Serial.println(pos);
  if( isr_flag == 1 ) {
    detachInterrupt(0);
    handleGesture();
    isr_flag = 0;
    attachInterrupt(0, interruptRoutine, FALLING);
  }

  if (getup_overflow >= 2083 && pos==0){
    analogWrite(REDPIN, 255);
    //getup_overflow = 0;
    Serial.println("RED");
  }

  if (todrink_overflow >= 6249){
    analogWrite(BLUEPIN, 255);
  //  todrink_overflow = 0;
    Serial.println("BLUE");
  }
}

void interruptRoutine() {
  isr_flag = 1;
}

void handleGesture() {
    if ( apds.isGestureAvailable() ) {
    switch ( apds.readGesture() ) {
      case DIR_UP:
        Serial.println("UP");
        analogWrite(BLUEPIN, 0);
        todrink_overflow = 0;
        break;
      case DIR_DOWN:
        Serial.println("DOWN");
        break;
      case DIR_LEFT:
        Serial.println("LEFT");
        analogWrite(REDPIN, 0);
        getup_overflow = 0;
        break;
      case DIR_RIGHT:
        Serial.println("RIGHT");
        stopActuator();
        break;
      case DIR_NEAR:
        Serial.println("NEAR");
        extendActuator();
        delay(5000);
        pos=0;
        stopActuator();
        analogWrite(REDPIN, 0);
        break;
      case DIR_FAR:
        Serial.println("FAR");
        retractActuator();
        delay(5000);
        pos=1;
        stopActuator();
        break;
      default:
        Serial.println("NONE");
    }
  }
}

//Set one relay one and the other off
//this will move extend the actuator
void extendActuator()
{
   digitalWrite(pinRelayB, LOW);
   delay(250);
   digitalWrite(pinRelayA, HIGH);
}
 
//Set one relay off and the other on 
//this will move retract the actuator 
void retractActuator()
{
   digitalWrite(pinRelayA, LOW);
   delay(250);
   digitalWrite(pinRelayB, HIGH);
}
 
//Set both relays off
//this will stop the actuator in a braking
void stopActuator()
{
   digitalWrite(pinRelayA, LOW);
   digitalWrite(pinRelayB, LOW);
}