// ****************************************************************************************** //
// SETTINGS                                                                                   //
// ****************************************************************************************** //
// SERIAL COMMUNICATION 
#include <SoftwareSerial.h>
#define RX    1               // For beaver board = 1  // For Master = 8
#define TX    0               // For beaver board = 0  // For Master = 7
SoftwareSerial Serial(RX,TX);


// SERVO MOTORS 
int pin_servo = 7;           // Pin connected to SO
int tservo    = 2000;        // Time beavers will remain up/down (i.e. resfresh time in msec)
                             // For some reason it doesnt wait this time. I've check the clock
                             // and everything is fine... weird

// SONAR
const int trigPin = 2;       // Pin connected to trigger
const int echoPin = 3;       // Pin connecter to receiver (i.e. echo)
long      duration;          // Time lapse for echo to bounce back
int       distance;          // Computed distance
int       readNum = 5;       // Make 5 readings and then take the average
int       minDis  = 20;      // Minimum distance (cm)

// Other variables
char  ID       = '4';           // Beaver ID
int   imax     = tservo/20;     // 20 = 20 msec = duration of PWM cycle
int   i        = 0;             // Will need this for forloops
char  POS      = 'd';           // State variable for the beaver head position (u= up, d=down)
char  ACT      = 'p';           // This variable controls whether the board can talk/control motor (passive, active)
char  ACK      = 'y';           // We use this variable to acknowledge communication between boards 




// ****************************************************************************************** //
// SETUP                                                                                      //
// ****************************************************************************************** //
void setup() {
  // Set pin modes
  pinMode(pin_servo, OUTPUT);
  pinMode(trigPin,   OUTPUT);
  pinMode(echoPin,   INPUT);
  pinMode(TX,        INPUT_PULLUP);
  
  // Move head downwards
  servoDOWN();

  // Start serial communication
  Serial.begin(9600); //Sets the data rate in bits per second (baud) for serial data transmission
  //Serial.println("Init beaver");


}

// ****************************************************************************************** //
// LOOP                                                                                       //
// ****************************************************************************************** //
void loop() {

  if(Serial.available()){             // If data is available ...
    char val = Serial.read();         // read it!
    
    if(ACT == 'p'){                   // If this board is pasive ...
      if(val == ID){                  // check if Master/GUI is calling it ...
        ACT = 'a';                    // if so, make it active, ...
        //Serial.println("Active");
        pinMode(TX, OUTPUT);
        Serial.write("A");            //  and send acknowledge to master!
      }
    }

    if(ACT == 'a'){                   // If the board is active ...
      if(val == 'u'){                    // Check if the comming order ask to move beaver up (u), ...
        POS = 'u';                       // if so, do it, ...
        ACT = 'p';                       // make the board passive, ...
        //Serial.println("Up-Pasive");
        Serial.write("U");              // and send acknowledge to master (Up)!
        pinMode(TX, INPUT_PULLUP);
      }
      if(val == 'd'){                    // Check if the comming order ask to move beaver down (d), ...
        POS = 'd';                       // if so, do it, ...
        ACT = 'p';                       // make the board passive, ...
        //Serial.println("Down-Pasive");
        Serial.write("D");              // and send acknowledge to master (Down)!
        pinMode(TX, INPUT_PULLUP);
      }
      if(val == 'w') {                   // Check  if master asks where (w) the hand/cat is, ...
        distance = distSONAR();          // if so, find distance to hand/cat (use sonar readigs), ...
        //Serial.print("Distance: ");        // Print the distance on the Serial Monitor
        //Serial.println(distance);      
        ACT = 'p';                       // make the board passive, ...
        //Serial.write(distance >> 8);     // and send the info (high byte)
        Serial.write(lowByte(distance));
        //Serial.write(distance & 0xFF);   // (low byte)!
        Serial.write(highByte(distance));
        pinMode(TX, INPUT_PULLUP);
      }
    } 
  }

  if(POS == 'u'){                      // If the board is active
    distance = distSONAR();              // Find distance to hand/cat (use sonar readigs)
    //Serial.print("Distance: ");        // Print the distance on the Serial Monitor
    //Serial.println(distance);       
    if(distance < minDis){               // In case of danger ...
      POS = 'd';                           // hide the head, ...
      pinMode(TX, OUTPUT);
      Serial.write("H");                   //  and send warning to master (Hide)!
      pinMode(TX, INPUT_PULLUP);
    } 
  }  


  if(POS == 'u'){servoUP();}           // Keep head up

  if(POS == 'd'){servoDOWN();}         // Keep head down

  

  
}

// ****************************************************************************************** //
// OTHER FUNCTIONS                                                                            //
// ****************************************************************************************** //
// MOVE SERVO UP
void servoUP() {                               // Use PWM to control servo
  for (i = 0; i < imax; ++i){
    digitalWrite(pin_servo,LOW);
    delayMicroseconds(19900);
    digitalWrite(pin_servo,HIGH);
    delayMicroseconds(100);
  }
}

// MOVE SERVO DOWN
void servoDOWN() {                            // Use PWM to control servo
  for (i = 0; i < imax; ++i){
    digitalWrite(pin_servo,LOW);
    delayMicroseconds(18500);
    digitalWrite(pin_servo,HIGH);
    delayMicroseconds(1500);
  }
}

// MEASURE DISTANCE
int distSONAR() {                          // Take a few  sonar readings and return the average.
  int distSum    = 0;                      // Each reading takes aprox 25 msec.
                                           // Measure the current time using millis().
                                           // (e.g. unsigned long StartTime = millis();)                             
  for (i = 0; i < readNum; ++i){ 
  // Take a Reading                                           
  digitalWrite(trigPin, LOW);              // Clears the trigPin
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);             // Sets the trigPin on HIGH state for 10 micro seconds
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);       // Reads the echoPin, returns the sound wave travel time in microseconds
  distSum += duration*0.034/2;             // Calculating the distance and add to the sum
  }

  distance = distSum / readNum;            // Compute the average
  return distance;
}