#define RFRONTPIN (5)
#define RBACKPIN  (6)
#define LFRONTPIN (9)
#define LBACKPIN  (10)
int rfmotor;
int rbmotor;
int lfmotor;
int lbmotor;
int check;
int check_a;
int check_b;
int time_check;
//int motor_first = 128;//�����l

void setup() {
    Serial.begin(9600);
    pinMode(RFRONTPIN, OUTPUT);
    pinMode(RBACKPIN, OUTPUT);
    pinMode(LFRONTPIN, OUTPUT);
    pinMode(LBACKPIN, OUTPUT);
}
void strait(int motor,int control_data){
  int imotor = curve(motor,control_data);
  if(-30<control_data&&control_data<30){
    rfmotor=imotor;
    rbmotor=0;
    lfmotor=imotor;
    lbmotor=0;
  }else if(control_data<=-30){
    rfmotor=0;
    rbmotor=0;
    lfmotor=imotor;
    lbmotor=0;
  }else if(30<=control_data){
    rfmotor=imotor;
    rbmotor=0;
    lfmotor=0;
    lbmotor=0;
  }
  drive(rfmotor,rbmotor,lfmotor,lbmotor);
}
void back(int motor,int control_data){
  int imotor = curve(motor,control_data);
  if(-30<control_data&&control_data<30){
    rfmotor=0;
    rbmotor=imotor;
    lfmotor=0;
    lbmotor=imotor;
  }else if(control_data<=-30){
    rfmotor=0;
    rbmotor=0;
    lfmotor=0;
    lbmotor=imotor;
  }else if(30<=control_data){
    rfmotor=0;
    rbmotor=imotor;
    lfmotor=0;
    lbmotor=0;
  }
  drive(rfmotor,rbmotor,lfmotor,lbmotor);
}
void circle(int motor,int control_data){
  int imotor = curve(motor,control_data);
  if(-30<control_data&&control_data<30){
    rfmotor=0;
    rbmotor=0;
    lfmotor=0;
    lbmotor=0;
  }else if(control_data<=-30){
    check=-3;
    rfmotor=0;
    rbmotor=imotor;
    lfmotor=imotor;
    lbmotor=0;
  }else if(30<=control_data){
    check=3;
    rfmotor=imotor;
    rbmotor=0;
    lfmotor=0;
    lbmotor=imotor;
  }
  drive(rfmotor,rbmotor,lfmotor,lbmotor);
}  

void stop(){
  rfmotor=0;
  rbmotor=0;
  lfmotor=0;
  lbmotor=0;
  drive(rfmotor,rbmotor,lfmotor,lbmotor);
}
int curve(int motor, int control_data){
  if(-30<control_data&&control_data<30){
    return motor;
  }else{
    int amotor = control_data * 2;
    return amotor;
  }
  /*
  if(-30<control_data&&control_data<30){
    return motor;  
  }else if(-20<control_data&&control_data<=-10){
    //return motor*0.10;
    return 36;
  }else if(-30<control_data&&control_data<=-20){
    //return motor*0.10;
    return 36;
  }else if(-40<control_data&&control_data<=-30){
    //return motor*0.25;
    return 74;
  }else if(-50<control_data&&control_data<=-40){
    //return motor*0.40;
    return 109;
  }else if(-60<control_data&&control_data<=-50){
    //return motor*0.55;
    return 146;
  }else if(-70<control_data&&control_data<=-60){
    //return motor*0.70;
    return 182;
  }else if(-80<control_data&&control_data<=-70){
    //return motor*0.85;
    return 218;
  }else if(control_data<=-80){
    //return motor;
    return 255;
  }else if(10<=control_data&&control_data<20){
    //return motor*0.10;
    return 36;
  }else if(20<=control_data&&control_data<30){
    //return motor*0.10;
    return 36;
  }else if(30<=control_data&&control_data<40){
    //return motor*0.25;
    return 74;
  }else if(40<=control_data&&control_data<50){
    //return motor*0.40;
    return 109;
  }else if(50<=control_data&&control_data<60){
    //return motor*0.55;
    return 146;
  }else if(60<=control_data&&control_data<70){
    //return motor*0.70;
    return 182;
  }else if(70<=control_data&&control_data<80){
    //return motor*0.85;
    return 218;
  }else if(80<=control_data){
    //return motor;
    return 255;
  }else{
    //return motor*0;
    return 0;
  }
  */
}
void A_circle(int motor,int control_data){
  int time=abs(control_data*1);
  //time=time_check;
  if(-1<control_data&&control_data<1){
    rfmotor=0;
    rbmotor=0;
    lfmotor=0;
    lbmotor=0;
  }else if(control_data<=-1){
    rfmotor=0;
    rbmotor=255;
    lfmotor=255;
    lbmotor=0;
  }else if(1<=control_data){
    rfmotor=255;
    rbmotor=0;
    lfmotor=0;
    lbmotor=255;
  }
  if(check_b==0){
    for(int i = 0 ; i <= time ; i++){
      analogWrite(RFRONTPIN,rfmotor);
      analogWrite(RBACKPIN,rbmotor);
      analogWrite(LFRONTPIN,lfmotor);
      analogWrite(LBACKPIN,lbmotor);
      delay(10);
    }
  }
  check_b=1;
  analogWrite(RFRONTPIN,255);
  analogWrite(RBACKPIN,255);
  analogWrite(LFRONTPIN,255);
  analogWrite(LBACKPIN,255);
}

