   writeSerial(val.byteVal[2]);
   writeSerial(val.byteVal[3]);
 }
-void sendShort(double value) {
+
+void sendShort(double value){
   writeSerial(3);
   valShort.shortVal = value;
   writeSerial(valShort.byteVal[0]);
   writeSerial(valShort.byteVal[1]);
 }
-short readShort(int idx) {
+
+short readShort(int idx){
   valShort.byteVal[0] = readBuffer(idx);
-  valShort.byteVal[1] = readBuffer(idx + 1);
-  return valShort.shortVal;
-}
-float readFloat(int idx) {
+  valShort.byteVal[1] = readBuffer(idx+1);
+  return valShort.shortVal; 
+}
+
+float readFloat(int idx){
   val.byteVal[0] = readBuffer(idx);
-  val.byteVal[1] = readBuffer(idx + 1);
-  val.byteVal[2] = readBuffer(idx + 2);
-  val.byteVal[3] = readBuffer(idx + 3);
+  val.byteVal[1] = readBuffer(idx+1);
+  val.byteVal[2] = readBuffer(idx+2);
+  val.byteVal[3] = readBuffer(idx+3);
   return val.floatVal;
 }
-long readLong(int idx) {
+
+long readLong(int idx){
   val.byteVal[0] = readBuffer(idx);
-  val.byteVal[1] = readBuffer(idx + 1);
-  val.byteVal[2] = readBuffer(idx + 2);
-  val.byteVal[3] = readBuffer(idx + 3);
+  val.byteVal[1] = readBuffer(idx+1);
+  val.byteVal[2] = readBuffer(idx+2);
+  val.byteVal[3] = readBuffer(idx+3);
   return val.longVal;
 }
 
-void readSensor(int device) {
-  /**************************************************
-      ff 55 len idx action device port slot data a
-      0  1  2   3   4      5      6    7    8
-  ***************************************************/
-  float value = 0.0;
-  int port, pin;
-  port = readBuffer(6);
-  pin = port;
-  switch (device) {
-    case ALIVE: {
-        sendString("ALIVE!");
-        writeSerial(device);
-      }
-      break;
-    case  DIGITAL: {
-        pinMode(pin, INPUT);
-        sendFloat(digitalRead(pin));
-        writeSerial(pin);
-        writeSerial(device);
-      }
-      break;
-    case  ANALOG: {
-        pin = analogs[pin];
-        pinMode(pin, INPUT);
-        sendFloat(analogRead(pin));
-        writeSerial(pin);
-        writeSerial(device);
-      }
-      break;
-    case  PULSEIN: {
-        int pw = readShort(7);
-        pinMode(pin, INPUT);
-        sendShort(pulseIn(pin, HIGH, pw));
-        writeSerial(pin);
-        writeSerial(device);
-      }
-      break;
-    case ULTRASONIC: {
-        int trig = readBuffer(6);
-        int echo = readBuffer(7);
-        pinMode(trig, OUTPUT);
-        digitalWrite(trig, LOW);
-        delayMicroseconds(2);
-        digitalWrite(trig, HIGH);
-        delayMicroseconds(10);
-        digitalWrite(trig, LOW);
-        pinMode(echo, INPUT);
-        float value = pulseIn(echo, HIGH, 30000) / 58.0;
-        writeHead();
-        writeSerial(command_index);
-        sendFloat(value);
-        writeSerial(trig);
-        writeSerial(echo);
-        writeSerial(device);
-      }
-      break;
-    case TIMER: {
-        sendFloat((float)currentTime);
-        writeSerial(device);
-      }
-      break;
-  }
-}
-
+int searchServoPin(int pin){
+  for(int i=0;i<8;i++){
+    if(servo_pins[i] == pin){
+      return i;
+    }
+    if(servo_pins[i]==0){
+      servo_pins[i] = pin;
+      return i;
+    }
+  }
+  return 0;
+}
+
+void setPortWritable(int pin) {
+  if(digitals[pin] == 0) {
+    digitals[pin] = 1;
+    pinMode(pin, OUTPUT);
+  } 
+}
+
+void callOK(){
+  writeSerial(0xff);
+  writeSerial(0x55);
+  writeEnd();
+}
+
+void callDebug(char c){
+  writeSerial(0xff);
+  writeSerial(0x55);
+  writeSerial(c);
+  writeEnd();
+}
+

  lcd.print("with Entry");
}

unsigned char readBuffer(int index) {
  return buffer[index];
}

void writeBuffer(int index, unsigned char c) {
  buffer[index] = c;
}
void writeHead() {
  writeSerial(0xff);
  writeSerial(0x55);
}
void writeEnd() {
  Serial.println();
}
void writeSerial(unsigned char c) {
  Serial.write(c);
}
void readSerial() {
  isAvailable = false;
  if (Serial.available() > 0) {
    isAvailable = true;
    serialRead = Serial.read();
  }
}

void parseData() {

  command_index = (uint8_t)idx;
  int action = readBuffer(4);
  int device = readBuffer(5);
  int port = readBuffer(6);
  switch(action){
    case GET:{
      if(device == ULTRASONIC){
        if(!isUltrasonic) {
          setUltrasonicMode(true);
          trigPin = readBuffer(6);
          echoPin = readBuffer(7);
          digitals[trigPin] = 1;
          digitals[echoPin] = 1;
          pinMode(trigPin, OUTPUT);
          pinMode(echoPin, INPUT);
          delay(50);
        } else {
          int trig = readBuffer(6);
          int echo = readBuffer(7);
          if(trig != trigPin || echo != echoPin) {
            trigPin = trig;
            echoPin = echo;
            digitals[trigPin] = 1;
            digitals[echoPin] = 1;
            pinMode(trigPin, OUTPUT);            
            pinMode(echoPin, INPUT);
            delay(50);
          }
        }
      } else if(port == trigPin || port == echoPin) {
        setUltrasonicMode(false);
        digitals[port] = 0;
      } else {
        digitals[port] = 0;
      }
    }
    break;
    case SET:{
      runModule(device);
      callOK();
    }
    break;
    case RESET:{
      callOK();
    }
    break;
  }
}

void runModule(int device) {
  //0xff 0x55 0x6 0x0 0x1 0xa 0x9 0x0 0x0 0xa
  int port = readBuffer(6);
  int pin = port;

  if(pin == trigPin || pin == echoPin) {
    setUltrasonicMode(false);
  }
  
  switch(device){
    case DIGITAL:{      
      setPortWritable(pin);
      int v = readBuffer(7);
      digitalWrite(pin,v);
    }
    break;
    case PWM:{
      setPortWritable(pin);
      int v = readBuffer(7);
      analogWrite(pin,v);
    }
    break;
    case TONE:{
      setPortWritable(pin);
      int hz = readShort(7);
      int ms = readShort(9);
      if(ms>0) {
        tone(pin, hz, ms);
      } else {
        noTone(pin);
      }
    }
    break;
    case SERVO_PIN:{
      setPortWritable(pin);
      int v = readBuffer(7);
      if(v>=0&&v<=180){
        Servo sv = servos[searchServoPin(pin)];
        sv.attach(pin);
        sv.write(v);
      }
    }
    break;
    case TIMER:{
      lastTime = millis()/1000.0; 
    }
    break;
  }
}

void sendPinValues() {  
  int pinNumber = 0;
  for (pinNumber = 0; pinNumber < 12; pinNumber++) {
    if(digitals[pinNumber] == 0) {
      sendDigitalValue(pinNumber);
      callOK();
    }
  }
  for (pinNumber = 0; pinNumber < 6; pinNumber++) {
    if(analogs[pinNumber] == 0) {
      sendAnalogValue(pinNumber);
      callOK();
    }
  }
  
  if(isUltrasonic) {
    sendUltrasonic();  
    callOK();
  }
}

void setUltrasonicMode(boolean mode) {
  isUltrasonic = mode;
  if(!mode) {
    lastUltrasonic = 0;
  }
}

void sendUltrasonic() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  float value = pulseIn(echoPin, HIGH, 30000) / 29.0 / 2.0;

  if(value == 0) {
    value = lastUltrasonic;
  } else {
    lastUltrasonic = value;
  }
  writeHead();
  sendFloat(value);
  writeSerial(trigPin);
  writeSerial(echoPin);
  writeSerial(ULTRASONIC);
  writeEnd();
}

void sendDigitalValue(int pinNumber) {
  pinMode(pinNumber,INPUT);
  writeHead();
  sendFloat(digitalRead(pinNumber));  
  writeSerial(pinNumber);
  writeSerial(DIGITAL);
  writeEnd();
}

void sendAnalogValue(int pinNumber) {
  writeHead();
  sendFloat(analogRead(pinNumber));  
  writeSerial(pinNumber);
  writeSerial(ANALOG);
  writeEnd();
}

void writeBuffer(int index,unsigned char c){
  buffer[index]=c;
}

void writeHead(){
  writeSerial(0xff);
  writeSerial(0x55);
}

void writeEnd(){
  Serial.println();
}

void writeSerial(unsigned char c){
  Serial.write(c);
}

void sendString(String s){
  int l = s.length();
  writeSerial(4);
  writeSerial(l);
  for(int i=0;i<l;i++){
    writeSerial(s.charAt(i));
  }
}

void sendFloat(float value){ 
  writeSerial(2);
  val.floatVal = value;
  writeSerial(val.byteVal[0]);
