/* MindBullets.pde - Interface to trigger a modified 
** Nerf Stampede to fire when the appropriate signals
** are received from a NeuroSky EEG headset.
**
** Several portions of this code are borrowed and 
** modified from the NeuroSky Arduino project files.
** The rest is from Hex Machina. Permission is granted
** to modify and distribute for non-commercial, 
** non-military purposes.
**
** (C) 2011 Hex Machina (except where noted)
*/

#define MACADDR 0013ef004ba7
#define BTBAUDRATE 57600

// Attention threshold 1-100 above which the fire command is sent
#define FIRE_THRESHOLD 40

// Number below which we consider the signal to be good, 1-255.
// Lower non-zero numbers denote better signal. 
#define POOR_QUALITY_THRESHOLD 200

// Set to 1 to program the BlueSMiRF to search automatically
#define PROGRAM 0

// Set to 1 to have the BlueSMiRF search just once on start
#define ONTHEFLY 0

// Set to 1 to enable pass-through terminal mode for querying 
// the BlueSMiRF
#define TERMINAL 0

// Set to 0 to use NewSoftSerial--note this is not recommended
// for gathering actual data from the EEG headset
#define HARDSERIAL 1

// Increase non-zero values for increasing levels of debug info
#define DEBUG 0

// Make sure you've downloaded and installed the 
// NewSoftSerial library prior to enabling
#if HARDSERIAL == 0
  #include <NewSoftSerial.h>
#endif

char mac[13] = "MACADDR";
char key[5] = "0000";
#if HARDSERIAL != 0
  int RX = 0;
  int TX = 1;
#else
  int RX = 2;
  int TX = 3;
#endif
int BTPWR = 7;
int FIRE_READY = 8;
int FIRE = 9;
int ATT_LED = 10;
int MED_LED = 11;
int STATUS = 13;

// The following definitions are pqart of the NeuroSky sample code
// checksum variables
byte generatedChecksum = 0;
byte checksum = 0;
int payloadLength = 0;
byte payloadData[64] = {0};
byte poorQuality = 0;
byte attention = 0;
byte meditation = 0;

// system variables
long lastReceivedPacket = 0;
boolean bigPacket = false;
// End NeuroSky sample vars
#if HARDSERIAL == 0
  NewSoftSerial BTSerial(RX,TX);
#endif

void setup()
{
  Serial.begin(57600);
  #if HARDSERIAL == 0
    BTSerial.begin(BTBAUDRATE);
  #endif
  pinMode( RX, INPUT );
  pinMode( TX, OUTPUT );
  pinMode( STATUS, OUTPUT );
  pinMode( BTPWR, OUTPUT );
  pinMode( FIRE_READY, OUTPUT );
  pinMode( FIRE, OUTPUT );
  pinMode( ATT_LED, OUTPUT );
  pinMode( MED_LED, OUTPUT );
  
  digitalWrite( BTPWR, HIGH );
  delay( 500 );
  #if TERMINAL != 1
    #if PROGRAM == 1
      BTProgram();
    #endif
    #if ONTHEFLY == 1
      BTInit();
    #endif
  #endif
  blink( 3, 200 );
  #if DEBUG >= 1
    Serial.println("Ready");
  #endif
}

void loop()
{
  #if TERMINAL == 1 && HARDSERIAL == 0
    BTSerial.print("$$$");
    while ( true )
    {
      BluetoothConduit();
      delay( 2 );
    }
  #endif
  readNeuroValues();
  if (attention >= FIRE_THRESHOLD )
    fire();
}

#if HARDSERIAL == 0
/* This passes commands from the Serial terminal in the
** Arduino IDE to the attached Bluetooth module and relays
** responses from the Bluteooth module back to the IDE.
** NewSoftSerial is required for this.
*/
void BluetoothConduit()
{
  int count = 0;
  if (Serial.available())
  {
    Serial.print("> ");
    while (Serial.available())
    {
      count++;
      char a = Serial.read();
      BTSerial.print(a);
      Serial.print(a);
    }
    BTSerial.print('\r');
    BTSerial.flush();
    Serial.println();
  }
  delay( 20 );
  count = 0;
  if (BTSerial.available())
  {
    Serial.print("< ");
    while (BTSerial.available())
    {
      count++;
      char a = BTSerial.read();
      Serial.print(a);
    }
    Serial.println();
    Serial.flush();
  }
}
#endif

