//This code enables to use 14 digital outputs or 6 servos. Ch3,5,6,9,10,11 can be used for both digital output and servo control.
//It seems that different from read() command, readString() command has a wait time and which makes a synchronization of frame rate between arduino and python program.

#include <Servo.h>
Servo servoCh3;//Declar Servo object
Servo servoCh5;//Declar Servo object
Servo servoCh6;//Declar Servo object
Servo servoCh9;//Declar Servo object
Servo servoCh10;//Declar Servo object
Servo servoCh11;//Declar Servo object
void setup(){
  //Serial.begin(9600);
  Serial.begin(115200); //Init serial port as 115200 bps
  Serial.setTimeout(4); //Serial command wait time in milli sec(This waittime almost determine the framerate of arduino), 30, 16
}
int Counter[14];
int DesignatedAngle[14];
int CurrAngle[14];   //14
//lint LoopCnt=0;
char CharArray[17]; //15,100
int Mode[14]; //1:digital output 2:servo 3:digital input
int AnalogInputValue[6];  //Keep the values of analog inputs
char DigitalInputValue[14];  //Keep the values of digital inputs
String OutText; //Transfer the text in this variable to python
int DigitalInOut = 0;
String InputString;
void loop(){  //This loop is synchronized with loop of python
  //LoopCnt++;
  //inputchar = Serial.read();  // Get the input from the PC
  //String InputString = Serial.readStringUntil('~'); //main loop stop until arrival of terminal key
  InputString = Serial.readString();
  InputString.toCharArray(CharArray, 17);   //14,100 this number needs to be larger than ch number by 2 somehow
  
  //Switching of output modes
  for(int i = 0; i <= 13; i++){
    if(Mode[i]!=1){
      if(CharArray[i] == 124 || CharArray[i] == 125){  //If current mode is servo control and receives digital output command
        Mode[i]=1;  // Switch to digital out mode
        pinMode(i, OUTPUT);
      }
    }
    if(i==3 || i==5 || i==6 || i==9 || i==10 || i==11){
      if(Mode[i]!=2 && CharArray[i] <= 122 && CharArray[i] > 0){  //If current mode is digital output and receives servo command
        Mode[i]=2;  // Switch to PWM mode
      }
    }
    if(Mode[i]!=3){
      if(CharArray[i] == 123){  //If current mode is servo control and receives digital output command
        Mode[i]=3;  // Switch to digital input mode
        pinMode(i, INPUT_PULLUP);
      }
    }
  }

  //Update of outputs
  for(int i = 0; i <= 13; i++){
    if(Mode[i]==1){ //If current mode is digital output
      if(CharArray[i] == 124){
        digitalWrite(i, HIGH); //Digital output ON
      }
      if(CharArray[i] == 125){
        digitalWrite(i, LOW); //Digital output OFF
      }
    }
    if(Mode[i]==2){ //If current mode is servo control
      if(i==3){
        servoCh3.attach(3);
      }
      if(i==5){
        servoCh5.attach(5);
      }
      if(i==6){
        servoCh6.attach(6);
      }
      if(i==9){
        servoCh9.attach(9);
      }
      if(i==10){
        servoCh10.attach(10);
      }
      if(i==11){
        servoCh11.attach(11);
      }
      DesignatedAngle[i] = CharArray[i];
      
      if(CurrAngle[i] != DesignatedAngle[i]){
        Counter[i]++;
      }
      if(CurrAngle[i] == DesignatedAngle[i]){
        Counter[i]=0;
        if(i==3){
          servoCh3.detach();
        }
        if(i==5){
          servoCh5.detach();
        }
        if(i==6){
          servoCh6.detach();
        }
        if(i==9){
          servoCh9.detach();
        }
        if(i==10){
          servoCh10.detach();
        }
        if(i==11){
          servoCh11.detach();
        }
      }
      
      if(Counter[i]%4==0){  //update of servo position
        if(CurrAngle[i] > DesignatedAngle[i]){
          CurrAngle[i]-=1;
        }
        if(CurrAngle[i] < DesignatedAngle[i]){
          CurrAngle[i]+=1;
        }
        //servoCh3.write(CurrAngle[i]);  //Change the angle of servo arm to the designated degree
        if(i==3){
          servoCh3.write(CurrAngle[i]);
        }
        if(i==5){
          servoCh5.write(CurrAngle[i]);
        }
        if(i==6){
          servoCh6.write(CurrAngle[i]);
        }
        if(i==9){
          servoCh9.write(CurrAngle[i]);
        }
        if(i==10){
          servoCh10.write(CurrAngle[i]);
        }
        if(i==11){
          servoCh11.write(CurrAngle[i]);
        }
      }
    }
  }
  
  //Transfer data to python
  if(CharArray[14] == 1){ //Arduino to python transfer is activated
    OutText="";
    for(int i = 0; i <= 5; i++){  // Read analog input
      if(i==0){
        AnalogInputValue[i] = analogRead(A0);
      }
      if(i==1){
        AnalogInputValue[i] = analogRead(A1);
      }
      if(i==2){
        AnalogInputValue[i] = analogRead(A2);
      }
      if(i==3){
        AnalogInputValue[i] = analogRead(A3);
      }
      if(i==4){
        AnalogInputValue[i] = analogRead(A4);
      }
      if(i==5){
        AnalogInputValue[i] = analogRead(A5);
      }
      
      OutText+=(i+1)*-1;
      OutText+=",";
      OutText+=AnalogInputValue[i];
      OutText+=",";
    }
    for(int i = 0; i < 14; i++){  // Read digital input
      if(Mode[i]==3){
        DigitalInputValue[i] = digitalRead(i);  //Somehow digitalinput doesn't work in channel 0 and 1...
        if(DigitalInputValue[i]==0){
          DigitalInOut=3;
        }
        if(DigitalInputValue[i]==1){
          DigitalInOut=2;
        }
      }
      if(Mode[i]!=3){
        DigitalInOut = 1;
      }

      OutText+=(i+6+1)*-1;
      OutText+=",";
      OutText+=DigitalInOut;
      OutText+=",";
    }
      
    Serial.print(OutText);    //It seems arduino wait at here until this output data is read by python
  }
  
  
  //===========================Spare command=================================================================
  //Serial.print(CurrAngle);    //It seems arduino wait at here until this output data is read by python
  //Serial.println(LoopCnt);    //It seems arduino wait at here until this output data is read by python
  //delay(1);
}
