#include <Servo.h>
Servo pinky, ringF, middleF, indexF, thumb;
int bend = 800, straight = 2200, half = 1800, wait = 500, pbend = 2200, pstraight = 800, phalf = 1300;

#if defined(ARDUINO) && ARDUINO >= 100
  #include "Arduino.h"
  #include "SoftwareSerial.h"
  SoftwareSerial port(12,13);
#else // Arduino 0022 - use modified NewSoftSerial
  #include "WProgram.h"
  #include "NewSoftSerial.h"
  NewSoftSerial port(12,13);
#endif

#include "EasyVR.h"
EasyVR easyvr(port);

//Groups and Commands
enum Groups
{
  GROUP_0  = 0,
  GROUP_1  = 1,
};

enum Group0 
{
  G0_JILL = 0,
};

enum Group1 
{
  G1_TEST = 0,
  G1_PINCH = 1,
  G1_GRAB = 2,
  G1_ROCK = 3,
  G1_GNARLY = 4,
  G1_POINT = 5,
  G1_PEACE = 6,
  G1_FLICK = 7,
  G1_NEUTRAL = 8,
  G1_TYPE = 9,
  G1_RELAX = 10,
  G1_MOUSE = 11,
  G1_WAVE = 12,
  G1_THUMBS_UP = 13,
  G1_PENCIL = 14,
};


EasyVRBridge bridge;

int8_t group, idx;

void setup()
{
  pinky.attach(3);
  ringF.attach(5);
  middleF.attach(6);
  indexF.attach(9);
  thumb.attach(10);
  neutral();
  delay(500);
  
  // bridge mode?
  if (bridge.check())
  {
    cli();
    bridge.loop(0, 1, 12, 13);
  }
  // run normally
  Serial.begin(9600);
  port.begin(9600);

  if (!easyvr.detect())
  {
    Serial.println("EasyVR not detected!");
    for (;;);
  }

  easyvr.setPinOutput(EasyVR::IO1, LOW);
  Serial.println("EasyVR detected!");
  easyvr.setTimeout(5);
  easyvr.setLanguage(0);

  group = EasyVR::TRIGGER; //<-- start group (customize)
}

void action();

void loop()
{
  easyvr.setPinOutput(EasyVR::IO1, HIGH); // LED on (listening)

  Serial.print("Say a command in Group ");
  Serial.println(group);
  easyvr.recognizeCommand(group);

  do
  {
    // can do some processing while waiting for a spoken command
  }
  while (!easyvr.hasFinished());
  
  easyvr.setPinOutput(EasyVR::IO1, LOW); // LED off

  idx = easyvr.getWord();
  if (idx >= 0)
  {
    // built-in trigger (ROBOT)
    group = GROUP_1;
    return;
  }
  idx = easyvr.getCommand();
  if (idx >= 0)
  {
    // print debug message
    uint8_t train = 0;
    char name[32];
    Serial.print("Command: ");
    Serial.print(idx);
    if (easyvr.dumpCommand(group, idx, name, train))
    {
      Serial.print(" = ");
      Serial.println(name);
    }
    else
      Serial.println();
    easyvr.playSound(0, EasyVR::VOL_FULL);
    // perform some action
    action();
  }
  else // errors or timeout
  {
    if (easyvr.isTimeout())
      Serial.println("Timed out, try again...");
    int16_t err = easyvr.getError();
    if (err >= 0)
    {
      Serial.print("Error ");
      Serial.println(err, HEX);
    }
  }
}

