#include <SoftwareSerial.h>   //Software Serial Port
#include <Servo.h> 
#include <SPI.h>
//#include <Ethernet.h>
const int opendeur = 20;
const int geslotendeur = 205;
int teller = opendeur;
int stap;
int deurchecker = 5;
float volt;
float afstand;

int LDR = 4;
int waardeLDR;
boolean modewaarde = false;
Servo myservo; 

#define RxD 2
#define TxD 3
#define DEBUG_ENABLED  1
SoftwareSerial blueToothSerial(RxD,TxD);
#define START_CMD_CHAR '*'
#define END_CMD_CHAR '#'
#define DIV_CMD_CHAR '|'
#define CMD_DIGITALWRITE 10
#define CMD_ANALOGWRITE 11
#define CMD_TEXT 12
#define CMD_READ_ARDUDROID 13
#define MAX_COMMAND 20  // max command number code. used for error checking.
#define MIN_COMMAND 10  // minimum command number code. used for error checking. 
#define IN_STRING_LENGHT 40
#define MAX_ANALOGWRITE 255
#define PIN_HIGH 3
#define PIN_LOW 2


String inText;
//int pos =1;



//unsigned long lastConnectionTime = 0;          // last time you connected to the server, in milliseconds
//boolean lastConnected = false;                 // state of the connection last time through the main loop


void setup()
{
    Serial.begin(9600);
  
    myservo.attach(9); 
    pinMode(RxD, INPUT);
    pinMode(TxD, OUTPUT);
   
    setupBlueToothConnection();
    myservo.write(opendeur);
    
}

void loop()
   {
           waardeLDR = map(analogRead(LDR),0,1023,0,1000);
           Serial.println(waardeLDR);
           if ((waardeLDR<930) and (teller > 20) and (modewaarde == true))
            {
              myservo.write(opendeur);
              teller = 20;
              blueToothSerial.flush();
              blueToothSerial.print("com:A: deur open");
              delay(400);
            }
            if ((waardeLDR>970) and (teller < 205) and (modewaarde == true))
            {
              myservo.write(geslotendeur);
              teller = 205;
              blueToothSerial.flush();
              blueToothSerial.print("com:A: deur toe");
              delay(400);
            }
           
           int ard_command = 0;
           int pin_num = 0;
           int pin_value = 0;

           char get_char = ' ';  //read serial

  // wait for incoming data
           if (blueToothSerial.available() < 1) return; // if serial empty, return to loop().

  // parse incoming command start flag 
           get_char = blueToothSerial.read();
           if (get_char != START_CMD_CHAR) return; // if no command start flag, return to loop().

  // parse incoming command type
           ard_command = blueToothSerial.parseInt(); // read the command
         //   Serial.println(ard_command);
  // parse incoming pin# and value  
           pin_num = blueToothSerial.parseInt(); // read the pin
           pin_value = blueToothSerial.parseInt();  // read the value
           
           if (ard_command == CMD_DIGITALWRITE){  
           if (pin_value == PIN_LOW) pin_value = LOW;
            else if (pin_value == PIN_HIGH) pin_value = HIGH;
           else return; // error in pin value. return. 
           set_digitalwrite( pin_num,  pin_value);  // Uncomment this function if you wish to use 
           return;  // return from start of loop()
           }
           //krijg analoge data van ardudroid
  if (ard_command == CMD_ANALOGWRITE) {  
    analogWrite(  pin_num, int(pin_value) ); 
    //add you code hier
    if (pin_num == 11)
      {
   //  myservo.write(int(pin_value)); 
      delay(100);
      
      }
    return;  // Done. return to loop();
     }
      // 4) SEND DATA TO ARDUDROID
  if (ard_command == CMD_READ_ARDUDROID) { 
    // char send_to_android[] = "Place your text here." ;
    // Serial.println(send_to_android);   // Example: Sending text
   // blueToothSerial.print(" Analog 0 = "); 
   // blueToothSerial.println(analogRead(A0));  // Example: Read and send Analog pin value to Arduino
    return;  // Done. return to loop();
  }
  
  if (ard_command == CMD_TEXT){  
  
    inText =""; //clears variable for new input   
    while (blueToothSerial.available())  {
      char c = blueToothSerial.read();  //gets one byte from serial buffer
      Serial.print(c);
     
      delay(5);
      if (c == END_CMD_CHAR) { // if we the complete string has been read
        if ((inText == "open") and (modewaarde == false))
               {
                myservo.write(opendeur);
                teller = opendeur;
                blueToothSerial.flush();
                delay(400);
                blueToothSerial.print("com:M: deur open");
                delay(1);
                 }
          if ((inText == "toe") and (modewaarde == false))
               {
                myservo.write(geslotendeur);
                teller = geslotendeur;
                blueToothSerial.flush();
                delay(400);
                blueToothSerial.print("com:M: deur toe");
                delay(1);
                
               }
           if (inText == "auto")
               {
               modewaarde = true;
                 
                
               }
            if (inText =="manueel")
            {
               modewaarde = false;
                 
                
               }
            
            if (inText =="check")
               {   
                  float volt = analogRead(deurchecker) * 0.0048828125 *2 ; 
                  float afstand = 27*pow(volt, -1) ;
                  blueToothSerial.flush();
                  if (modewaarde == false)
                  {
                   if (afstand < 20)
                      {
                        blueToothSerial.print("check:M: deur open");
                      }
                    if (afstand > 20)
                      {
                        blueToothSerial.print("check:M: deur toe");
                      }
                   }
                   if (modewaarde == true)
                  {
                   if (afstand < 20)
                      {
                        blueToothSerial.print("check:A: deur open");
                      }
                    if (afstand > 20)
                      {
                        blueToothSerial.print("check:A: deur toe");
                      }
                   }
               } 
        break;
      }              
      else {
        if (c !=  DIV_CMD_CHAR) {
          inText += c; 
         
          delay(5);
                                }
       
      }
    }
  }
   }
