import lejos.nxt.*;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.MotorPort;
import lejos.nxt.NXTMotor;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.NXTLineLeader;
import lejos.util.Delay;
import lejos.nxt.addon.ColorHTSensor;
import lejos.nxt.addon.*;
import java.io.*;
import lejos.nxt.comm.*;
import lejos.util.Delay;
import lejos.nxt.comm.*;
class LineLeader{
    static NXTLineLeader liner = new NXTLineLeader(SensorPort.S1);//全域宣告
    static UltrasonicSensor us = new UltrasonicSensor(SensorPort.S2); //宣告聲波物件
    static RFIDSensor rfs = new RFIDSensor(SensorPort.S3); //宣告RFIDSensor物件
    static UltrasonicSensor us2 = new UltrasonicSensor(SensorPort.S4); //宣告聲波物件
    static NXTMotor MotorA = new NXTMotor(MotorPort.A);
    static NXTMotor MotorB = new NXTMotor(MotorPort.B);
    static NXTMotor MotorC = new NXTMotor(MotorPort.C);
    static int b,afactories1,afactories2,afactories3;
    static int re;

    public static void main(String arg[]){
            Button.ESCAPE.addButtonListener(new ButtonListener(){
                public void buttonPressed(Button b){System.exit(1);}
                public void buttonReleased(Button b) {}
           });
        LineLeader ll = new LineLeader();
    ll.Calibration();
    LCD.drawString("Waiting...", 0, 0);
        NXTConnection connection = Bluetooth.waitForConnection(); //等待連線
    DataInputStream dataIn = connection.openDataInputStream();//取得輸入串流
    DataOutputStream dataOut =connection.openDataOutputStream();
    LCD.drawString("OK...", 0, 0);
    try{
        while(true){
            b = dataIn.read();
            afactories1 = dataIn.read();
            afactories2 = dataIn.read();
            afactories3 = dataIn.read();
             if(b==1){
                 ll.followLine(afactories1,afactories2,afactories3,dataIn,re);
                 LCD.drawString("ok", 0, 1);
           }
       }
     }catch(IOException e) {System.exit(1);}
   }
   public void Calibration()  { //校正程式
           liner.wakeUp();
           LCD.drawString("Calibration: ", 0, 0);
           LCD.drawString("Place LineLeader", 0, 1);
           LCD.drawString("over bright area ", 0, 2);
           LCD.drawString("Press Enter", 0, 3);
           Button.ENTER.waitForPressAndRelease();
           LCD.drawString("**** wait ******", 0, 3);
           liner.calibrate(NXTLineLeader.LineColor.WHITE); //校正白色值
           LCD.clear();

           LCD.drawString("Calibration: ", 0, 0);
           LCD.drawString("Place LineLeader", 0, 4);
           LCD.drawString("over black area ", 0, 5);
           LCD.drawString("Press Enter", 0, 6);
           Button.ENTER.waitForPressAndRelease();
           LCD.drawString("**** wait ******", 0, 7);
           liner.calibrate(NXTLineLeader.LineColor.BLACK); //校正黑色值
           LCD.clear();
           LCD.drawString("Calibration done", 0, 4);
           Button.ENTER.waitForPressAndRelease();
           LCD.clear();

       }

