//---------------------------------------------------------
// VelociGesture Example
// 2022.06.02 RSP
// Target: Arduino Pro Mini 5V/16MHz
// Diagnostic and example program
//---------------------------------------------------------

// using I2C-USART bridge for 2'nd serial port
#include "FEARLESS_Serial_I2C_Bridge.h"
SerialI2Cbridge i2cSerial; // create a serial bridge

// VelociBus interface functions
#include "FEARLESS_VelociBus.h"
VelociBus vbus;            // create a VelociBus handler

uint8_t mode = 0;
String MODE_DESC[3] = { "Gesture", "Color Identification", "Proximity" };
uint8_t sensitivity = 0;
String SENS_DESC[4] = { "Off (0)", "Low (1)", "Medium (2)", "High (3)" };

//---------------------------------------------------------

void setup()
{
  Wire.begin();
  Wire.setClock(400000); // high speed why not

  // diagnostics
  Serial.begin(115200); 
Serial.println(F("STARTUP"));

  // bridge serial VelociBus to I2C
  i2cSerial.begin(); // defaults to I2C address 8, ready pin 2
  vbus.begin( &i2cSerial );
// benign but slow chain test  
Serial.print(F("CHAIN LENGTH = ")); Serial.println(vbus.chainLength());
// quicker test with possible side effects
Serial.print(F("COMMAND CHAIN LENGTH = ")); Serial.println(vbus.commandChainLength());

          
//  vbus.gestureColor( 0, 1, true );
//  vbus.gestureProximity( 0,3,true,true );
  vbus.gestureMode( 0,1,true );
}

//---------------------------------------------------------

void loop()
{ 
  // periodically change the configuration
if (0)
  { static uint8_t c = 0;
    static uint32_t tm = millis();
    if (millis() - tm > 10000UL || tm == 0)
    { tm = millis();
Serial.print(MODE_DESC[mode]); Serial.print(" Mode, "); Serial.print(SENS_DESC[sensitivity]); Serial.println(" sensitivity.");
      switch (c)
      { case 0:
          vbus.gestureMode( 0, sensitivity, true );
          break;
    
        case 1:
          vbus.gestureColor( 0, sensitivity, true );
          break;
    
        case 2:
          vbus.gestureProximity( 0, sensitivity, true, true );
      }
      sensitivity = (sensitivity+1) % 4;
      if (sensitivity == 0) mode = (mode+1) % 3;
    }
  }

  // receive & decode VelociBus events
  vbus_packet pkt;
  if (vbus.poll( &pkt ))
  { 
Serial.print("PKT "); Serial.print(pkt.address,HEX);   Serial.print(", "); Serial.println(pkt.data,HEX);  
    switch (pkt.data >> 4)
    { case 0: Serial.println("BAD DATA"); break;
      case 1:
        Serial.print("Color identified: ");
        if (pkt.data & 0x08) Serial.print("W");
        if (pkt.data & 0x04) Serial.print("R");
        if (pkt.data & 0x02) Serial.print("G");
        if (pkt.data & 0x01) Serial.print("B");
        Serial.println();
        break;
      case 2:
        Serial.print("Proximity: ");
        if ((pkt.data & 0x03) == 0) Serial.print("Exit");
        if ((pkt.data & 0x03) == 1) Serial.print("Far");
        if ((pkt.data & 0x03) == 2) Serial.print("Near");
        Serial.println();
        break;
      case 3:
        Serial.print("Gesture: ");
        if (pkt.data & 0x04) Serial.print("Indeterminate");
        else switch (pkt.data & 0x03)
        { case 0: Serial.print("UP");   break;
          case 1: Serial.print("DOWN"); break;
          case 2: Serial.print("LEFT"); break;
          case 3: Serial.print("RIGHT");
        }      
        Serial.println();
    }
  }
}