/* Tries to manually connect to the MAC address, rather than
** programming the Bluetooth module. Good for one-shot solutions.
*/
void BTInit()
{
  Serial.print( "$$$" );
  delay( 150 );
  
  Serial.flush();
  
  Serial.print( "C," );
  Serial.print( mac );
  Serial.print( '\r' );
  Serial.flush();
  
  Serial.print( "---\r" );
  delay( 150 );
  #if DEBUG >= 1
    Serial.println("Inited");
  #endif
}

/* Programs the Bluetooth module to automatically go into
** Bluetooth master mode and scan for a given MAC address.
** After programming the Bluetooth module, the Arduino will
** have to be reprogrammed to interpret the data coming in.
** This is heavily modified from the NeuroSky sample code.
** (Portions (C) 2011 NeuroSky)
*/
void BTProgram()
{
#if HARDSERIAL == 0
  BTSerial.print( "$$$" );
  delay( 150 );
  
  BTSerial.print( "SU,57\r" );
  if ( !OKrcvd("CMD") ) { Serial.println("No SU"); return; }
  BTSerial.flush();
  
  BTSerial.print( "SR," );
  BTSerial.print( mac );
  BTSerial.print( '\r' );
  if ( !OKrcvd("AOK") ) { Serial.println("No SR"); return; }
  BTSerial.flush();
  
  BTSerial.print( "SP," );
  BTSerial.print( key );
  BTSerial.print( '\r' );
  if ( !OKrcvd("AOK") ) { Serial.println("No SP"); return; }
  BTSerial.flush();
  
  BTSerial.print( "SM,3\r" );
  if ( !OKrcvd("AOK") ) { Serial.println("No SM"); return; }
  BTSerial.flush();
  
  BTSerial.print( "---\r" );
  delay( 150 );
  #if DEBUG >= 1
    Serial.println("Inited");
  #endif
#else
  Serial.print( "$$$" );
  delay( 150 );
  
  Serial.print( "SU,57\r" );
  if ( !OKrcvd("CMD") ) { Serial.println("No SU"); return; }
  Serial.flush();
  
  Serial.print( "SR," );
  Serial.print( mac );
  Serial.print( '\r' );
  if ( !OKrcvd("AOK") ) { Serial.println("No SR"); return; }
  Serial.flush();
  
  Serial.print( "SP," );
  Serial.print( key );
  Serial.print( '\r' );
  if ( !OKrcvd("AOK") ) { Serial.println("No SP"); return; }
  Serial.flush();
  
  Serial.print( "SM,3\r" );
  if ( !OKrcvd("AOK") ) { Serial.println("No SM"); return; }
  Serial.flush();
  
  Serial.print( "---\r" );
  delay( 150 );
  #if DEBUG >= 1
    Serial.println("Inited");
  #endif
#endif
}

/* Checks to make sure a given three-character response is 
** received from the Bluetooth module. Heavily modified from
** NeuroSky sample code. (Portions (C) 2011 NeuroSky)
*/
boolean OKrcvd(char cmd[])
{
  int time = 0;
  #if HARDSERIAL != 0
    while( Serial.available() < 3 && time < 1000 ) { time++; delay( 1 ); }
  #else
    while( BTSerial.available() < 3 && time < 1000 ) { time++; delay( 1 ); }
  #endif
  if ( time >= 1000 )
  {
    digitalWrite( BTPWR, LOW );
    Serial.println( "Timeout on msg" );
    return false;
  }
  char str[3];
  for ( int i = 0; i < 3; i++ )
    #if HARDSERIAL != 0
      str[ i ] = (char) Serial.read();
    #else
      str[ i ] = (char) BTSerial.read();
    #endif
  if (str[ 0 ] != cmd[ 0 ] || str[ 1 ] != cmd[ 1 ] || str[ 2 ] != cmd[ 2 ] )
  {
    digitalWrite( BTPWR, LOW );
    Serial.print( "Got wrong msg: " );
    Serial.print( str );
    Serial.print( ", expected " );
    Serial.print( cmd );
    Serial.println();
    return false;
  }
  return true;
}

