#include<SPI.h>
#include "Arduino.h"
#include "HardwareSerial.h"
#include<SoftwareSerial.h>
#include<Servo.h>
#define  trigPin   2          //超声波模块的Trig口 6#
#define  echoPin   5          //超声波模块的echo口 5#
#define  ServoPin  3          //底座舵机端口 3#
#define  propellerFix 75
unsigned long time=0;
bool increasing=true;
bool clr=false;
//0: increase 1: decrease 2: clr
int status=0;
int clrtime=40;
int x=0;
int distRec[90];
int leftAlert=0;
int frontAlert=0;
int rightAlert=0;
int rudderModify=0;
const int autoTurn1=200;
const int autoTurn2=400;
const int alert=1000;
const int interval=100;
String sending="",received="";
Servo MyServo;
Servo RadarServo;
SoftwareSerial bluetoothSerial(7,8);
int count =0; 
int IN1 =9;
int IN2 =10;
int PWM=6;
int angle=90;
int period=25;
// float dist;
long dj,z;//dj是舵机，z是螺旋桨转速
//long propeller=0,rudder=0;//propeller为螺旋桨（0-1023，512为静止），rudder为舵（0-1023，512为静止）
//long s_propeller=0,s_rudder=0;//发送的信号
long propeller,rudder;//校验序列、读取内容和解码内容 
long response=0;
const byte addresses[6] = "00001";  //创建一个数组，建立接收机地址，或者说两个模块将用于通信的“管道”
int calculateDistance()
{ 
  long duration;
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); 
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  return duration*0.034/2;
}

