   if (serial_out[0])
     Serial.write(serial_out);
 
- //   Serial.print("freeMemory()=");
- // Serial.println(freeMemory());
+  //   Serial.print("freeMemory()=");
+  // Serial.println(freeMemory());
 
   //delay(1000);
 }

}
//--------------------------------------------------------------------------------------------
//Main loop
void loop() 
{
  char serial_out[100];

// check to see if there are new results from ultrasonic sensor
  ultrasonic_sensors_controller.update_results(serial_out);
  if (serial_out[0])
    Serial.write(serial_out);
  
  if (Serial.available() || current_buffer[0]) {
    int num_read = 0;
    char serial_buffer[65];
  
    //num_read = Serial.readBytes(serial_buffer, 64); //Read up to 64 bytes
    
    int incomingByte = Serial.read();
      if (incomingByte != -1){
      serial_buffer[0] = incomingByte;
      num_read = 1;
    }
    serial_buffer[num_read] = 0;// terminate the string
    if (serial_buffer[0] || current_buffer[0]){
      if (serial_buffer[0])
        strcat(current_buffer, serial_buffer);
      
      #ifdef DEBUG
        Serial.write("initial buffer is=");
        Serial.write(current_buffer);
        Serial.println();
      #endif
      
      // parse from the beginning until I find a M, D, L, S, A, P, B, U, G, T
      int current_buffer_length = strlen(current_buffer);
      for (int i = 0; i < current_buffer_length; i++)
        if ((current_buffer[i] >= 'A' && current_buffer[i] <= 'Z') || (current_buffer[i] >= 'a' && current_buffer[i] <= 'z')){// a command
          // find the terminal character #
          int j = current_buffer_length - 1;
          for (; j > i && current_buffer[j] != '#'; j--);// parse (from the end) until I find the termination char
          if (j > i){

              #ifdef DEBUG
                char tmp_str[64];
                strncpy(tmp_str, current_buffer + i, j - i);
                tmp_str[j - i] = 0;
                Serial.write("current command is=");
                Serial.write(tmp_str);
                Serial.println();
              #endif

             parse_and_execute_commands(current_buffer + i, j - i, serial_out);
             if (serial_out[0])
               Serial.write(serial_out);
             
                    
              // remove the current executed command
              strcpy(current_buffer, current_buffer + j + 1);// not sure if this is good due to overlaps
              
              #ifdef DEBUG
                Serial.write("buffer left=");
                Serial.write(current_buffer);
                Serial.write("\n----------------\n");
                //Serial.println(strlen(current_buffer)); // buffer length
              #endif

            break; //for i
          }
          else{// the string is not completed ... so I must wait for more...
            break; // for i
          }
        }

bool first_start;

//--------------------------------------------------------------------------------------------
void setup()
{
  first_start = 0;
  strcpy(firmware_version, "2016.05.10.0");

  current_buffer[0] = 0;

  Serial.begin(115200); //Open Serial connection

#ifdef DEBUG
  Serial.write("Commands are:");
  Serial.println(F("T# // test connection. Returns T#."));
  Serial.println(F("MSx y# // Moves stepper motor x with y steps. If y is negative the motor runs in the opposite direction. The motor remains locked at the end of the movement. Outputs MSx d# when motor rotation is over. If movement was complete, then d is 0, otherwise is the distance to go."));
  Serial.println(F("MDx y# // Moves dc motor x for y miliseconds. Outputs MDx d# when motor rotation is over. If movement was complete, then d is 0, otherwise is the time to go."));
  Serial.println(F("MVx y# // Moves servo motor x for y steps.");
                 Serial.println(F("HSx# // Moves stepper motor x to home position. The first sensor in the list of sensors will establish the home position. The motor does nothing if no sensor is attached. Returns HSx#."));
                 Serial.println(F("HDx# // Moves DC motor x to home position. The first sensor in the list of sensors must be the button which establish the home position. The motor does nothing if no sensor is attached. Returns HDx#."));
                 Serial.println(F("HVx# // Moves servo motor x to home position.  Returns HVx#.");

                                Serial.println(F("DSx#  // Disables stepper motor x. Outputs DSx# when done."));
                                Serial.println(F("DDx#  // Disables dc motor x. Outputs DDx# when done."));

                                Serial.println(F("Lx#  // Locks stepper motor x. Outputs Lx# when done."));

                                Serial.println(F("SSx s a# // Sets speed of stepper motor x to s and the acceleration to a."));
                                Serial.println(F("SDx s# // Sets speed of motor x to s."));
                                Serial.println(F("SPx min max home# // Sets the parameters of a potentiometer. Min and max are the limits where it can move and home is from where we bring the robot when we start."));

                                Serial.println(F("ASx n Py Bz ... # // Attach to stepper motor x a list of n sensors (like Potentiometer y, Button z etc)."));
                                Serial.println(F("ADx n Py Bz ... # // Attach to dc motor x a list of n sensors (like Potentiometer y, Button z etc)."));
                                Serial.println(F("AVx n Py Bz ... # // Attach to servo motor x a list of n sensors (like Potentiometer y, Button z etc)."));

                                Serial.println(F("Ux# // Gets the distance as measured by the ultrasonic sensor x. Outputs Ux d# when the reading has been completed. Note that this works in \"background\" and can output after some time (and in the meantime the program can do something else."));
                                Serial.println(F("Bx# // Gets the status of the button x. Outputs Bx s#"));
                                Serial.println(F("Px# // Gets the position of the potentiometer x. Outputs Px p#"));
                                Serial.println(F("Ix# // Gets the value of infrared sensor x. Outputs Ix v#"));

                                Serial.println(F("GSx# // Gets the parameters for stepper motor x: speed acceleration num_sensors sensor_index1, sensor_type1 sensor_index1, sensor_type1. Outputs GSx s a 1 0 0#"));
                                Serial.println(F("GDx# // Gets the parameters for dc motor x: speed num_sensors sensor_index1, sensor_type1 sensor_index1, sensor_type1. Outputs GDx s a 1 0 0#"));
                                Serial.println(F("GVx# // Gets the parameters for dc motor x: speed num_sensors sensor_index1, sensor_type1 sensor_index1, sensor_type1. Outputs GVx s a 1 0 0#"));
                                Serial.println(F("GPx# // Gets the parameters for potentiometer x: min max home. Outputs PPx l u h#"));
                                Serial.println(F("GUx# // Gets the parameters for ultrasound x: trig_pin echo_pin. Outputs UPx t e#"));

                                Serial.println(F("CS n d1 s1 e1 d2 s2 e2# // Creates the stepper motors controller and set some of its parameters. n is the number of motors, d, s, e are dir, step and enable pins. Outputs CS# when done."));
                                Serial.println(F("CD n p1 d11 d12 e1 p2 d21 d22 e2# // Creates the dc motors controller and set some of its parameters. n is the number of motors, p is the pwm_pin, d1 and d2 are the direction pins and e is the enable pins. Outputs CD# when done."));
                                Serial.println(F("CV n# // Creates the servo motors controller and set some of its parameters. n is the number of motors, Outputs CV# when done."));

                                Serial.println(F("CP n p1 l1 h1 _h1 _d1 p2 l2 h2 _h2 _d2# // Creates the potentiometers controller and set some of its parameters. n is the number of potentiometers, p is the output pin, l, h and _h are bottom, upper and home position, _d is the directon of the sensor relative to the direction in which the motor is moving. Outputs CP# when done."));
                                Serial.println(F("CU n t1 e1 t2 e2# // Creates the ultrasonic controller and set some of its parameters. n is the number of sonars, t and e are trigger and echo pins. Outputs CU# when done."));
                                Serial.println(F("CI n pin1 min1 max1 home1 dir1 pin2 min2 max2 home2 dir2# // Creates the infrared controller and set some of its parameters. n is the number of infrared sensors, p is the analog pin and min, max and home are the lower, upper bounds and home position of this sensor. Outputs CI# when done."));
                                Serial.println(F("CB n p1 p2# // Creates the buttons controller and set some of its parameters. n is the number of button sensors, p is the digital pin. Outputs CB# when done."));


                                Serial.println(F("V# // Outputs version string. eg: 2016.01.17.0#"));


                                Serial.println(F("Motor index is between 0 and num_motors - 1"));

                                Serial.println();
#endif
                                Serial.write("T#");// initialization is over; must check for T# string (which is the alive test)
}
               //--------------------------------------------------------------------------------------------
               void parse_and_execute_commands(char* tmp_str, byte str_length, char *serial_out)
{
  byte i = 0;
  serial_out[0] = 0;
  while (i < str_length) {
    // can be more than 1 command in a string, so I have to check again for a letter
    if ((tmp_str[i] >= 'A' && tmp_str[i] <= 'Z') || (tmp_str[i] >= 'a' && tmp_str[i] <= 'z')) {

      if (tmp_str[i] == 'M' || tmp_str[i] == 'm') { // moves motor
        if (tmp_str[i + 1] == 'S' || tmp_str[i + 1] == 's') { // stepper motor
          int motor_index, num_steps;
          int num_read = sscanf(tmp_str + i + 2, "%d%d", &motor_index, &num_steps);
          if (num_read == 2) {
            stepper_motors_controller.move_motor(motor_index, num_steps);
            is_command_running = 1;
            i += 5;