// A basic blink algorithm.
void blink( int times, int duration )
{
  for ( int i = 0; i < times; i++ )
  {
    digitalWrite( STATUS, HIGH );
    delay( duration );
    digitalWrite( STATUS, LOW );
    delay( duration );
  }
}

/* Reads in one byte from the chosen serial interface.
** Lifted almost directly from the NeuroSKy sample code.
** (Portions (C) 2011 NeuroSky)
*/
byte ReadOneByte() {
  int ByteRead;

  #if HARDSERIAL != 0
    while(!Serial.available());
    ByteRead = Serial.read();
  #else
    while(!BTSerial.available());
    ByteRead = BTSerial.read();
  #endif

  return ByteRead;
}

/* Reads in data coming from the NeuroSky headset, checks 
** for integrity, and then assigns values to the attention
** and meditation values if everything checks out. Debugging
** and LED status added by Hex Machina, all else is NeuroSky 
** sample code. (Portions (C) 2011 NeuroSky)
*/
void readNeuroValues() {
  // Look for sync bytes
  if(ReadOneByte() == 170) {
    #if DEBUG >= 2
      Serial.print("-");
    #endif
    if(ReadOneByte() == 170) {
      #if DEBUG >= 2
        Serial.println("+");
      #endif

      payloadLength = ReadOneByte();
      if(payloadLength > 169)                      //Payload length can not be greater than 169
      {
        #if DEBUG >= 1
          Serial.print( "Payload length excessive: " );
          Serial.println( payloadLength );
        #endif
        return;
      }

      generatedChecksum = 0;        
      #if DEBUG >= 3
        Serial.print( "Reading payload... " );
      #endif
      for(int i = 0; i < payloadLength; i++) {  
        payloadData[i] = ReadOneByte();            //Read payload into memory
        generatedChecksum += payloadData[i];
      }
      #if DEBUG >= 3
        Serial.println( "Done" );
      #endif

      checksum = ReadOneByte();                      //Read checksum byte from stream      
      generatedChecksum = 255 - generatedChecksum;   //Take one's compliment of generated checksum

      if(checksum == generatedChecksum) {

        poorQuality = POOR_QUALITY_THRESHOLD;
        attention = 0;
        meditation = 0;

        for(int i = 0; i < payloadLength; i++) {    // Parse the payload
          switch (payloadData[i]) {
          case 2:
            i++;            
            poorQuality = payloadData[i];
            #if DEBUG >= 1
              Serial.print( poorQuality, DEC );
              Serial.print( ": " );
            #endif
            bigPacket = true;            
            break;
          case 4:
            i++;
            attention = payloadData[i];                        
            break;
          case 5:
            i++;
            meditation = payloadData[i];
            break;
          case 0x80:
            i = i + 3;
            break;
          case 0x83:
            i = i + 25;
            break;
          default:
            break;
          } // switch
        } // for loop

        bigPacket = false;
      }
      else {
        #if DEBUG >= 2
          Serial.println("Bad checksum!");
        #endif
      }  // end if else for checksum
      
      #if DEBUG >= 1
        if ( attention > 0 || meditation > 0 )
        {
          Serial.print( attention, DEC );
          Serial.print( ", " );
          Serial.println( meditation, DEC );
        }
      #endif
      
      // Adjust status LEDs
      analogWrite( ATT_LED, attention * 2 + 55 );
      analogWrite( MED_LED, meditation * 2 + 55 );

    } // end if read 0xAA byte
  } // end if read 0xAA byte
}

/* Fire the Nerf Stampede. Because of the two-relay system,
** the ground connection between the trigger and the motor 
** must be broken first (FIRE_READY is brought high), and
** then the fire command is issued (FIRE is brought high).
** To stop, the ground connection between the trigger and
** motor must be re-enabled (FIRE_READY and FIRE both 
** brought low simultaneously).
*/
void fire()
{
  #if DEBUG >= 1
    Serial.println( "Fire!" );
  #endif
  digitalWrite( STATUS, HIGH );
  digitalWrite( FIRE_READY, HIGH );
  delay( 100 );
  digitalWrite( FIRE, HIGH );
  delay( 150 );
  digitalWrite( FIRE_READY, LOW );
  digitalWrite( FIRE, LOW );
  digitalWrite( STATUS, LOW );
}