void setupBlueToothConnection()
       {          
  blueToothSerial.begin(9600);  
  blueToothSerial.print("AT");
  delay(400); 
  blueToothSerial.print("AT+DEFAULT");         
  delay(2000); 
  blueToothSerial.print("AT+NAMERaf");    
  delay(400);
  blueToothSerial.print("AT+PIN1234");           
  delay(400);
  blueToothSerial.print("AT+AUTH1");             //
  delay(400);    
  blueToothSerial.flush();
       }
       
 void set_digitalwrite(int pin_num, int pin_value)
        {
          switch (pin_num) {
                            case 13:
                            pinMode(13, OUTPUT);
                            digitalWrite(13, pin_value);  
                            // add your code here 
                if (modewaarde == false)
                {
                            //blueToothSerial.print("openen.");
                            delay(1);
                            if (teller < 30 )
                                           {
                                            
                                           }

                             else          {    
                                           for (teller= 205;teller > 20; teller --)
                                              {
                                               myservo.write(teller);
                                               delay(10);
                                               
                                              
                                               stap = stap + 1;
                                                if ( stap == 40)
                                                      {
                                                      blueToothSerial.print(teller);
                                                      delay(1);
                                                      stap = 0;
                                                      } 
                                               
                                               }
                                               delay(400);
                                               blueToothSerial.print("com:M: deur open");
                                                      delay(1);
                                            }
                           }
                            break;
                            case 12:
                            pinMode(12, OUTPUT);
                            digitalWrite(12, pin_value);   
                            // add your code here  
                          // blueToothSerial.print("sluiten.");
                           delay(1);
                   if (modewaarde == false)
                   {
                           if (teller > 190 )
                                           {
                                            
                                           }

                             else          { 
                                              for (teller= 20; teller < 205;teller ++)
                                                {
                                                      myservo.write(teller);
                                                      delay(10);
                                                      stap = stap + 1;
                                                      if ( stap == 40)
                                                      {
                                                      blueToothSerial.print(teller);
                                                      delay(1);
                                                      stap = 0;
                                                      } 
                                                 } 
                                                 delay(400);
                                                 blueToothSerial.print("com:M: deur gesloten");
                                                      delay(1); 
                                               } 
                         }
                          break;
                          case 11:
                           pinMode(11, OUTPUT);
                           digitalWrite(11, pin_value);         
                           // add your code here 
  
                           break;
                          case 10:
                           pinMode(10, OUTPUT);
                          digitalWrite(10, pin_value);
                          // add your code here 
   
                         break;
                        case 9:
                         pinMode(9, OUTPUT);
                         digitalWrite(9, pin_value);         
                         // add your code here 
   
    
   
                         break;
                         case 8:
                          pinMode(8, OUTPUT);
                          digitalWrite(8, pin_value); 
                           break;
                          case 7:
                         pinMode(7, OUTPUT);
                         digitalWrite(7, pin_value); 
                        // blueToothSerial.println(analogRead(A5));  
                      // add your code here 
                           break;
                         case 6:
                         pinMode(6, OUTPUT);
                         digitalWrite(6, pin_value);         
                         // add your code here 
                          break;
                         case 5:
                        pinMode(5, OUTPUT);
                       digitalWrite(5, pin_value); 
                      // add your code here       
                          break;
                      case 4:
                         pinMode(4, OUTPUT);
                        digitalWrite(4, pin_value);         
                      // add your code here 
                        break;
                       case 3:
                         pinMode(3, OUTPUT);
                          digitalWrite(3, pin_value);         
                       // add your code here 
                         break;
                        case 2:
                        pinMode(2, OUTPUT);
                        digitalWrite(2, pin_value); 
                          // add your code here       
                          break;      
    // default: 
    // if nothing else matches, do the default
    // default is optional
  } 
}