   public void followLine(int afactories1,int afactories2,int afactories3,DataInputStream dataIn,int re ){

           int sound,sound2;
           int save=0;
           int total=0;
           int save2=0;
           int save1=0;
           int steering;
           int a=0;//經過rfidA,B,C
           int b=0;
           int c=0;
           int d=0;//做過避障動作
           int e=0;//取球動作
           int llResult;

           long rfid1,rfid2,rfid3,rfid4,rfid5,rfid6,rfid7,rfid8,rfid9,
           rfid10,rfid11,rfid12,rfid13,rfid14,rfid15,rfid16,rfid17,rfid18,rfid19,rfid20;

           String  r1="462887518288";
           String  r2="620140363856";
           String  r3="952060739664";
           String  r4="376166023248";
           String  r5="672418168912";
           String  r6="1048630460496";
           String  r7="511893831760";
           String  r8="577291354192";
           String  r9="748687392848";
           String  r10="471292837968";
           String  r15="534442344528";
           String  r16="200055586896";
           rfid1=Long.parseLong(r1);//工作站A
           rfid2=Long.parseLong(r2);//工作站A
           rfid3=Long.parseLong(r3);//工作站B
           rfid4=Long.parseLong(r4);//工作站B
           rfid5=Long.parseLong(r5);//工作站C
           rfid6=Long.parseLong(r6);//工作站C
           rfid7=Long.parseLong(r7);//取球
           rfid8=Long.parseLong(r8);//取球
           rfid9=Long.parseLong(r9);//彎進去A停車場
           rfid10=Long.parseLong(r10);//彎進去A停車場
           rfid15=Long.parseLong(r15);//A停車場
           rfid16=Long.parseLong(r16);//A停車場
           long id;
           while (!Button.ESCAPE.isDown()){ //如果跳出按鈕沒被按下
                  id = rfs.readTransponderAsLong(true);
                  llResult = liner.getResult();
                  sound = us.getDistance();
                  sound2 = us2.getDistance();
                  LCD.clear();
                  LCD.drawInt(liner.getResult(), 0, 0);
                  LCD.drawString(Integer.toBinaryString(llResult), 0, 1);
                  LCD.drawInt(a, 0, 1);
                  LCD.drawInt(b, 0, 1);
                  if(id==rfid1||id==rfid2){//A工作站
                            a++;
                           }
                  if(id==rfid3||id==rfid4){//B工作站
                            b++;
                         }
                  if(id==rfid5||id==rfid6){//C工作站
                            c++;
                        }
                  if(id==rfid7||id==rfid8){//取球  (可能不需要RFID)
                            e++;
                      }
                  if(sound<=18){//取球前方的柵欄
                          while(sound<30){
                                 sound = us.getDistance();
                                 Motor.A.setSpeed(0);
                                 Motor.B.setSpeed(0);
                                 Motor.A.forward();
                                 Motor.B.forward();
                               }
                   }
                    else if(id==rfid1||id==rfid2){
                          for(int i=0;i<afactories2;i++){
                              Motor.A.setSpeed(0);
                              Motor.B.setSpeed(0);
                              Motor.C.setSpeed(400);
                              Motor.C.forward();
                              Delay.msDelay(900);
                              Motor.A.setSpeed(0);
                              Motor.B.setSpeed(0);
                              Motor.C.setSpeed(0);
                              Delay.msDelay(500);
                         }
                              Motor.A.setSpeed(300);
                              Motor.B.setSpeed(300);
                              Motor.A.forward();
                              Motor.B.forward();
                              Delay.msDelay(500);
               }
                   else if(id==rfid3||id==rfid4){
                       for(int i=0;i<afactories3;i++){
                                   Motor.A.setSpeed(0);
                                   Motor.B.setSpeed(0);
                                   Motor.C.setSpeed(400);
                                   Motor.C.forward();
                                   Delay.msDelay(900);
                                   Motor.A.setSpeed(0);
                                   Motor.B.setSpeed(0);
                                   Motor.C.setSpeed(0);
                                   Delay.msDelay(500);
                       }
                    Motor.A.setSpeed(300);
                    Motor.B.setSpeed(300);
                    Motor.A.forward();
                    Motor.B.forward();
                    Delay.msDelay(500);
               }

               else if(id==rfid5||id==rfid6){
                       for(int i=0;i<afactories1;i++){
                                   Motor.A.setSpeed(0);
                                   Motor.B.setSpeed(0);
                                   Motor.C.setSpeed(400);
                                   Motor.C.forward();
                                   Delay.msDelay(900);
                                   Motor.A.setSpeed(0);
                                   Motor.B.setSpeed(0);
                                   Motor.C.setSpeed(0);
                                   Delay.msDelay(500);
                      }
                    Motor.A.setSpeed(300);
                    Motor.B.setSpeed(300);
                    Motor.A.forward();
                    Motor.B.forward();
                    Delay.msDelay(500);

               }
            else if(id==rfid9||id==rfid10){//彎進去A停車場
                Motor.A.setSpeed(400);
                Motor.B.setSpeed(0);
                Motor.A.forward();
                Motor.B.forward();
                Delay.msDelay(850);
                a=0;
                b=0;
                c=0;
                d=0;
                e=0;
            }
            else if(id==rfid15||id==rfid16){//A停車場
                Motor.A.setSpeed(400);
                Motor.B.setSpeed(400);
                Motor.A.forward();
                Motor.B.forward();
                Delay.msDelay(300);
                break;
            }
            llResult = liner.getResult();
        if(llResult==24){
        Motor.A.setSpeed(300);
        Motor.B.setSpeed(300);
        Motor.A.forward();
        Motor.B.forward();
        }
        else if(llResult==16){//400 500
            Motor.A.setSpeed(300);
            Motor.B.setSpeed(270);
            Motor.A.forward();
            Motor.B.forward();
       }

        else if(llResult==8){//400 500
            Motor.A.setSpeed(270);
            Motor.B.setSpeed(300);
            Motor.A.forward();
            Motor.B.forward();
      }
        else if(llResult==48)//400 500
            Motor.A.setSpeed(300);
            Motor.B.setSpeed(240);
            Motor.A.forward();
            Motor.B.forward();
      }
        else if(llResult==12){//400 500
            Motor.A.setSpeed(240);
            Motor.B.setSpeed(300);
            Motor.A.forward();
            Motor.B.forward();
      }
        else if(llResult==32){//400 500
            Motor.A.setSpeed(300);
            Motor.B.setSpeed(210);
            Motor.A.forward();
            Motor.B.forward();
      }
        else if(llResult==4){//400 500
            Motor.A.setSpeed(210);
            Motor.B.setSpeed(300);
            Motor.A.forward();
            Motor.B.forward();
      }
           else if(llResult==96){//400 500
            Motor.A.setSpeed(300);
            Motor.B.setSpeed(180);
            Motor.A.forward();
            Motor.B.forward();
      }
        else if(llResult==6){//400 500
            Motor.A.setSpeed(180);
            Motor.B.setSpeed(300);
            Motor.A.forward();
            Motor.B.forward();
      }
        else if(llResult==64){//400 500
            Motor.A.setSpeed(300);
            Motor.B.setSpeed(150);
            Motor.A.forward();
            Motor.B.forward();
     }
        else if(llResult==2){//400 500
            Motor.A.setSpeed(150);
            Motor.B.setSpeed(300);
            Motor.A.forward();
            Motor.B.forward();
     }
        else if(llResult==192){//400 500
            Motor.A.setSpeed(300);
            Motor.B.setSpeed(120);
            Motor.A.forward();
            Motor.B.forward();
    }
        else if(llResult==3){//400 500
            Motor.A.setSpeed(120);
            Motor.B.setSpeed(300);
            Motor.A.forward();
            Motor.B.forward();
    }
        else if(llResult==128){//400 500
            Motor.A.setSpeed(300);
            Motor.B.setSpeed(90);
            Motor.A.forward();
            Motor.B.forward();
    }
        else if(llResult==1){//400 500
            Motor.A.setSpeed(90);
            Motor.B.setSpeed(300);
            Motor.A.forward();
            Motor.B.forward();
    }
        else if(llResult>0&&llResult<16){
            Motor.A.setSpeed(90);
            Motor.B.setSpeed(300);
            Motor.A.forward();
            Motor.B.forward();
    }
        else if(llResult>=16&&llResult<256){
            Motor.A.setSpeed(300);
            Motor.B.setSpeed(90);
            Motor.A.forward();
            Motor.B.forward();
    }


  }
             Motor.A.setSpeed(0);
            Motor.B.setSpeed(0);
            Motor.A.forward();
            Motor.B.forward();
 }
}