void drive(int rfmotor,int rbmotor,int lfmotor,int lbmotor){
  switch(check){
    case 1:
      if(check_a=1){
      }else{
        analogWrite(RFRONTPIN,255);
        analogWrite(RBACKPIN,0);
        analogWrite(LFRONTPIN,255);
        analogWrite(LBACKPIN,0);
        delay(100);
        check_a=1;
      }
      break;
    case 2:
      if(check_a=2){
      }else{
        analogWrite(RFRONTPIN,0);
        analogWrite(RBACKPIN,255);
        analogWrite(LFRONTPIN,0);
        analogWrite(LBACKPIN,255);
        delay(100);
        check_a=2;
      }
      break;
    case 3:
      if(check_a=3){
      }else{
        analogWrite(RFRONTPIN,255);
        analogWrite(RBACKPIN,0);
        analogWrite(LFRONTPIN,0);
        analogWrite(LBACKPIN,255);
        delay(100);
        check_a=3;
      }
      break;
    case -3:
      if(check_a=-3){
      }else{
        analogWrite(RFRONTPIN,0);
        analogWrite(RBACKPIN,255);
        analogWrite(LFRONTPIN,255);
        analogWrite(LBACKPIN,0);
        delay(100);
        check_a=-3;
      }
      break;
    default:
      break;
  }
  analogWrite(RFRONTPIN,rfmotor);
  analogWrite(RBACKPIN,rbmotor);
  analogWrite(LFRONTPIN,lfmotor);
  analogWrite(LBACKPIN,lbmotor);
}
void loop(){
  char data[6];
  int data_int[6];
  int control_data;
  int motor=100;
  int timer=0;
  int cmd=0;
  check_a=0;
  check_b=0;
  time_check=0;
  while(1){
  for(int i=0; i<6; i++){
    data[i]=Serial.read();
    data_int[i]=data[i]-'0';
  }
  delay(64);
  if(data_int[3]==1){
    control_data=-1*10*data_int[4]+data_int[5];
  }else{
    control_data=10*data_int[4]+data_int[5];
  }
  //Serial.println(control_data);
  //Serial.println(check_b);
  Serial.println(timer+motor*100000);
  switch(data_int[1]){
    case 0:
      timer+=1;
      if(timer==2700){
        motor=80;
      }else if(timer==3100){
        motor=70;
      }else if(timer==3200){
        motor=60;
      }else if(timer==3500){
        motor=50;
      }
      if(cmd==1){
        timer=0;
        motor=100;
        cmd=2;
      }else if(cmd==4){
        cmd=5;
      }
      break;
    case 1:
      //motor*=0.2;
      timer+=1;
      if(timer==2700){
        motor=80;
      }else if(timer==3100){
        motor=70;
      }else if(timer==3200){
        motor=60;
      }else if(timer==3500){
        motor=50;
      }
      break;
    case 2:
      //motor*=0.4;
      //motor=57;
      timer+=1;
      if(timer==2700){
        motor=80;
      }else if(timer==3100){
        motor=70;
      }else if(timer==3200){
        motor=60;
      }else if(timer==3500){
        motor=50;
      }
      break;
    case 3:
      //motor*=0.6;
      timer+=1;
      if(timer==2700){
        motor=80;
      }else if(timer==3100){
        motor=70;
      }else if(timer==3200){
        motor=60;
      }else if(timer==3500){
        motor=50;
      }
      break;
    case 4:
      //motor*=0.8;
      timer+=1;
      if(timer==2700){
        motor=80;
      }else if(timer==3100){
        motor=70;
      }else if(timer==3200){
        motor=60;
      }else if(timer==3500){
        motor=50;
      }
      break;
    case 5:
      //motor*=1.0;
      timer+=1;
      if(timer==2700){
        motor=80;
      }else if(timer==3100){
        motor=70;
      }else if(timer==3200){
        motor=60;
      }else if(timer==3500){
        motor=50;
      }
      break;
    case 6:
      //motor*=1.2;
      timer+=1;
      if(timer==2700){
        motor=80;
      }else if(timer==3100){
        motor=70;
      }else if(timer==3200){
        motor=60;
      }else if(timer==3500){
        motor=50;
      }
      break;
    case 7:
      //motor*=1.4;
      timer+=1;
      if(timer==2700){
        motor=80;
      }else if(timer==3100){
        motor=70;
      }else if(timer==3200){
        motor=60;
      }else if(timer==3500){
        motor=50;
      }
      break;
    case 8:
      //motor*=1.6;
      timer+=1;
      if(timer==2700){
        motor=80;
      }else if(timer==3100){
        motor=70;
      }else if(timer==3200){
        motor=60;
      }else if(timer==3500){
        motor=50;
      }
      break;
    case 9:
      //motor*=2.0;
      timer+=1;
      if(timer==2700){
        motor=80;
      }else if(timer==3100){
        motor=70;
      }else if(timer==3200){
        motor=60;
      }else if(timer==3500){
        motor=50;
      }
      ;
      if(cmd==0){
        cmd=1;
      }else if(cmd==2){
        cmd=4;
      }else if(cmd==5){
        timer=0;
        motor=255;
        cmd=0;
      }
      break;
    default:
      break;
  }
  switch(data_int[0]){
    case 1:
      strait(motor,control_data);
      check=1;
      break;
    case 2:
      back(motor,control_data);
      check=2;
      break;
    case 3:
      circle(motor,control_data);
      //check=3;
      break;
    case 4:
      check=4;
      check_b=0;
      stop();
      break;
    case 5:
      A_circle(motor,control_data);
      break;
    default:
      break;
  }
  //Serial.println(motor);
  }
}