PuAiSim/Assets/controller.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class controller : MonoBehaviour
{
    public GameObject machineArm;
    public GameObject scanBoard;
    public GameObject elevator_left;
    public GameObject elevator_right;
    public GameObject carModel;
    public GameObject eventGenerator;
    public GameObject[] transferVehicles;
    /// <summary>
    /// 0 idle, 1 停车机械手抓车, 2 停车右电梯送出中跑车接车, 3 停车左电梯送中跑车上楼并送车，cmd=2, 4 停车
    /// 4 fetch trans get car cmd=3, 5 fetch trans go downstairs, 6 fetch arm get car cmd=4, 7 fetch trans go back
    /// </summary>
    public int state;
    public bool ready;
    // 0等待锁，1设备锁
    public int[] doorLock = { 0, 0, 0, 0, 0, 0 };
    public GameObject[] doors;

    /// <summary>
    /// 停车终端个数，从生成器获取
    /// </summary>
    public int numOfParkTerm;
    public int[] termState = { 0, 0, 0, 0, 0, 0 };
    public int[] currTermVar = { 0, 0, 0, 0, 0, 0 };
    private int[] scanned = { 0, 0, 0, 0, 0, 0 };
    private static int[] termIndex = { 4, 6, 7, 9, 10, 12 };
    private List<CarInfo> movingCars;
    public int[] parkSpace;
    private bool initialized;
    private float prevTime;
    // 当前处理指令0-5
    public int currIndex;
    public GameObject parkUsingTrans;
    public GameObject fetchUsingTrans;
    private const int cmdInterval = 3;
    // 准备停入编号
    public int parkSpaceIndex;
    // 准备取出编号
    public int fetchSpaceIndex;
    //// 停车指令终端号
    //private int parkTermIndex;
    // 取车指令终端号
    public int fetchTermIndex;
    private float[] termX = { 1.2f, 6.2f, 8.7f, 13.7f, 16.2f, 21.2f };
    /// <summary>
    /// 中跑车根据左右上排序，便于控制
    /// </summary>
    public GameObject[] orderedTrans;
    /// <summary>
    /// 中跑车状态，0空闲状态；1上中跑车到左暂存位；2右中跑车上行到右边暂存位，可取车；3左中跑车向右到右暂存位，停车接车；4上中跑车到二楼左暂存位
    /// </summary>
    public int transState;

    public GameObject CreateCar(Vector3 position)
    {
        if (carModel != null)
        {
            GameObject aCar = Instantiate(carModel);
            aCar.GetComponent<Transform>().position = position;
            //movingCars.Add(new CarInfo(aCar,position));
            return aCar;
        }
        return null;
    }

    public void DestroyCar(GameObject car)
    {
        //Debug.Log("-----");
        CarInfo temp = new CarInfo(car, Vector3.zero);
        //temp.rndValue = value;
        //Debug.Log(movingCars.Contains(temp));
        if (movingCars.Contains(temp))
        {
            temp = movingCars.Find((CarInfo ci) => { return ci.Equals(temp); });
            int space = (temp.floor - 2) * 15 + temp.space;
            if (parkSpace[space] != 0)
            {
                parkSpace[space] = 0;
            }
            movingCars.Remove(temp);
            Destroy(car);
        }
    }

    public void AddToGarage(GameObject car, int floor, int space)
    {
        if (movingCars != null)
        {
            CarInfo temp = new CarInfo(car, floor, space);
            temp.state = 2;
            movingCars.Add(temp);
            int spaceNo = (floor - 2) * 15 + space;
            parkSpace[spaceNo] = 1;
        }
    }

    public GameObject GetCar(int floor, int space)
    {
        if (movingCars != null)
        {
            for (int i = 0; i < movingCars.Count; i++)
            {
                if (movingCars[i].floor == floor && movingCars[i].space == space && movingCars[i].handle != null)
                {
                    return movingCars[i].handle;
                }
            }
        }
        return null;
    }

    // Start is called before the first frame update
    void Start()
    {
        for (int i = 0; i < doorLock.Length; i++)
        {
            doorLock[i] = 1;
        }
        Random.InitState((int)(Time.time * 1000.0f));
        transferVehicles = GameObject.FindGameObjectsWithTag("transfer");
        if (transferVehicles.Length != 3 || machineArm == null || elevator_left == null || elevator_right == null || carModel == null)
        {
            initialized = false;
            Debug.Log("初始化失败");
            return;
        }
        else
        {
            //termState = new int[6];
            //currTermVar = new int[6];
            fetchTermIndex = -1;
            transState = 0;
            orderedTrans = new GameObject[3];
            int count = 0;
            for (int i = 0; i < transferVehicles.Length; i++)
            {
                //Debug.Log(transferVehicles[i].GetComponent<transferBehavior>().GetFloor()+","+transferVehicles[i].GetComponent<transferBehavior>().GetNearestTerm());
                // 左中跑车
                if (transferVehicles[i].GetComponent<transferBehavior>().GetFloor() == 2 && transferVehicles[i].GetComponent<transferBehavior>().GetSpace() >= 12)
                {
                    orderedTrans[0] = transferVehicles[i]; count++;
                }
                // 右中跑车
                else if (transferVehicles[i].GetComponent<transferBehavior>().GetFloor() == 2 && transferVehicles[i].GetComponent<transferBehavior>().GetSpace() <= 4)
                {
                    orderedTrans[1] = transferVehicles[i]; count++;
                }
                // 上中跑车
                else if (transferVehicles[i].GetComponent<transferBehavior>().GetFloor() != 2)
                {
                    orderedTrans[2] = transferVehicles[i]; count++;
                }
            }
            if (count != 3)
            {
                initialized = false;
                Debug.Log("初始化失败");
                return;
            }
            ready = false;
            parkUsingTrans = null;
            state = 0;
            prevTime = Time.time;
            movingCars = new List<CarInfo>();
            initialized = true;
            currIndex = 0;
            parkSpace = new int[165];
            //// 中跑车排序
            //int floor = 13;
            //int index = -1;
            //for (int i = 0; i < transferVehicles.Length; i++)
            //{
            //    for (int j = i; j < transferVehicles.Length; j++)
            //    {
            //        if (transferVehicles[j].GetComponent<transferBehavior>().GetFloor() < floor)
            //        {
            //            floor = transferVehicles[j].GetComponent<transferBehavior>().GetFloor();
            //            index = j;
            //        }
            //    }
            //    GameObject temp = transferVehicles[i];
            //    transferVehicles[i] = transferVehicles[index];
            //    transferVehicles[index] = temp;
            //    floor = 13;
            //    index = -1;
            //}

            // object reference test
            //GameObject temp = CreateCar(new Vector3(5, 5, 5));
            //AddToGarage(temp, 2, 2);
            //test = temp;
            //temp = null;
            //DestroyCar(test);
            //Debug.Log("完成");
        }
    }

    // 根据停取车选择中跑车编号
    public int GetSpecificTrans(bool park)
    {
        for (int i = 0; i < transferVehicles.Length; i++)
        {
            int temp = transferVehicles[i].GetComponent<transferBehavior>().GetFloor();
        }
        return -1;

    }

    // 模拟PLC读取终端指令
    void UpdateCmd()
    {
        //Debug.Log("00000");
        if (parkSpace == null)
        {
            parkSpace = new int[165];
            for (int i = 1; i < 166; i++)
            {
                parkSpace[i] = 0;
            }
        }
        //Debug.Log("currIndex: "+currIndex);
        //Debug.Log("currIndex: "+currIndex +", state: "+currTermVar[currIndex]);
        if (currIndex > 5)
        {
            currIndex = 0;
        }
        if (currTermVar[currIndex] == 0)
        {
            currIndex++;
            // 遍历完6个终端
            if (currIndex > 5)
            {
                currIndex = 0;
                // 拷贝指令
                for (int i = 0; i < 6; i++)
                {
                    currTermVar[i] = termState[i];
                }
            }
        }
        //Debug.Log("22222");
        if (fetchTermIndex <= 0)
        {
            for (int i = 0; i < 6; i++)
            {
                if (termState[i] == 2)
                {
                    fetchTermIndex = i;
                    break;
                }
            }
        }
        //Debug.Log("curIndex: " + currIndex + ", " + currTermVar[currIndex] + ", " + termState[currIndex]);
        // 更新停取车位编号
        //Debug.Log("33333");
        if (currTermVar[currIndex] == 1)
        {
            //Debug.Log("44444");
            //parkTermIndex = currIndex;
            if (parkSpaceIndex <= 0)
            {
                //parkSpaceIndex = Mathf.FloorToInt(Random.value*(167-46))+46;//暂时测试5楼以上功能
                //Debug.Log("找空位");
                for (int i = 46; i < 166; i++)
                {
                    //Debug.Log("检测到停车指令，车位" + i + ": " + parkSpace[i]);
                    if (parkSpace[i] == 0)
                    {
                        parkSpaceIndex = i; break;
                    }
                    //for (int j = 0; j < movingCars.Count; j++)
                    //{
                    //    if ((movingCars[j].floor - 2) * 15 + movingCars[j].space == parkSpaceIndex)
                    //    {
                    //        parkSpaceIndex = 0;
                    //        break;
                    //    }
                    //}
                    //if (parkSpaceIndex != 0) break;
                }
            }
        }
        // 取车位指令不断遍历
        else if (currTermVar[currIndex] == 2)
        {
            Debug.Log(movingCars.Count);
            if (movingCars.Count <= 0)
            {
                currIndex++;
                if (currIndex > 5)
                {
                    currIndex = 0;
                }
            }
            // 取车指令已就位
            else if (fetchTermIndex != -1)
            {
                Debug.Log("55555");
                //不是遍历到的编号，则跳过直到找到正确编号为止
                if (fetchTermIndex != currIndex)
                {
                    currIndex++;
                    return;
                }
                else
                {
                    while (fetchSpaceIndex <= 0 && movingCars.Count > 0)
                    {
                        int index = Mathf.FloorToInt(Random.value * movingCars.Count);
                        fetchSpaceIndex = (movingCars[index].floor - 2) * 15 + movingCars[index].space;
                    }
                    if (parkSpace[fetchSpaceIndex] > 0)
                    {
                        parkSpace[fetchSpaceIndex] = 0;
                    }
                }
            }
        }
    }

    /// <summary>
    /// 中跑车运动控制，上方5、6层为缓冲
    /// </summary>
    void TransferMove()
    {
        //if (currTermVar[currIndex] == 0) return;
        if (orderedTrans.Length != 3) return;
        int fetchFloor = (fetchSpaceIndex - 1) / 15 + 2;
        int fetchX = fetchSpaceIndex - (fetchFloor - 2) * 15;
        int parkFloor = (parkSpaceIndex - 1) / 15 + 2;
        int parkX = parkSpaceIndex - (parkFloor - 2) * 15;
        // 中跑车循环，无论停取，接到车给中跑车变量赋值
        switch (transState)
        {
            case 0:
                if (parkSpaceIndex > 0 || fetchSpaceIndex > 0)
                {
                    transState = 1;
                }
                break;
            case 1://上左
                Debug.Log("1、上左");
                int floor = orderedTrans[2].GetComponent<transferBehavior>().GetFloor();

                if (floor == 2)
                {
                    Debug.Log("上左异常");
                    return;
                }
                // 停在楼上左边缓冲位则进下一步
                if (orderedTrans[2].GetComponent<transferBehavior>().state == 0 && orderedTrans[2].GetComponent<transferBehavior>().GetSpace() == 13)
                {
                    transState = 2;
                    return;
                }
                if (orderedTrans[2].GetComponent<transferBehavior>().state == 0)
                {
                    //Debug.Log("上左 车状态0");
                    //Debug.Log(parkX +", "+ orderedTrans[2].GetComponent<transferBehavior>().GetSpace());
                    // 空中跑车不同层不取车
                    if (fetchFloor != floor && orderedTrans[2].GetComponent<transferBehavior>().load == null)
                    {
                        orderedTrans[2].GetComponent<transferBehavior>().SetTarget(floor, 13);
                    }
                    // 空中跑车同层则取车
                    else if (orderedTrans[2].GetComponent<transferBehavior>().load == null && fetchUsingTrans == null)
                    {
                        // 取车调用电梯便于移动
                        fetchUsingTrans = orderedTrans[2];
                        orderedTrans[2].GetComponent<transferBehavior>().SetTarget(floor, fetchX);
                        if (fetchX <= 2 && elevator_right.GetComponent<elevatorBehavior>().state == 0 && elevator_right.GetComponent<elevatorBehavior>().load == null)
                        {
                            elevator_right.GetComponent<elevatorBehavior>().targetFloor = floor;
                        }
                        else if (fetchX >= 14 && elevator_left.GetComponent<elevatorBehavior>().state == 0 && elevator_left.GetComponent<elevatorBehavior>().load == null)
                        {
                            elevator_left.GetComponent<elevatorBehavior>().targetFloor = floor;
                        }
                    }
                    // 取车中跑车接到车，送到13位
                    else if (fetchUsingTrans == orderedTrans[2] && orderedTrans[2].GetComponent<transferBehavior>().load != null && orderedTrans[2].GetComponent<transferBehavior>().GetSpace() != 13)
                    {
                        orderedTrans[2].GetComponent<transferBehavior>().SetTarget(floor, 13);
                    }
                    // 停车中跑车有车，送入车位
                    else if (parkUsingTrans == orderedTrans[2] && orderedTrans[2].GetComponent<transferBehavior>().load != null && parkFloor == floor /*&& parkX != orderedTrans[2].GetComponent<transferBehavior>().GetSpace()*/)
                    {
                        orderedTrans[2].GetComponent<transferBehavior>().SetTarget(floor, parkX);
                        if (parkX <= 2 && elevator_right.GetComponent<elevatorBehavior>().state == 0 && elevator_right.GetComponent<elevatorBehavior>().load == null)
                        {
                            elevator_right.GetComponent<elevatorBehavior>().targetFloor = floor;
                        }
                        else if (parkX >= 14 && elevator_left.GetComponent<elevatorBehavior>().state == 0 && elevator_left.GetComponent<elevatorBehavior>().load == null)
                        {
                            elevator_left.GetComponent<elevatorBehavior>().targetFloor = floor;
                        }
                    }
                    // 与上部条件隔离，停车中跑车已送完车，清理
                    if (parkUsingTrans == orderedTrans[2] && orderedTrans[2].GetComponent<transferBehavior>().load == null && parkFloor == floor && parkX == orderedTrans[2].GetComponent<transferBehavior>().GetSpace())
                    {
                        currTermVar[currIndex] = 0;
                        termState[currIndex] = 0;
                        parkSpaceIndex = -1;
                        parkUsingTrans = null;
                    }
                }
                if (orderedTrans[2].GetComponent<transferBehavior>().state == 4)
                {
                    // 有取车指令空载取车
                    if (fetchX > 0 && orderedTrans[2].GetComponent<transferBehavior>().GetSpace() == fetchX && orderedTrans[2].GetComponent<transferBehavior>().load == null)
                    {
                        orderedTrans[2].GetComponent<transferBehavior>().SendCmd(3);
                    }
                    // 有停车指令送车，清除停车指令，复位变量
                    else if (parkX > 0 && orderedTrans[2].GetComponent<transferBehavior>().GetSpace() == parkX && orderedTrans[2].GetComponent<transferBehavior>().load != null)
                    {
                        Debug.Log("停车入库，清除指令");
                        orderedTrans[2].GetComponent<transferBehavior>().SendCmd(2);
                        //state = 0;
                    }
                    // 无取车指令或已在13位
                    else if (fetchX <= 0 || orderedTrans[2].GetComponent<transferBehavior>().GetSpace() == 13)
                    {
                        orderedTrans[2].GetComponent<transferBehavior>().SendCmd(-1);
                    }
                }
                break;
            case 2://右上
                Debug.Log("2、右上");
                floor = orderedTrans[1].GetComponent<transferBehavior>().GetFloor();
                int targetFloor = 5;
                // 修改目标楼层
                if (parkFloor <= 2 && orderedTrans[2].GetComponent<transferBehavior>().GetFloor() == 5)
                {
                    targetFloor = 6;
                }
                // 停车中跑车上行，修改目标楼层
                if (parkFloor > 2 && parkUsingTrans == orderedTrans[1] && orderedTrans[1].GetComponent<transferBehavior>().load != null)
                {
                    targetFloor = parkFloor;
                }
                // 移动中跑车
                if (orderedTrans[1].GetComponent<transferBehavior>().state == 0)
                {
                    if (floor != targetFloor || orderedTrans[1].GetComponent<transferBehavior>().GetSpace() != 3)
                    {
                        orderedTrans[1].GetComponent<transferBehavior>().SetTarget(targetFloor, 3);
                    }
                    else
                    {
                        transState = 3;
                        return;
                    }
                }
                else if (orderedTrans[1].GetComponent<transferBehavior>().state == 4)
                {
                    orderedTrans[1].GetComponent<transferBehavior>().SendCmd(-1);
                }
                if (elevator_right.GetComponent<elevatorBehavior>().state == 0)
                {
                    if (orderedTrans[1].GetComponent<transferBehavior>().waitForElevator && elevator_right.GetComponent<elevatorBehavior>().load == null)
                    {
                        elevator_right.GetComponent<elevatorBehavior>().targetFloor = 2;
                    }
                    else if (orderedTrans[1].GetComponent<transferBehavior>().state == 2 && elevator_right.GetComponent<elevatorBehavior>().load != null)
                    {
                        elevator_right.GetComponent<elevatorBehavior>().targetFloor = targetFloor;
                    }
                }
                break;
            case 3://左右，机械手到位则停车接车或取车送车
                Debug.Log("3、左右");
                floor = orderedTrans[0].GetComponent<transferBehavior>().GetFloor();
                int space = orderedTrans[0].GetComponent<transferBehavior>().GetSpace();

                if (orderedTrans[0].GetComponent<transferBehavior>().state == 4)
                {
                    // 中跑车送车交接
                    if (machineArm.GetComponent<machineArmBehavior>().state == 160 && orderedTrans[0].GetComponent<transferBehavior>().load != null && fetchUsingTrans == orderedTrans[0])
                    {
                        orderedTrans[0].GetComponent<transferBehavior>().SendCmd(4);
                        Debug.Log("取车送车，待机械手抓");
                    }
                    // 中跑车停车接车或空载
                    else if (fetchUsingTrans != orderedTrans[0])
                    {
                        if (floor == 2 && space == 3)
                        {
                            orderedTrans[0].GetComponent<transferBehavior>().SendCmd(-1);
                        }
                    }
                }
                if (orderedTrans[0].GetComponent<transferBehavior>().state == 0)
                {
                    // 与上部条件隔离，取车中跑车已送完车，清理
                    if (fetchUsingTrans == orderedTrans[0] && orderedTrans[0].GetComponent<transferBehavior>().load == null)
                    {
                        Debug.Log("取车完成，清理");
                        currTermVar[currIndex] = 0;
                        termState[currIndex] = 0;
                        fetchSpaceIndex = -1;
                        fetchTermIndex = -1;
                        fetchUsingTrans = null;
                    }
                    // 左中跑车负载，先启动机械手取车
                    if (floor == 2 && orderedTrans[0].GetComponent<transferBehavior>().load != null && fetchUsingTrans == orderedTrans[0])
                    {
                        // 机械手等待取车
                        if (machineArm.GetComponent<machineArmBehavior>().state == 0 && machineArm.GetComponent<machineArmBehavior>().terminalIndex == -1)
                        {
                            machineArm.GetComponent<machineArmBehavior>().terminalIndex = fetchTermIndex + 7;
                        }
                        orderedTrans[0].GetComponent<transferBehavior>().SetTarget(2, termIndex[fetchTermIndex]);
                    }
                    // 停车机械手等中跑车,中跑车移动
                    else if (parkUsingTrans == null && parkSpaceIndex > 0 && floor == 2 && space != termIndex[currIndex] && orderedTrans[0].GetComponent<transferBehavior>().load == null)
                    {
                        // 机械手等待停车
                        if (machineArm.GetComponent<machineArmBehavior>().state == 0 && machineArm.GetComponent<machineArmBehavior>().terminalIndex == -1)
                        {
                            machineArm.GetComponent<machineArmBehavior>().terminalIndex = currIndex + 1;
                        }
                        orderedTrans[0].GetComponent<transferBehavior>().SetTarget(2, termIndex[currIndex]);
                    }
                    // 中跑车接到车，且不是取车中跑车
                    else if (parkSpaceIndex > 0 && floor == 2 && space == termIndex[currIndex] && orderedTrans[0].GetComponent<transferBehavior>().load != null && fetchUsingTrans != orderedTrans[0])
                    {
                        if (parkUsingTrans == null)
                        {
                            parkUsingTrans = orderedTrans[0];
                        }
                        orderedTrans[0].GetComponent<transferBehavior>().SetTarget(2, 3);
                    }
                    else if (floor == 2 && space == 3)
                    {
                        transState = 4;
                    }
                    else
                    {
                        orderedTrans[0].GetComponent<transferBehavior>().SetTarget(2, 3);
                    }
                }
                break;
            case 4://上下
                Debug.Log("4、上下");
                floor = orderedTrans[2].GetComponent<transferBehavior>().GetFloor();
                space = orderedTrans[2].GetComponent<transferBehavior>().GetSpace();
                if (orderedTrans[2].GetComponent<transferBehavior>().state == 0)
                {
                    if (floor != 2 || space != 13)
                    {
                        orderedTrans[2].GetComponent<transferBehavior>().SetTarget(2, 13);
                    }
                    else
                    {
                        // 更新中跑车顺序
                        GameObject temp = orderedTrans[0];
                        orderedTrans[0] = orderedTrans[2];
                        orderedTrans[2] = orderedTrans[1];
                        orderedTrans[1] = temp;
                        transState = 0;
                    }
                }
                else if (orderedTrans[2].GetComponent<transferBehavior>().state == 4)
                {
                    orderedTrans[2].GetComponent<transferBehavior>().SendCmd(-1);
                }
                if (elevator_left.GetComponent<elevatorBehavior>().state == 0)
                {
                    if (orderedTrans[2].GetComponent<transferBehavior>().waitForElevator && elevator_left.GetComponent<elevatorBehavior>().load == null)
                    {
                        elevator_left.GetComponent<elevatorBehavior>().targetFloor = floor;
                    }
                    else if (orderedTrans[2].GetComponent<transferBehavior>().state == 2 && elevator_left.GetComponent<elevatorBehavior>().load != null)
                    {
                        elevator_left.GetComponent<elevatorBehavior>().targetFloor = 2;
                    }
                }
                break;
            default: break;
        }
        //// 处理停车中
        //if (currIndex <= numOfParkTerm - 1 && currTermVar[currIndex] == 1)
        //{
        //}
        //// 处理取车中
        //else if(currIndex > numOfParkTerm - 1 && currTermVar[currIndex] == 2)
        //{
        //}
    }

    void BasicStateMachine()
    {
        if (parkSpace == null)
        {
            parkSpace = new int[165];
            for (int i = 1; i < 166; i++)
            {
                parkSpace[i] = 0;
            }
        }
        switch (state)
        {
            case 0:
                UpdateCmd();
                //Debug.Log("更新cmd完成");
                if (currTermVar[currIndex] == 1 && parkSpaceIndex > 0)
                {
                    state = 1;
                }
                else if (currTermVar[currIndex] == 2 && fetchSpaceIndex > 0)
                {
                    state = 4;
                }
                break;
            // 停车控制机械手抓车
            case 1:
                if (machineArm.GetComponent<machineArmBehavior>().state == 23)
                {
                    state = 2;
                }
                if (machineArm.GetComponent<machineArmBehavior>().state == 0 && machineArm.GetComponent<machineArmBehavior>().terminalIndex == -1)
                {
                    machineArm.GetComponent<machineArmBehavior>().terminalIndex = currIndex + 1;
                }
                break;
            case 2:
                if (machineArm.GetComponent<machineArmBehavior>().state == 14)
                {
                    machineArm.GetComponent<machineArmBehavior>().state = 24;
                }
                //中跑车运动移至transMove
                if (parkUsingTrans != null)
                {
                    int transState = parkUsingTrans.GetComponent<transferBehavior>().state;
                    int transFloor = parkUsingTrans.GetComponent<transferBehavior>().GetFloor();
                    int transSpace = parkUsingTrans.GetComponent<transferBehavior>().GetSpace();
                    ////Debug.Log("curr trans: " + transFloor + ", " + transSpace);
                    //// 未抵达目标点
                    //if (transState == 0 && (transFloor != 2 || transSpace != termIndex[currIndex]))
                    //{
                    //    parkUsingTrans.GetComponent<transferBehavior>().SetTarget(2, termIndex[currIndex]);
                    //}
                    //// 等电梯
                    //else if (parkUsingTrans.GetComponent<transferBehavior>().waitForElevator)
                    //{
                    //    //Debug.Log(elevator_right.GetComponent<elevatorBehavior>().state);
                    //    //Debug.Log(elevator_right.GetComponent<elevatorBehavior>().load == null);
                    //    if (transSpace < 6 && elevator_right.GetComponent<elevatorBehavior>().state == 0 && elevator_right.GetComponent<elevatorBehavior>().load == null)
                    //    {
                    //        elevator_right.GetComponent<elevatorBehavior>().targetFloor = transFloor;
                    //    }
                    //    else if (transSpace > 10 && elevator_left.GetComponent<elevatorBehavior>().state == 0 && elevator_left.GetComponent<elevatorBehavior>().load == null)
                    //    {
                    //        elevator_left.GetComponent<elevatorBehavior>().targetFloor = transFloor;
                    //    }
                    //}
                    //else if (transState == 2)
                    //{
                    //    if (transSpace < 6 && elevator_right.GetComponent<elevatorBehavior>().state == 0 && elevator_right.GetComponent<elevatorBehavior>().load != null)
                    //    {
                    //        elevator_right.GetComponent<elevatorBehavior>().targetFloor = 2;
                    //    }
                    //    else if (transSpace > 10 && elevator_left.GetComponent<elevatorBehavior>().state == 0 && elevator_left.GetComponent<elevatorBehavior>().load != null)
                    //    {
                    //        elevator_left.GetComponent<elevatorBehavior>().targetFloor = 2;
                    //    }
                    //}
                    /*else */
                    if (transFloor == 2 && transSpace == termIndex[currIndex] && parkUsingTrans.GetComponent<transferBehavior>().load != null)
                    {
                        // 中跑车到停车接车位置
                        state = 3;
                    }
                }
                // 寻找空闲中跑车，赋值由transMove完成
                else if (parkUsingTrans == null)
                {
                    //int transIndex = GetSpecificTrans(Mathf.FloorToInt((parkSpaceIndex - 1) / 15.0f) + 2);
                    //Debug.Log("choose trans: " + transIndex);
                    //if (transIndex != -1)
                    //    usingTrans = transferVehicles[transIndex];

                    //for (int i = 0; i < transferVehicles.Length; i++)
                    //{
                    //    if (transferVehicles[i].GetComponent<transferBehavior>().GetFloor() < manDist)
                    //    {
                    //        manDist = transferVehicles[i].GetComponent<transferBehavior>().GetFloor();
                    //        usingTrans = transferVehicles[i];
                    //    }
                    //}

                }
                break;
            case 3:
                if (parkUsingTrans != null)
                {
                    GameObject transLoad = parkUsingTrans.GetComponent<transferBehavior>().load;
                    int transState = parkUsingTrans.GetComponent<transferBehavior>().state;
                    int transFloor = parkUsingTrans.GetComponent<transferBehavior>().GetFloor();
                    int transSpace = parkUsingTrans.GetComponent<transferBehavior>().GetSpace();
                    int targetFloor = Mathf.FloorToInt((parkSpaceIndex - 1) / 15.0f) + 2;
                    int targetSpace = parkSpaceIndex - (targetFloor - 2) * 15;

                    if (transLoad == null && transState == 0)
                    {
                        if (machineArm.GetComponent<machineArmBehavior>().state == 14)
                        {
                            machineArm.GetComponent<machineArmBehavior>().state = 24;
                        }
                        // 停车中跑车空载，空闲且在目标层，对应中跑车1状态
                        if (transFloor == targetFloor)
                        {
                            currTermVar[currIndex] = 0;
                            termState[currIndex] = 0;
                            parkSpaceIndex = -1;
                            parkUsingTrans = null;
                            state = 0;
                            //eventGenerator.GetComponent<eventGenerateBehavior>().myState[currIndex]=0;
                        }
                    }

                    //// 中跑车在下方接车点
                    //if (transLoad != null && transState == 0 && transFloor == 2 && transSpace == termIndex[currIndex])
                    //{
                    //    if (parkSpaceIndex > 0 && parkSpaceIndex < 166)
                    //    {
                    //        parkUsingTrans.GetComponent<transferBehavior>().SetTarget(targetFloor, targetSpace);
                    //    }
                    //}
                    // 电梯由transMove控制
                    //// 电梯去二楼接
                    //else if (parkUsingTrans.GetComponent<transferBehavior>().waitForElevator)
                    //{
                    //    if (/*transSpace < 6 && */elevator_right.GetComponent<elevatorBehavior>().state == 0 && elevator_right.GetComponent<elevatorBehavior>().load == null)
                    //    {
                    //        elevator_right.GetComponent<elevatorBehavior>().targetFloor = 2;
                    //    }
                    //    //else if (transSpace > 10 && elevator_left.GetComponent<elevatorBehavior>().state == 0 && elevator_left.GetComponent<elevatorBehavior>().load == null)
                    //    //{
                    //    //    elevator_left.GetComponent<elevatorBehavior>().targetFloor = 2;
                    //    //}
                    //}
                    //// 电梯去目标层
                    //else if (transState == 2)
                    //{
                    //    if (/*transSpace < 6 &&*/ elevator_right.GetComponent<elevatorBehavior>().state == 0 && elevator_right.GetComponent<elevatorBehavior>().load != null)
                    //    {
                    //        elevator_right.GetComponent<elevatorBehavior>().targetFloor = targetFloor;
                    //    }
                    //    //else if (transSpace > 10 && elevator_left.GetComponent<elevatorBehavior>().state == 0 && elevator_left.GetComponent<elevatorBehavior>().load != null)
                    //    //{
                    //    //    elevator_left.GetComponent<elevatorBehavior>().targetFloor = targetFloor;
                    //    //}
                    //}
                    //// 中跑车在上方车位,等待送车进车位
                    //if (transFloor == targetFloor)
                    //{
                    //    if (transState == 4)
                    //    {
                    //        // 送车
                    //        if (transSpace == targetSpace)
                    //        {
                    //            parkUsingTrans.GetComponent<transferBehavior>().SendCmd(2);
                    //        }
                    //        // 移位状态复位
                    //        else if (transLoad == null)
                    //        {
                    //            parkUsingTrans.GetComponent<transferBehavior>().SendCmd(-1);
                    //        }
                    //    }
                    //    else if (transLoad == null && transState == 0)
                    //    {
                    //        //if (transSpace == targetSpace)
                    //        //{
                    //        if (machineArm.GetComponent<machineArmBehavior>().state == 14)
                    //        {
                    //            machineArm.GetComponent<machineArmBehavior>().state = 24;
                    //        }
                    //        if (transSpace > 2 && transSpace < 14)
                    //        {
                    //            currTermVar[currIndex] = 0;
                    //            termState[currIndex] = 0;
                    //            parkSpaceIndex = -1;
                    //            parkUsingTrans = null;
                    //            state = 0;
                    //            //eventGenerator.GetComponent<eventGenerateBehavior>().myState[currIndex]=0;
                    //        }
                    //        else
                    //        {
                    //            parkUsingTrans.GetComponent<transferBehavior>().SetTarget(transFloor, transSpace <= 3 ? 3 : 13);
                    //        }
                    //        //}
                    //    }
                    //    else if (transState == 3)
                    //    {
                    //        //Debug.Log(targetSpace);
                    //        if (targetSpace < 4 && elevator_right.GetComponent<elevatorBehavior>().state == 0 && elevator_right.GetComponent<elevatorBehavior>().load == null)
                    //        {
                    //            elevator_right.GetComponent<elevatorBehavior>().targetFloor = targetFloor;
                    //        }
                    //        else if (targetSpace > 12 && elevator_left.GetComponent<elevatorBehavior>().state == 0 && elevator_left.GetComponent<elevatorBehavior>().load == null)
                    //        {
                    //            elevator_left.GetComponent<elevatorBehavior>().targetFloor = targetFloor;
                    //        }
                    //    }
                    //}
                }
                break;

            // 取车
            case 4:

                break;
            case 5:

                break;
            case 6:

                break;
            case 7:

                break;
            default: break;
        }
    }

    /// <summary>
    /// 检查停取终端状态，停车终端是否有车等待，取车终端是否有人等待
    /// </summary>
    void CheckState()
    {
        int[] waitingForDoors = eventGenerator.GetComponent<eventGenerateBehavior>().waitingForDoors;
        GameObject[] objs = eventGenerator.GetComponent<eventGenerateBehavior>().myObject;
        doors = eventGenerator.GetComponent<eventGenerateBehavior>().doors;
        // 检查门状态并长期上锁
        for (int i = 0; i < doors.Length; i++)
        {
            if (doors[i].GetComponent<doorBehavior>().close && doorLock[i] != 1 && !doors[i].GetComponent<doorBehavior>().moving)
            {
                doorLock[i] = 1;
            }
            else if (!doors[i].GetComponent<doorBehavior>().close || doors[i].GetComponent<doorBehavior>().moving)
            {
                doorLock[i] = 0;
            }
        }

        // 检查停车终端等待开门车辆
        for (int i = 0; i < termState.Length; i++)
        {
            if (termState[i] != 0)
                continue;
            if (i < numOfParkTerm)
            {
                if (waitingForDoors[i] == 1)
                {
                    //找机会开门。机械手，扫描板与二楼中跑车不在上空可开门。否则等待或移动空闲设备到旁边
                    int armState = machineArm.GetComponent<machineArmBehavior>().state;
                    int armTerm = machineArm.GetComponent<machineArmBehavior>().GetNearestTerm();
                    if (armTerm == i)
                    {
                        if (armState == 0)
                        {
                            // 往右移
                            if (i > 0 && doors[i - 1].GetComponent<doorBehavior>().close && !doors[i - 1].GetComponent<doorBehavior>().moving)
                            {
                                float diff = machineArm.transform.position.x - termX[i - 1];
                                machineArm.GetComponent<machineArmBehavior>().ArmMove(0, diff, 0);
                            }
                            else if (i < termState.Length - 1 && doors[i + 1].GetComponent<doorBehavior>().close && !doors[i + 1].GetComponent<doorBehavior>().moving)
                            {
                                float diff = machineArm.transform.position.x - termX[i + 1];
                                machineArm.GetComponent<machineArmBehavior>().ArmMove(0, diff, 0);
                            }
                        }
                        else
                        {
                            Debug.Log("机械手位置异常");
                        }
                        continue;
                    }

                    int boardState = scanBoard.GetComponent<scanBoardBehavior>().state;
                    int boardTerm = scanBoard.GetComponent<scanBoardBehavior>().GetNearestTerm();
                    if (boardTerm == i)
                    {
                        if (boardState == 0)
                        {
                            // 往右移
                            if (i > 0 && doors[i - 1].GetComponent<doorBehavior>().close && !doors[i - 1].GetComponent<doorBehavior>().moving)
                            {
                                float diff = scanBoard.transform.position.x - termX[i - 1];
                                if (Mathf.Abs(diff) > 0.25f)
                                {
                                    scanBoard.transform.Translate(Vector3.left * scanBoard.GetComponent<scanBoardBehavior>().speedX * Time.deltaTime);
                                }
                            }
                            else if (i < termState.Length - 1 && doors[i + 1].GetComponent<doorBehavior>().close && !doors[i + 1].GetComponent<doorBehavior>().moving)
                            {
                                float diff = scanBoard.transform.position.x - termX[i + 1];
                                if (Mathf.Abs(diff) > 0.25f)
                                {
                                    scanBoard.transform.Translate(Vector3.right * scanBoard.GetComponent<scanBoardBehavior>().speedX * Time.deltaTime);
                                }
                            }
                        }
                        else
                        {
                            Debug.Log("扫描板位置异常");
                        }
                        continue;
                    }

                    bool transCheck = true;
                    for (int j = 0; j < transferVehicles.Length; j++)
                    {
                        int index = transferVehicles[j].GetComponent<transferBehavior>().GetNearestTerm();
                        int transState = transferVehicles[j].GetComponent<transferBehavior>().state;
                        // 上方有中跑车
                        if (i == index)
                        {
                            if (transState == 0)
                            {
                                // 往右移
                                if (i > 0 && doors[i - 1].GetComponent<doorBehavior>().close && !doors[i - 1].GetComponent<doorBehavior>().moving)
                                {
                                    float diff = transferVehicles[j].transform.position.x - termX[i - 1];
                                    transferVehicles[j].GetComponent<transferBehavior>().TransferMove(0, diff, 0);
                                }
                                else if (i < termState.Length - 1 && doors[i + 1].GetComponent<doorBehavior>().close && !doors[i + 1].GetComponent<doorBehavior>().moving)
                                {
                                    float diff = transferVehicles[j].transform.position.x - termX[i + 1];
                                    transferVehicles[j].GetComponent<transferBehavior>().TransferMove(0, diff, 0);
                                }
                            }
                            else
                            {
                                Debug.Log("中跑车位置异常");
                                transCheck &= false;
                            }
                            continue;
                        }
                    }
                    if (!transCheck) continue;
                    // 检查通过后开门
                    doors[i].GetComponent<doorBehavior>().close = false;
                    waitingForDoors[i] = 0;
                }
                // 终端车辆已停好且未处理，关门扫描后发停车指令
                else if (objs[i] != null && objs[i].transform.position.z <= -2.9f && termState[i] == 0)
                {
                    if (!doors[i].GetComponent<doorBehavior>().moving)
                    {
                        if (!doors[i].GetComponent<doorBehavior>().close)
                        {
                            doors[i].GetComponent<doorBehavior>().close = true;
                        }
                        else if (scanned[i] == 1)
                        {
                            scanned[i] = 0;
                            termState[i] = 1;
                            eventGenerator.GetComponent<eventGenerateBehavior>().ClearObject(i + 1, true);
                            //Debug.Log(i + 1);
                        }
                        else if (scanned[i] == 0)
                        {
                            if (scanBoard.GetComponent<scanBoardBehavior>().state == 0 && !scanBoard.GetComponent<scanBoardBehavior>().scanned)
                            {
                                if (scanBoard.GetComponent<scanBoardBehavior>().GetNearestTerm() < i)
                                    scanBoard.GetComponent<scanBoardBehavior>().targetTerm = i + 2;
                                else
                                    scanBoard.GetComponent<scanBoardBehavior>().targetTerm = i;
                            }
                            else if (scanBoard.GetComponent<scanBoardBehavior>().scanned)
                            {
                                scanned[i] = 1;
                                scanBoard.GetComponent<scanBoardBehavior>().scanned = false;
                            }
                        }
                    }
                }
            }
            // 检查取车终端是否空闲，且有人等待，有则发送取车指令
            else if (i >= numOfParkTerm && termState[i] == 0 && objs[i] != null && objs[i].transform.position.z <= 0.5f)
            {
                if (movingCars != null && movingCars.Count > 0)
                {
                    termState[i] = 2;
                    eventGenerator.GetComponent<eventGenerateBehavior>().ClearObject(i + 1, true);
                }
            }
        }
    }

    // Update is called once per frame
    void Update()
    {
        if (initialized)
        {
            numOfParkTerm = eventGenerator.GetComponent<eventGenerateBehavior>().numOfParkTerm;
            if (movingCars == null)
            {
                movingCars = new List<CarInfo>();
            }
            try
            {
                UpdateCmd();
            }
            catch (System.Exception e) { Debug.Log(e.StackTrace + "\n异常。。。。。\n" + e.Message); }
            CheckState();
            TransferMove();
            //BasicStateMachine();

            ////控制频率,30s生成停取指令
            //if ((Time.time - prevTime) > cmdInterval)
            //{
            //    //elevator_left.GetComponent<elevatorBehavior>().targetFloor = Random.Range(1, 5);
            //    int termRndNum = Mathf.FloorToInt(Random.value * 6);
            //    //Debug.Log(termRndNum);
            //    if (termState[termRndNum] == 0)
            //    {
            //        //if (termRndNum > 2 && movingCars.Count > 0)
            //        //    termState[termRndNum] = 2;
            //        if (termRndNum <= 2)
            //            termState[termRndNum] = 1;
            //    }
            //    prevTime = Time.time;
            //}

        }
    }
}

public class CarInfo
{
    public CarInfo(GameObject handle, int floor, int space)
    {
        this.handle = handle;
        this.floor = floor;
        this.space = space;
        pos = new Vector3((space - 1) * 35.0f / 14.0f - 6.3f, (floor - 1) * 3.0f, -7.9f);
    }
    public CarInfo(GameObject handle, Vector3 pos)
    {
        this.handle = handle;
        this.pos = pos;
        floor = Mathf.RoundToInt(pos.y / 3.0f + 1);
        space = Mathf.RoundToInt((pos.x + 6.3f) * 15.0f / 35.0f + 1);
    }
    public override bool Equals(object obj)
    {
        if (obj == null) return false;
        //return rndValue == ((CarInfo)obj).rndValue;
        //Debug.Log(handle);
        //Debug.Log(((CarInfo)obj).handle);
        return handle.Equals(((CarInfo)obj).handle);
    }
    public override int GetHashCode()
    {
        return handle.GetHashCode();
    }

    public GameObject handle;
    public int floor;
    public int space;
    public Vector3 pos;
    public int state; // 0 idle, 1 attached, 2 in space
}