//mattie's test troep
#include "pitches.h"
#include <Servo.h>          // Include servo library
#include <SoftwareSerial.h> //bluetooth
#define RxD 6
#define TxD 7
#define DEBUG_ENABLED  1

SoftwareSerial blueToothSerial(RxD,TxD);

const int baudRate = 9600;  //Default bauldRate
int melody[] = {
  NOTE_C6 };
int noteDurations[] = {
  6 };
Servo servoLeft;            // Declare left servo
Servo servoRight;
int powerLED = 13;
int actLED = 12;
int speakerID = 8;


void setup()
{
  Serial.begin(baudRate);
  Serial.println("ohaaaai");
  pinMode(RxD, INPUT); //bluetooth
  pinMode(TxD, OUTPUT); //bluetooth
  setupBlueToothConnection();

  pinMode(powerLED, OUTPUT);
  pinMode(actLED, OUTPUT);

  activateLED();
  muziek();

  // servoLeft.attach(2);
  // servoRight.attach(4);
  // stopServo();

}

void loop()
{
  //blinkActivityLed();

  char recvChar;
  while(1){
    if(blueToothSerial.available()){//check if there's any data sent from the remote bluetooth shield
      recvChar = blueToothSerial.read();
      Serial.print(recvChar);
    }
    if(Serial.available()){//check if there's any data sent from the local serial terminal, you can add the other applications here
      recvChar  = Serial.read();
      //Serial.print("sending through bluetooth......");
      //Serial.println(recvChar);
      blueToothSerial.print(recvChar);
    }
  }
}

void setupBlueToothConnection()
{
  blueToothSerial.begin(38400); //Set BluetoothBee BaudRate to default baud rate 38400
  blueToothSerial.print("\r\n+STWMOD=0\r\n"); //set the bluetooth work in slave mode
  blueToothSerial.print("\r\n+STNA=DamBot\r\n"); //set the bluetooth name as "SeeedBTSlave"
  blueToothSerial.print("\r\n+STOAUT=1\r\n"); // Permit Paired device to connect me
  blueToothSerial.print("\r\n+STAUTO=0\r\n"); // Auto-connection should be forbidden here
  delay(2000); // This delay is required.
  blueToothSerial.print("\r\n+INQ=1\r\n"); //make the slave bluetooth inquirable
  Serial.println("The slave bluetooth is inquirable!");
  delay(2000); // This delay is required.
  blueToothSerial.flush();
}

void blinkActivityLed()
{
  digitalWrite(actLED, HIGH);
  delay(200);
  digitalWrite(actLED, LOW);
  delay(200);
}


void muziek()
{
  // delay(10);
  // iterate over the notes of the melody:
  for (int thisNote = 0; thisNote < sizeof(melody) - 1; thisNote++) {

    // to calculate the note duration, take one second
    // divided by the note type.
    //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
    int noteDuration = 1000/noteDurations[thisNote];
    tone(speakerID, melody[thisNote],noteDuration);

    // to distinguish the notes, set a minimum time between them.
    // the note's duration + 30% seems to work well:
    //dit was * 1.30
    int pauseBetweenNotes = noteDuration * 2;
    delay(pauseBetweenNotes);
    // stop the tone playing:
    noTone(speakerID);
  }
  noTone(speakerID);
}

void knipperLed()
{

  // Serial.println("dit is een test");
  // delay(1000);

  digitalWrite(13, HIGH);   //LED AAN
  digitalWrite(12, LOW);   //LED UIT
  digitalWrite(11, HIGH);
  digitalWrite(10, LOW);

  delay(250);

  digitalWrite(11, LOW);
  digitalWrite(10, HIGH);

  delay(250);


  digitalWrite(13, LOW);    //LED UIT
  digitalWrite(12, HIGH);  //LED AAN
  digitalWrite(11, HIGH);
  digitalWrite(10, LOW);

  delay(250);

  digitalWrite(11, LOW);
  digitalWrite(10, HIGH);

  delay(250);

}

void activateLED()
{
  digitalWrite(powerLED, HIGH);
  digitalWrite(actLED, LOW);
}



void stopServo()
{
  servoLeft.writeMicroseconds(1500);
  servoRight.writeMicroseconds(1500);
}

void forward()
{
  servoLeft.writeMicroseconds(1700);
  servoRight.writeMicroseconds(1300);
}

void backward()
{
  servoLeft.writeMicroseconds(1300);
  servoRight.writeMicroseconds(1700);
}