# 參考 4/30 更改後Checkpoint特化型版本

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


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

//-----------------------------------------mfrc522------------------------------------------
#define RST_PIN    9 
#define SS_PIN    10

//----------------------------------------bluetooth-----------------------------------------
#define rx 2  // 接收腳
#define tx 8  // 傳送腳
#define communicateRate 9600

//---------------------------------------PID control-----------------------------------------
#define kP 0.4
#define kI 0
#define kD 1.15
#define vB 80
#define vT 110


//-----------------------------------------globals------------------------------------------
// 讀卡的
MFRC522 *mfrc522;
SoftwareSerial BT(rx, tx);

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

// 循跡
String string = "111613432115";
char Forward  = '1';
char GoBack   = '2';
char Right    = '3';
char Left     = '4';
char Halt     = '5';
char Bonus    = '6';

unsigned long t =0;
int i=0;






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






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;
}






void Tracking()
{
    int l3 = digitalRead(A0);
    int l2 = digitalRead(A1);
    int  m = digitalRead(A2);
    int r2 = digitalRead(A3);
    int r3 = digitalRead(A4);
    
    int sum =l3 + l2 + m + r2 + r3;
      
    if(sum == 0)
        MotorWriting(wheel_L,wheel_R,-90,-90);
    else if(sum >= 4 && (t == 0 || millis()-t > 1100))
    {
      t = millis();
        manual();
    }
    else
    {
         /*if(sum >= 4 && string[i] == Forward && (t == 0 || millis()-t > 1100))
         {
             t = millis();
             
             BT.write(i);
             
             ++i;
         }*/
    
         static double error =0;
         static double sumError =0;
         double dError =-error;
    
         error =l3*-2 + l2*-1 + r2*1 + r3*2;
         error *=100/(sum*2);
         dError +=error;
         sumError +=error;
                 
         int vL =vB + kP*error + kD*dError + kI*sumError;
         int vR =2*vB-vL;
         
         MotorWriting(wheel_L,wheel_R,vL,vR);
    }
}






void manual()
{
    if(string[i] == Forward){
      delay(75);
      MotorWriting(wheel_L, wheel_R, 80, 80);
      delay(200);

    int l3 = digitalRead(A0);
    int l2 = digitalRead(A1);
    int  m = digitalRead(A2);
    int r2 = digitalRead(A3);
    int r3 = digitalRead(A4);
    
    int sum =l3 + l2 + m + r2 + r3;

    for(int i =0, d =50; !sum; ++i)
    {
        int time =d*i;

        while(time > 0 && !sum)
        {
            l3 = digitalRead(A0);
            l2 = digitalRead(A1);
             m = digitalRead(A2);
            r2 = digitalRead(A3);
            r3 = digitalRead(A4);

            sum =l3 + l2 + m + r2 + r3;

            i%2 ? MotorWriting(wheel_L, wheel_R, -90, 80) : MotorWriting(wheel_L, wheel_R, 80, -85);
            delay(10);

            time -=10;
        }
    }
    }
    else if(string[i] == Right)
    {
        delay(50);
        MotorWriting(wheel_L, wheel_R, 0, 0);
        delay(500);
        MotorWriting(wheel_L, wheel_R, vT, -0.8*vT);
        delay(350);

        while(!digitalRead(A2));
    }
    else if(string[i] == Left)
    {
        delay(50);
        MotorWriting(wheel_L, wheel_R, 0, 0);
        delay(500);
        MotorWriting(wheel_L, wheel_R, -0.8*vT, vT);
        delay(350);
        
        while(!digitalRead(A2));
    }
    else if(string[i] == GoBack)
    {
        MotorWriting(wheel_L,wheel_R,0,0);           //在偵測到RFID前都要前進
        MotorWriting(wheel_L,wheel_R,-80,-80);
        
        while(!RFID()){}
        
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,-180,120);
        delay(500);
        
        while(!digitalRead(A2));
    }
    else if(string[i] == Halt)
    {
        MotorWriting(wheel_L,wheel_R,0,0);
        
        while(1);
    }
    else if(string[i] == Bonus)
    {
        /*MotorWriting(wheel_L, wheel_R, 0, 0);
        delay(500);
        MotorWriting(wheel_L, wheel_R, -0.8*vT, vT);
        delay(400);
        
        while(!digitalRead(A0));*/

        MotorWriting(wheel_L, wheel_R, -90, -90);
        delay(450);
        
        MotorWriting(wheel_L, wheel_R, 0, 80);
        delay(150);
        MotorWriting(wheel_L, wheel_R, 0, 150);
        delay(100);
        MotorWriting(wheel_L, wheel_R, 0, 200);
        delay(450);

        MotorWriting(wheel_L, wheel_R, -95, -80);

        while(!RFID());
    }

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






void setup()
{
    //藍芽
    BT.begin(communicateRate);
    Serial.begin(communicateRate);
    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:"));
    
    
    
}

void loop()
{
  static bool start =false;
    
    if(start == true)
        Tracking();
    else if (BT.available() && BT.read() == 's')
        start = true;
}