void action()
{
    switch (group)
    {
    case GROUP_0:
      switch (idx)
      {
      case G0_JILL:
        // write your action code here
        group = GROUP_1;
        break;
      }
      break;
    case GROUP_1:
      switch (idx)
      {
        Serial.println("here");
      case G1_TEST:
        test();
        break;
      case G1_PINCH:
        pinch();
        break;
      case G1_GRAB:
        grab();
        break;
      case G1_ROCK:
        rock();
        break;
      case G1_GNARLY:
        gnarly();
        break;
      case G1_POINT:
        point();
        break;
      case G1_PEACE:
        peace();
        break;
      case G1_FLICK:
        flick();
        break;
      case G1_NEUTRAL:
        neutral();
        break;
        case G1_TYPE:
        type();
        break;
      case G1_RELAX:
        neutral();
        group = GROUP_0;
        break;
      case G1_MOUSE:
        mouse();
        break;
      case G1_WAVE:
        wave();
        break;
      case G1_THUMBS_UP:
        thumbsup();
        break;
      case G1_PENCIL:
        pencil();
        break;
      }
      break;
    }
}

void test()
{
  Serial.println("HERE");
  drive(pinky, pbend);
  drive(pinky, pstraight);
  drive(ringF, bend);
  drive(ringF, straight);
  drive(middleF, bend);
  drive(middleF, straight);
  drive(indexF, bend);
  drive(indexF, straight);
  drive(thumb, bend);
  drive(thumb, straight);
}

void pinch()
{
  qDrive(thumb, bend);
  qDrive(ringF, straight);
  qDrive(middleF, straight);
  qDrive(pinky, pstraight);
  delay(200);
  qDrive(indexF, bend);
}

void grab()
{
  qDrive(indexF, bend);
  qDrive(thumb, bend);
  qDrive(ringF, bend);
  qDrive(middleF, bend);
  qDrive(pinky, pbend);
}

void rock()
{
  qDrive(middleF, bend);
  delay(200);
  qDrive(indexF, straight);
  qDrive(thumb, straight);
  qDrive(ringF, bend);
  qDrive(pinky, pstraight);
}

void gnarly()
{
  qDrive(middleF, bend);
  delay(200);
  qDrive(indexF, bend);
  qDrive(thumb, straight);
  qDrive(ringF, bend);
  qDrive(pinky, pstraight);
}

void point()
{
  qDrive(indexF, straight);
  qDrive(thumb, bend);
  qDrive(ringF, bend);
  qDrive(middleF, bend);
  qDrive(pinky, pbend);
}

void peace()
{
  qDrive(indexF, straight);
  qDrive(thumb, bend);
  qDrive(ringF, bend);
  qDrive(middleF, straight);
  qDrive(pinky, pbend);
}

void flick()
{
  qDrive(indexF, bend);
  qDrive(thumb, bend);
  qDrive(ringF, bend);
  qDrive(middleF, straight);
  qDrive(pinky, pbend);
}

void neutral()
{
  qDrive(indexF, straight);
  qDrive(thumb, straight);
  qDrive(ringF, straight);
  qDrive(middleF, straight);
  qDrive(pinky, pstraight);
}

void type()
{
  qDrive(thumb, half);
  qDrive(pinky, phalf);
  qDrive(indexF, half);
  qDrive(middleF, half);
  qDrive(ringF, half);
}

void mouse()
{
  qDrive(thumb, bend);
  qDrive(pinky, pbend);
  qDrive(indexF, bend);
  qDrive(middleF, straight);
  qDrive(ringF, straight);
}

void wave()
{
  for(int i = 0; i < 3; i++)
  {
    qDrive(middleF, bend);
    delay(200);
    qDrive(thumb, straight);
    qDrive(pinky, pbend);
    qDrive(indexF, bend);
    qDrive(ringF, bend);
    delay(500);
    neutral();
    delay(500);
  }
}

void thumbsup()
{
    qDrive(middleF, bend);
    delay(200);
    qDrive(thumb, straight);
    qDrive(pinky, pbend);
    qDrive(indexF, bend);
    qDrive(ringF, bend);
}

void pencil()
{
  qDrive(middleF, bend);
  delay(200);
  qDrive(thumb, bend);
  qDrive(pinky, pbend);
  qDrive(ringF, bend);
  delay(500);
  qDrive(indexF, bend);
}

void drive(Servo s, int pos)
{
  s.writeMicroseconds(pos);
  delay(wait);
}

void qDrive(Servo s, int pos)
{
  s.writeMicroseconds(pos);
}