void setup()
{
  Serial.begin(9600);
  bluetoothSerial.begin(57600);
  bluetoothSerial.listen();
  MyServo.attach(4);
  RadarServo.attach(ServoPin);
  pinMode(trigPin,OUTPUT);
  pinMode(echoPin,INPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(PWM,OUTPUT);
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  MyServo.write(45);
  RadarServo.write(0);
  delay(1000);  
}
long confirm(long x){
  if(x>=1000000&&x<=2000000)return x%1000000;
  else return -1;
}
void loop() {
  int distance=0;
 // Serial.print("s: ");
 // Serial.println(status); 
  if(status==0){
    if(x<180){
      x+=2;
    }
    else{
      status=2;
    }
    RadarServo.write(x);             //调整舵机角度
    //测距
    distance = calculateDistance();
    if(x!=180&&x%2==0)
      distRec[x/2]=distance;
    //send distance
    //response=1000000+x*1000+distance;
    response=0;
    response+=distance;
    //Serial.println(response);
    response+=(long)x*1000;
    //Serial.println(response);
    response+=1000000;
    //Serial.println(response);
  }
  else if(status==1){
    if(x>0){
      x-=2;
    }
    else{
      status=2;
    }
    RadarServo.write(x);             //调整舵机角度
    //测距
    distance = calculateDistance();
    if(x!=180&&x%2==0)
      distRec[x/2]=distance;
    //response=1000000+x*1000+distance;
    response=0;
    response+=distance;
    //Serial.println(response);
    response+=(long)x*1000;
    //Serial.println(response);
    response+=1000000;
    //Serial.println(response);
    //send distance
  }
  else if(status=2){
    clrtime--;
    if(clrtime<=0){
      clrtime=4;
      if(x>=180){
        status=1;
      }
      else{
        status=0;
      }
    }
    //send clear
    response=2000000;
  }
  sending=String(response);

  // Serial.print("x: ");
  // Serial.println(x);
  // Serial.print("d: ");
  // Serial.println(distance);
  // Serial.println(response);  
  Serial.println(sending);
  bluetoothSerial.println(sending);
  delay(interval);
  //delay(20-distance/17);
  Serial.println(distance);
  while(bluetoothSerial.available()>0){
    //Serial.println("available!");
    if(bluetoothSerial.peek()!='\n'){//peek()查看当前字节是不是换行
      received+=(char)bluetoothSerial.read();
      //Serial.println(received);
      //Serial.println("no \n");
    }
    else{
      bluetoothSerial.read();//把换行符读掉
      Serial.println(received);
      //Serial.println(received.length());
      //content=toInt(received);
      if(received[0]=='1'&&received.length()-1==7){
        propeller=received.substring(4).toInt();
        rudder=received.substring(1,4).toInt();
        if(propeller>600||propeller<400){
          if(propeller>600){ 
            long zheng=0.568*propeller-312.4;
            digitalWrite(IN2,HIGH);
            digitalWrite(IN1,LOW);
            analogWrite(PWM,zheng);
            z=zheng;
            Serial.println("forward propelling");
            Serial.println(z);
          }
          else if(propeller<400){
            long fan=255-0.568*propeller;
            digitalWrite(IN1,HIGH);
            digitalWrite(IN2,LOW);
            analogWrite(PWM,fan);
            z=fan;
            Serial.println("back propelling");
            Serial.println(z);
          }
        }
        else if(propeller<=600||propeller>=400){
          z=0;
          analogWrite(PWM,0);
          Serial.println("stop propelling");
          Serial.println(z);
        } 
        //Serial.println("inputing ruddering");
        if(rudder>600||rudder<400)
        {
          if(rudder<400){
            long fan1=0.1*rudder;
            MyServo.write(fan1);
            //Serial.println("turn left");
            dj=2*fan1-90;
            //Serial.println(dj);
          }
          else if(rudder>600){
            long zheng1=0.1*rudder-10.122;
            MyServo.write(zheng1);
            dj=2*zheng1-90;
            //Serial.println("turn right");
            //Serial.println(dj);
          }
        } 
        else if(rudder<=600||rudder>=400){
          dj=0;
          MyServo.write(45);
          //Serial.println("stop turing");
          //Serial.println(dj);                     
        }  
      }
      else if(received[0]=='2'&&received.length()-1==7){
        propeller=received.substring(4).toInt();
        rudder=received.substring(1,4).toInt();
        for(int i=0;i<30;i++){
          leftAlert+=distRec[i];
          frontAlert+=distRec[i+30];
          rightAlert+=distRec[i+60];
        }
        if(propeller>600||propeller<400){
          if(propeller>600){ 
            digitalWrite(IN2,HIGH);
            digitalWrite(IN1,LOW);
            analogWrite(PWM,propellerFix);
            Serial.println("forward propelling");
          }
          else if(propeller<400){
            digitalWrite(IN1,HIGH);
            digitalWrite(IN2,LOW);
            analogWrite(PWM,propellerFix);
            Serial.println("back propelling");
          }
        }
        else if(propeller<=600||propeller>=400){
          z=0;
          analogWrite(PWM,0);
          Serial.println("stop propelling");
          Serial.println(z);
        } 
        //Serial.println("inputing ruddering");
        if(leftAlert<alert&&frontAlert<alert&&rightAlert<alert){
          analogWrite(PWM,0);

        }
        else if(leftAlert<alert&&frontAlert<alert&&rightAlert>=alert){
          rudder-=autoTurn2;
          if(rudder<0){
            rudder=0;
          }
        }
        else if(leftAlert<alert&&frontAlert>=alert&&rightAlert<alert){
          
        }
        else if(leftAlert<alert&&frontAlert>=alert&&rightAlert>=alert){
          rudder-=autoTurn1;
          if(rudder<0){
            rudder=0;
          }
        }
        else if(leftAlert>=alert&&frontAlert<alert&&rightAlert<alert){
          rudder+=autoTurn2;
          if(rudder>999){
            rudder=999;
          }
        }
        else if(leftAlert>=alert&&frontAlert<alert&&rightAlert>=alert){
          rudder-=autoTurn2;
          if(rudder<0){
            rudder=0;
          }
        }
        else if(leftAlert>=alert&&frontAlert>=alert&&rightAlert<alert){
          rudder+=autoTurn1;
          if(rudder>999){
            rudder=999;
          }
        }
        else if(leftAlert>=alert&&frontAlert>=alert&&rightAlert>=alert){
          
        }        
        
        if(rudder>600||rudder<400)
        {
          if(rudder<400){
            long fan1=0.1*rudder;
            MyServo.write(fan1);
            //Serial.println("turn left");
            dj=2*fan1-90;
            //Serial.println(dj);
          }
          else if(rudder>600){
            long zheng1=0.1*rudder-10.122;
            MyServo.write(zheng1);
            dj=2*zheng1-90;
            //Serial.println("turn right");
            //Serial.println(dj);
          }
        } 
        else if(rudder<=600||rudder>=400){
          dj=0;
          MyServo.write(45);
          //Serial.println("stop turing");
          //Serial.println(dj);                     
        }  
      }
      received="";                                                             
    }
  }
}