   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
          }
        }

          else
            i++;// error on incomplete string (does nothing)
        }
        else if (tmp_str[i + 1] == 'D' || tmp_str[i + 1] == 'd') { // DC motor
          int motor_index, num_miliseconds;
          int num_read = sscanf(tmp_str + i + 2, "%d%d", &motor_index, &num_miliseconds);
          if (num_read == 2) {
            dc_motors_controller_TB6612FNG.move_motor(motor_index, num_miliseconds);
            is_command_running = 1;
            i += 5;
          }
          else
            i++;// error on incomplete string (does nothing)
        }
        else
          i++; // incomplete string
        continue;
      }

      // potentiometer
      if (tmp_str[i] == 'P' || tmp_str[i] == 'p') {
        int sensor_index;
        sscanf(tmp_str + i + 1, "%d", &sensor_index);
        int sensor_value = potentiometers_controller.get_position(sensor_index);
        sprintf(serial_out, "P%d %d#", sensor_index, sensor_value);
        i += 2;
        continue;
      }

      // ultrasonic
      if (tmp_str[i] == 'U' || tmp_str[i] == 'u') {
        int sensor_index;
        sscanf(tmp_str + i + 1, "%d", &sensor_index);
        ultrasonic_sensors_controller.trigger(sensor_index);
        //sprintf(serial_out, "U%d %d#", sensor_index, sensor_value);

        i += 2;
        continue;
      }

      // infrared
      if (tmp_str[i] == 'I' || tmp_str[i] == 'i') {
        int sensor_index;
        sscanf(tmp_str + i + 1, "%d", &sensor_index);
        int sensor_value = infrared_sensors_controller.get_signal_strength(sensor_index);
        sprintf(serial_out, "I%d %d#", sensor_index, sensor_value);
        i += 2;
        continue;
      }

      // disables motor
      if (tmp_str[i] == 'D' || tmp_str[i] == 'd') {
        int motor_index;
        if (tmp_str[i + 1] == 'S' || tmp_str[i + 1] == 's') { // disables stepper motor
          sscanf(tmp_str + i + 2, "%d", &motor_index);
          stepper_motors_controller.disable(motor_index);
          sprintf(serial_out, "DS%d#", motor_index);
        }
        else if (tmp_str[i + 1] == 'D' || tmp_str[i + 1] == 'd') { // disables DC motor
          sscanf(tmp_str + i + 2, "%d", &motor_index);
          dc_motors_controller_TB6612FNG.disable(motor_index);
          sprintf(serial_out, "DD%d#", motor_index);
        }
        i += 4;
        continue;
      }

      // locks motor
      if (tmp_str[i] == 'L' || tmp_str[i] == 'l') {
        int motor_index;
        sscanf(tmp_str + i + 1, "%d", &motor_index);
        stepper_motors_controller.lock(motor_index);
        sprintf(serial_out, "L%d#", motor_index);
        i += 2;
        continue;
      }

      // go home
      if (tmp_str[i] == 'H' || tmp_str[i] == 'h') {
        int motor_index;
        if (tmp_str[i + 1] == 'S' || tmp_str[i + 1] == 's') { // go stepper home
          sscanf(tmp_str + i + 2, "%d", &motor_index);
          stepper_motors_controller.go_home(motor_index, &potentiometers_controller);
          sprintf(serial_out, "HS%d#", motor_index);
        }
        else if (tmp_str[i + 1] == 'D' || tmp_str[i + 1] == 'd') { // go stepper home
          sscanf(tmp_str + i + 2, "%d", &motor_index);
          dc_motors_controller_TB6612FNG.go_home(motor_index, &buttons_controller);
          sprintf(serial_out, "HD%d#", motor_index);
        }
        is_command_running = 1;
        i += 4;
        continue;
      }

      // sets something
      if (tmp_str[i] == 'S' || tmp_str[i] == 's') {
        if (tmp_str[i + 1] == 'S' || tmp_str[i + 1] == 's') { // stepper motor speed and acceleration
          int motor_index;
          int motor_speed, motor_acceleration;
          sscanf(tmp_str + i + 2, "%d%d%d", &motor_index, &motor_speed, &motor_acceleration);
          stepper_motors_controller.set_speed_and_acceleration(motor_index, motor_speed, motor_acceleration);
          i += 5;
        }
        else if (tmp_str[i + 1] == 'P' || tmp_str[i + 1] == 'p') { // potentiometer min max home
          int pot_index, pot_min, pot_max, pot_home, pot_pin, pot_dir;
          sscanf(tmp_str + i + 2, "%d%d%d%d%d%d", &pot_index, &pot_pin, &pot_min, &pot_max, &pot_home, &pot_dir);
          potentiometers_controller.set_params(pot_index, pot_pin, pot_min, pot_max, pot_home, pot_dir);
          i += 7;
        }
        else if (tmp_str[i + 1] == 'D' || tmp_str[i + 1] == 'd') { // dc motor speed
          int motor_index;
          int motor_speed;
          sscanf(tmp_str + i + 2, "%d%d", &motor_index, &motor_speed);
          dc_motors_controller_TB6612FNG.set_speed(motor_index, motor_speed);
          i += 5;
        }
        continue;
      }

      if (tmp_str[i] == 'A' || tmp_str[i] == 'a') { // attach sensors to motors
        int motor_index, num_sensors;
        char motor_type = tmp_str[i];
        sscanf(tmp_str + i + 2, "%d%d", &motor_index, &num_sensors);
        if (motor_type == 'S' || motor_type == 's') // attach to stepper motors controller
          stepper_motors_controller.set_num_attached_sensors(motor_index, num_sensors);
        else if (motor_type == 'D' || motor_type == 'd') // attach to DC motors controller
          dc_motors_controller_TB6612FNG.set_num_attached_sensors(motor_index, num_sensors);

        // now I have to add sensors one by one
        int j = i + 4;
        i += 5;
        while (j < str_length) {
          if (tmp_str[j] == 'P' || tmp_str[j] == 'p') {
            int sensor_index;
            sscanf(tmp_str + j + 1, "%d", &sensor_index);
            if (motor_type == 'S' || motor_type == 's') // attach to stepper motors controller
              stepper_motors_controller.add_sensor(motor_index, POTENTIOMETER, sensor_index);
            else if (motor_type == 'D' || motor_type == 'd') // attach to DC motors controller
              dc_motors_controller_TB6612FNG.add_sensor(motor_index, POTENTIOMETER, sensor_index);
            i += 2;
          }
          else if (tmp_str[j] == 'B' || tmp_str[j] == 'b') {
            int sensor_index;
            sscanf(tmp_str + j + 1, "%d", &sensor_index);
            if (motor_type == 'S' || motor_type == 's') // attach to stepper motors controller
              stepper_motors_controller.add_sensor(motor_index, BUTTON, sensor_index);
            else if (motor_type == 'D' || motor_type == 'd') // attach to DC motors controller
              dc_motors_controller_TB6612FNG.add_sensor(motor_index, BUTTON, sensor_index);
            i += 2;
          }
          else if (tmp_str[j] == 'I' || tmp_str[j] == 'i') {
            int sensor_index;
            sscanf(tmp_str + j + 1, "%d", &sensor_index);
            if (motor_type == 'S' || motor_type == 's') // attach to stepper motors controller
              stepper_motors_controller.add_sensor(motor_index, INFRARED, sensor_index);
            else if (motor_type == 'D' || motor_type == 'd') // attach to DC motors controller
              dc_motors_controller_TB6612FNG.add_sensor(motor_index, INFRARED, sensor_index);
            i += 2;
          }
          else
            i++;
          j++;
        }
        i++;
        continue;
      }

      if (tmp_str[i] == 'G' || tmp_str[i] == 'g') { // for debugging purpose
        if (tmp_str[i + 1] == 'S' || tmp_str[i + 1] == 's') { // gets stepper motor speed and acceleration
          int motor_index;
          float motor_speed, motor_acceleration;
          sscanf(tmp_str + i + 2, "%d", &motor_index);
          stepper_motors_controller.get_speed_and_acceleration(motor_index, &motor_speed, &motor_acceleration);
          sprintf(serial_out, "GS%d %d %d#", motor_index, (int)motor_speed, (int)motor_acceleration);
          i += 4;
        }
        else if (tmp_str[i + 1] == 'D' || tmp_str[i + 1] == 'd') { // gets stepper motor speed and acceleration
          int motor_index;
          byte motor_speed;
          sscanf(tmp_str + i + 2, "%d", &motor_index);
          dc_motors_controller_TB6612FNG.get_speed(motor_index, &motor_speed);
          sprintf(serial_out, "GD%d %d#", motor_index, motor_speed);
          i += 4;
        }
        else if (tmp_str[i + 1] == 'P' || tmp_str[i + 1] == 'p') { // get potentiometer min max home
          int pot_index, pot_min, pot_max, pot_home, pot_dir;
          byte pot_pin;
          sscanf(tmp_str + i + 2, "%d", &pot_index);
          potentiometers_controller.get_params(pot_index, &pot_pin, &pot_min, &pot_max, &pot_home, &pot_dir);
          sprintf(serial_out, "GP%d %d %d %d %d %d#", pot_index, (int)pot_pin, pot_min, pot_max, pot_home, pot_dir);
          i += 5;
        }
        else if (tmp_str[i + 1] == 'U' || tmp_str[i + 1] == 'u') { // get potentiometer min max home
          int ultrasound_index;
          byte trig_pin, echo_pin;
          sscanf(tmp_str + i + 2, "%d", &ultrasound_index);
          ultrasonic_sensors_controller.get_sensor_pins(ultrasound_index, &trig_pin, &echo_pin);
          sprintf(serial_out, "GU%d %d %d#", ultrasound_index, trig_pin, echo_pin);
          i += 4;
        }
        else
          i++;// incomplete string
        continue;
      }
      // test connection
      if (tmp_str[i] == 'T' || tmp_str[i] == 't') {
        sprintf(serial_out, "T#");
        i += 2;
        continue;
      }
      // version number
      if (tmp_str[i] == 'V' || tmp_str[i] == 'v') {
        sprintf(serial_out, "%s#", firmware_version);
        i += 2;
        continue;
      }

      // create something
      if (tmp_str[i] == 'C' || tmp_str[i] == 'c') {
        if (tmp_str[i + 1] == 'S' || tmp_str[i + 1] == 's') { // create a list of stepper motors
          if (!stepper_motors_controller.is_running()) {
            int num_motors = 0;

            int num_consumed = 0;
            sscanf(tmp_str + i + 3, "%d%n", &num_motors, &num_consumed);

            int num_consumed_total = 3 + num_consumed;
            stepper_motors_controller.set_num_motors(num_motors);
            for (int k = 0; k < num_motors; k++) {
              int _step_pin, _dir_pin, _enable_pin;
              sscanf(tmp_str + i + num_consumed_total, "%d%d%d%n", &_dir_pin, &_step_pin, &_enable_pin, &num_consumed);
              stepper_motors_controller.set_pins(k, _dir_pin, _step_pin, _enable_pin);
              num_consumed_total += num_consumed + 1;
            }
            stepper_motors_controller.disable_all();
            sprintf(serial_out, "CS#");
            i += num_consumed_total;
          }
        }
        else if (tmp_str[i + 1] == 'D' || tmp_str[i + 1] == 'd') { // create a list of DC motors
          if (!dc_motors_controller_TB6612FNG.is_running()) {
            int num_motors = 0;

            int num_consumed = 0;
            sscanf(tmp_str + i + 3, "%d%n", &num_motors, &num_consumed);

            int num_consumed_total = 3 + num_consumed;
            dc_motors_controller_TB6612FNG.set_num_motors(num_motors);
            for (int k = 0; k < num_motors; k++) {
              int pwm_pin, dir_pin1, dir_pin2, enable_pin;
              sscanf(tmp_str + i + num_consumed_total, "%d%d%d%d%n", &pwm_pin, &dir_pin1, &dir_pin2, &enable_pin, &num_consumed);
              dc_motors_controller_TB6612FNG.set_motor_pins(k, pwm_pin, dir_pin1, dir_pin2, enable_pin);
              num_consumed_total += num_consumed + 1;
            }

            sprintf(serial_out, "CD#");
            i += num_consumed_total;
          }
        }
        else if (tmp_str[i + 1] == 'U' || tmp_str[i + 1] == 'u') { // create a list of ultrasonic sensors

          int num_sonars = 0;

          int num_consumed = 0;
          sscanf(tmp_str + i + 3, "%d%n", &num_sonars, &num_consumed);

          int num_consumed_total = 3 + num_consumed;
          ultrasonic_sensors_controller.set_num_sensors(num_sonars);
          for (int k = 0; k < num_sonars; k++) {
            int _trig_pin, _echo_pin;
            sscanf(tmp_str + i + num_consumed_total, "%d%d%n", &_trig_pin, &_echo_pin, &num_consumed);
            ultrasonic_sensors_controller.set_sensor_pins(k, _trig_pin, _echo_pin);
            num_consumed_total += num_consumed + 1;
          }

          sprintf(serial_out, "CU#");
          i += num_consumed_total;
        }
        else if (tmp_str[i + 1] == 'P' || tmp_str[i + 1] == 'p') { // create a list of potentiometers

          int num_potentiometers = 0;

          int num_consumed = 0;
          sscanf(tmp_str + i + 3, "%d%n", &num_potentiometers, &num_consumed);

          int num_consumed_total = 3 + num_consumed;
          potentiometers_controller.set_num_sensors(num_potentiometers);
          for (int k = 0; k < num_potentiometers; k++) {
            int _out_pin;
            int _low, _high, _home, _direction;
            sscanf(tmp_str + i + num_consumed_total, "%d%d%d%d%d%n", &_out_pin, &_low, &_high, &_home, &_direction, &num_consumed);
            potentiometers_controller.set_params(k, _out_pin, _low, _high, _home, _direction);
            num_consumed_total += num_consumed + 1;
          }

          sprintf(serial_out, "CP#");
          i += num_consumed_total;
        }
        else if (tmp_str[i + 1] == 'I' || tmp_str[i + 1] == 'i') { // create a list of infrared sensors

          int num_infrareds = 0;

          int num_consumed = 0;
          sscanf(tmp_str + i + 3, "%d%n", &num_infrareds, &num_consumed);

          int num_consumed_total = 3 + num_consumed;
          infrared_sensors_controller.set_num_sensors(num_infrareds);
          for (int k = 0; k < num_infrareds; k++) {
            int out_pin;
            int min_pos, max_pos, home_pos, _direction;
            int num_read = sscanf(tmp_str + i + num_consumed_total, "%d%d%d%d%d%n", &out_pin, &min_pos, &max_pos, &home_pos, &_direction, &num_consumed);
            if (num_read == 5)
              infrared_sensors_controller.set_params(k, out_pin, min_pos, max_pos, home_pos, _direction);
            else
              sprintf(serial_out, "E#");
            num_consumed_total += num_consumed + 1;
          }
          sprintf(serial_out, "CI#");
          i += num_consumed_total;
        }
        else if (tmp_str[i + 1] == 'B' || tmp_str[i + 1] == 'b') { // create a list of button sensors

          int num_buttons = 0;

          int num_consumed = 0;
          sscanf(tmp_str + i + 3, "%d%n", &num_buttons, &num_consumed);

          int num_consumed_total = 3 + num_consumed;
          buttons_controller.set_num_sensors(num_buttons);
          for (int k = 0; k < num_buttons; k++) {
            int _pin;
            sscanf(tmp_str + i + num_consumed_total, "%d%n", &_pin, &num_consumed);
            buttons_controller.set_params(k, _pin);
            num_consumed_total += num_consumed + 1;
          }
          sprintf(serial_out, "CB#");
          i += num_consumed_total;
        }
        else
          i++; // incomplete string
        continue;
      }
      else
        i++;// incomplete string
    }
    else
      i++;
