byte   gSeq = 0;
String gInputString = ""; 
String gOutputString = "";
String gInputBuffer = "";
boolean gIsInputSuccess = true;

//
int DIRA = 12;
int DIRB = 13;
int PWMA = 3;
int PWMB = 11;

void setup(){
  Serial.begin(115200);
  pinMode(DIRA, OUTPUT);
  pinMode(DIRB, OUTPUT);
}

void loop(){
//  sendMessage("loop:" + String(millis()));
  int t = millis();
  int a0 = analogRead(0);  
  int a1 = analogRead(1);
  int a2 = analogRead(2); // angle for motor1
  int a3 = analogRead(3); // angle for motor2
  setData("/1/ar/1_a.xhtml#t",  t);
  setData("/1/ar/1_a.xhtml#a0", a0);
  setData("/1/ar/1_a.xhtml#a1", a1);
  setData("/1/ar/1_a.xhtml#a2", a2);
  setData("/1/ar/1_a.xhtml#a3", a3);
  
  if (5 < a2 && a2 < 1018)
  { 
    int s = getData("/4/ar/4_z.xhtml#m1");
    if (gIsInputSuccess)
    {
      if (s>0)
      {
        digitalWrite(DIRA, HIGH);
        analogWrite(PWMA,s);
      }
      else
      {
        digitalWrite(DIRA, LOW);
        analogWrite(PWMA,-s);
      }
    }
    else
    {
        analogWrite(PWMA,0);      
    }
  }

  if (5 < a3 && a3 < 1018)
  { 
    int s = getData("/4/ar/4_z.xhtml#m2");
    if (gIsInputSuccess)
    {
      if (s>0)
      {
        digitalWrite(DIRB, HIGH);
        analogWrite(PWMB,s);
      }
      else
      {
        digitalWrite(DIRB, LOW);
        analogWrite(PWMB,-s);
      }
    }
    else
    {
        analogWrite(PWMB,0);      
    }    
  }
  
//  delay(10);
}

void sendMessage(String message)
{
  gOutputString = "M" + message;
  serialSend();
}

void setData(String path, int value)
{
  gOutputString = "S" + path + "," + String(value);
  serialSend();  
}

int getData(String path)
{
  gOutputString = "G" + path;  
  serialSendAndLoad();
  if (gIsInputSuccess)
    return int(gInputString.toInt());
  else
    return 0;  
}

void serialSend()
{
  gSeq ++;
  if (gSeq > 99)
  {
    gSeq = 1;
  }
  if (gSeq < 10)
  {
    Serial.print("0");
  }
  Serial.print(gSeq, DEC);
  Serial.println(gOutputString);
}

void serialSendAndLoad()
{
  serialSend();
  gIsInputSuccess = false;
  unsigned long timeout = millis() + 1000;
  gInputString = "";
  int isDataRecieved = false;
  while(!isDataRecieved && millis() < timeout)
  {
    while (Serial.available() > 0)
    {
      // get the new byte:
      char inChar = (char)Serial.read(); 
      // add it to the inputString:
      if (inChar != '\n' && inChar != '\r')
      {
        gInputBuffer += inChar;
      }
      // if the incoming character is a newline, set a flag
      // so the main loop can do something about it:
      if (inChar == '\n') 
      {
        //check input data
        if (gInputBuffer.length() >= 2)
        {
          String s = gInputBuffer.substring(0,2);
          if (s.toInt() == gSeq)
          {          
            gInputString = gInputBuffer.substring(2);
            isDataRecieved = true;
            gIsInputSuccess = true;
          }
          else
          {
            gIsInputSuccess = false;
          }
        }
        gInputBuffer = "";
      }
    }
  }
}


