# 4/26

#include <SoftwareSerial.h>
#include <SPI.h>
#include <MFRC522.h>
#include <time.h>
#include "wheel.h"

#define L_ENA 9
#define L_ENB 3  
#define L_IN1 7
#define L_IN2 6
#define L_IN3 5
#define L_IN4 4

SoftwareSerial BT(2, 8); // 接收腳, 傳送腳

#define RST_PIN    9 
#define SS_PIN    10
MFRC522 *mfrc522;

double kP = 1.0;
double kI = 0;
double kD = 0.8;
int vB = 70;
int counter = 0;

unsigned long lastTime =millis()-2000;

String string = "11143432115\0";
char val;
char Forward = '1';
char Right = '3';
char Left = '4';
char GoBack = '2';
char Halt = '5';

void MotorWriting(wheel wheel_L, wheel wheel_R, int vL, int vR){
  wheel_L.power(vL);
  wheel_R.power(vR);
}

wheel wheel_L(L_ENA, L_IN1, L_IN2);
wheel wheel_R(L_ENB, L_IN3, L_IN4);

bool RFID(){
  if(mfrc522->PICC_IsNewCardPresent() && mfrc522->PICC_ReadCardSerial()){
    Serial.println(F("**Card Detected:**"));
    mfrc522->PICC_DumpDetailsToSerial(&(mfrc522->uid));
    mfrc522->PICC_HaltA();
    mfrc522->PCD_StopCrypto1();

    //BT.write()
    return true;
    }
  return false;
}

/*void translate(double target)
{
int output =target < 0 ? -1*target : target;
int output2 =target < 0 ? int(-100*target)%100 : int(100*target)%100;

Serial.println(target);

if(target < 0)
BT.write('-');

int temp[10] ={};

int i =0;
for(; output > 0; ++i)
{
  temp[i] +=output%10;
  output /=10;
}

for(;i >= 0; --i)
  BT.write(temp[i]+48);
  
  BT.write('.');
  BT.write(output2/10+48); 
  BT.write(output2%10+48);
  BT.write("\n");
}*/

double Tracking(){
  
  
  int m= digitalRead(A2);
  int l2= digitalRead(A1);
  int l3= digitalRead(A0);
  int r2= digitalRead(A3);
  int r3= digitalRead(A4);
  //Serial.println(lastTime);
  //Serial.println(millis());
  static double last =0;
  static double sumError=0;
  double dError=0;
  double error=l3*-2 + l2*-1 + r2*1 + r3*2;
  int sum=l3 + l2 + m + r2 + r3;
  int vL=0,vR=0;
  
  if(sum >= 4){
    dError=0;
    sumError=0;
    
    if((vL>=0 || vR>=0) && millis()-lastTime > 900)
      manual(); 

     lastTime =millis();
    }
  else if(sum!=0){
    error *= 100/(sum*2);
    dError=(error-last);
    sumError += error;
    
    /*if(error<15 || error>-15){
      vL = vB + 1*error ;//+ 3*dError;
      vR = vB - 1*error ;//- 3*dError;
    }
    else{
    vL = vB + kP*error + kD*dError + kI*sumError;
    vR = vB - kP*error - kD*dError - kI*sumError;
    }*/
    vL = vB + kP*error + kD*dError + kI*sumError;
    vR = vB - kP*error - kD*dError - kI*sumError;

    if (BT.available()) {
      val = BT.read();
      string += val;
    }
   
   
    

    
    if(error >= 150){
    sumError = 0;
    counter = 0;
    }
    else{
      counter += 1;
    }
    
    MotorWriting(wheel_L,wheel_R,vL,vR);
  }
  else
  {
    //MotorWriting(wheel_L,wheel_R,-75,-75);
      static int M=1;
      static unsigned long now =millis();
      
      if(M%2==0)
      {
        MotorWriting(wheel_L,wheel_R,-70,70);
        delay(100*M);
      }
      else{
      MotorWriting(wheel_L,wheel_R,70,-70);
      delay(100*M);
      }
    M++;
    sumError = 0;
    
  }

  /*translate(error);
  translate(dError);
  translate(0.2*sumError);
  
  BT.write("\n\n\n");*/

    
  last =error;
  
  return error;
  
}

void manual()
{
 static int i=0;
 
    if(string[i] == '\0'){
      while(1)
      {MotorWriting(wheel_L,wheel_R,0,0);
      delay(10000);}
      
    }
    
      if(string[i] == Forward){
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,80,80);
        delay(700);
        
      }
      if(string[i] == Right){
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,80,80);
        delay(400);
        MotorWriting(wheel_L,wheel_R,80,-60);
        delay(900);
        
      }
      if(string[i] == Left){
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,80,80);
        delay(400);
        MotorWriting(wheel_L,wheel_R,-60,80);
        delay(900);
        
      }
      if(string[i] == GoBack){
        while(!RFID()){
          MotorWriting(wheel_L,wheel_R,0,0);
          
          MotorWriting(wheel_L,wheel_R,70,70);
          
        }
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,-130,100);
        delay(1080);
        
      }
      if(string[i] == Halt){
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(800);
        Serial.println(6);
      }
      
      lastTime =millis();
      i++;
}

void setup() {
   Serial.begin(9600);
   BT.begin(9600);  
   Serial.println("BT is ready!");
    
   SPI.begin();
   mfrc522 = new MFRC522(SS_PIN, RST_PIN);
   mfrc522->PCD_Init();
   Serial.println(F("Read UID on a MIFARE PICC:"));
}

double last;

void loop() {
  
  double current =Tracking();
  
  last =current;

  

}