# 4/29-30 整理後+倒車

#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 = 0.9;
double kI = 0.0;
double kD = 1.0;
int vB = 75;
int counter = 0;

String string = "113r62343r2315\0";
char val;
char Forward = '1';
char GoBack = '2';
char Right = '3';
char Left = '4';
char Halt = '5';
char Backward = '6';
char R = 'r';

unsigned long t = 0;
static int i = 0;

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:**"));
    byte *id = mfrc522->uid.uidByte;   // 取得卡片的UID
    byte idSize = mfrc522->uid.size;

    for (byte i = 0; i < idSize; i++) {  // 逐一顯示UID碼
      Serial.print(F("id["));
      Serial.print(i);
      Serial.print(F("]: "));
      Serial.println(id[i], HEX);       // 以16進位顯示UID值
      BT.write(id[i]);                  // 把ID傳給python
    }
    Serial.println();

    mfrc522->PICC_HaltA();
    mfrc522->PCD_StopCrypto1();
    return true;
  }
  return false;
}

double Tracking() {
  int m = digitalRead(A2);
  int l2 = digitalRead(A1);
  int l3 = digitalRead(A0);
  int r2 = digitalRead(A3);
  int r3 = digitalRead(A4);

  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 (BT.available()) {
    val = BT.read();
    string += val;
  }*/
  
  if (string[i] == Backward){
     MotorWriting(wheel_L, wheel_R, -96, -80);
     delay(600);
    if(sum >= 4){
     ++i;}
  }
  else if(sum == 0)
  {
    MotorWriting(wheel_L,wheel_R,-75,-75);
    vL=-75;
    vR=-75;
  }
  else if(sum >= 4){
    if(string[i] == Forward &&(t == 0 || millis()-t >500)){
        t =millis();


        
        manual();
    }
    else if(millis()-t <= 800)
      goto tag;
    else
      manual(); 
  }
  else {
    tag:
    error *= 100 / (sum * 2);
    dError = (error - last);
    //sumError += error;

    vL = vB + kP * error + kD * dError + kI * sumError;
    vR = vB - kP * error - kD * dError - kI * sumError;

    MotorWriting(wheel_L, wheel_R, vL, vR);
  }


  

  last = error;

  return error;
}

void manual()
{
  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(300);
  }

  if (string[i] == Right) {
    MotorWriting(wheel_L, wheel_R, 0, 0);
    delay(500);
    MotorWriting(wheel_L, wheel_R, 200, 0);
    delay(380);
  }
  else if (string[i] == Left) {
    MotorWriting(wheel_L, wheel_R, 0, 0);
    delay(500);
    MotorWriting(wheel_L, wheel_R, 0, 200);
    delay(420);
  }
  else if (string[i] == GoBack) {
   /* while (!RFID()) {                         ///在偵測到RFID前都要前進
      MotorWriting(wheel_L, wheel_R, 0, 0);
      delay(100);
      MotorWriting(wheel_L, wheel_R, -70, -70);
      delay(100);
    }*/

    MotorWriting(wheel_L, wheel_R, -180, 120); ///迴轉
    delay(850);
    MotorWriting(wheel_L, wheel_R, 120, -180); ///煞車
    delay(50);

    MotorWriting(wheel_L, wheel_R, 0, 0);
    delay(500);
    MotorWriting(wheel_L, wheel_R, 70, 70);   ///往前一點才不會在死巷再次感應到
    delay(500);
  }
   else if (string[i] == R) {
    MotorWriting(wheel_L, wheel_R, 0, 0);
      delay(100);
      MotorWriting(wheel_L, wheel_R, -96, -80);
      
    while (!RFID()) {                         ///在偵測到RFID前都要前進
      
    }
  }
  else if (string[i] == Halt) {
    MotorWriting(wheel_L, wheel_R, 0, 0);
    delay(800);
  }

  BT.write(i);
  i++;
}

void setup() {
  Serial.begin(9600);
  BT.begin(9600);
  Serial.println(F("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;
bool start = false;

void loop() {
  /*if (start == true) {*/
    double current = Tracking();
    last = current;
  /*}
  else {
    if (BT.read() == 's') {
      start = true;
    }
  }*/
}