StandardSence/StandardScene.Protocol.VDA5050/VDACar/VDA5050Interface.cs

using SimpleCore.BasicProps;
using SimpleCore.Library;
using SimpleCore.Traffic;
using SimpleCore;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using CommonUsage.Protocols.VDA5050.Objects;
using System.Security.Cryptography.X509Certificates;
using System.Numerics;
using System.Threading;
using System.Net;
using System.Net.Http;
using Nancy.Routing;
using Newtonsoft.Json;
using Simple3;

namespace StandardScene.CarTypes
{
    internal class VDA5050Interface : AGVInterface
    {
        public static int TaskId = 0;
        HttpClient hc = new HttpClient();

        public override bool TryLock(int siteId)
        {
            if (!_car.status.usage.Get().scheduling) throw new Exception("abandoned");
            var siteId_temp = siteId;
            var pendinglocks0 = _car.status.pendingLocks[0];
            if (siteId_temp != pendinglocks0)
            {
                throw new Exception("lock not according to sequence");
            }
            return TrafficControl.TryLock(_car, siteId);
        }

        public override void Leave(int siteId)
        {
            TrafficControl.Leave(_car, siteId);
        }

        public VDA5050Interface(int id)
        {
            _car = (VDA5050Car)SimpleLib.GetCar(id);
            // _car.RouteCache = new();
            // _car.TaskId = TaskId++;
            if (_car.RouteCache.Count ==0||_car.RouteCache.Count>0&&_car.RouteCache.Last().State==VDA5050Segment.SegState.Executed)//接续任务
            {
                // _car.RouteCache = new();
                // _car.ResetLastNodeSequenceId();
                _car.ReseData();
                _car.TaskId = TaskId;
                TaskId += 1;
            }
        }

        public void Go(int srcId, int dstId, int trackId, int speed = -1, bool reverse = false, float[] trackTypeInfo = null)
        {
            VDA5050Segment src, dst, track;
            lock (_car.RouteCache)
            {
                //Console.WriteLine("_car.RouteCache.Count: " + _car.RouteCache.Count);
                if (_car.RouteCache.Count == 0)
                {
                    src = new(new node()
                    {
                        nodeId = VDA5050Helper.GetVDA5050Id(srcId),
                        nodePosition = new() { x = SimpleLib.GetSite(srcId).x, y = SimpleLib.GetSite(srcId).y }
                    }, VDA5050Segment.SegState.Waiting);
                    _car.RouteCache.Add(src);
                }
                else src = _car.RouteCache.Last();

                if (trackTypeInfo.Length != 0 && trackTypeInfo[0] == 3)
                {
                    var controlPointNum = (int)trackTypeInfo[1];
                    var controlPoints = new controlPoint[controlPointNum];
                    for (int pt = 2; pt < controlPointNum * 2 + 2; pt += 2)
                    {
                        controlPoints[pt/2-1] = new controlPoint(trackTypeInfo[pt], trackTypeInfo[pt + 1], trackTypeInfo[1 + controlPointNum * 2 + pt / 2]);
                    }
                    var knotVector = new float[controlPointNum*2];
                    for (int i = 0; i < controlPointNum; i++)
                    {
                        knotVector[i] = 0f;
                        knotVector[i + controlPointNum] = 1f;
                    }
                    var trajectory = new trajectory(){controlPoints = controlPoints,degree = controlPointNum,knotVector = knotVector };
                    track = new(new edge()
                    {
                        edgeId = VDA5050Helper.GetVDA5050Id(trackId),
                        startNodeId = VDA5050Helper.GetVDA5050Id(srcId),
                        endNodeId = VDA5050Helper.GetVDA5050Id(dstId),
                        trajectory = trajectory,
                        orientation = reverse ? Math.PI : 0.0,
                        action = [new action() { actionDescription = $"{VDA5050Helper.GetVDA5050Id(trackId)}", actionId = Guid.NewGuid().ToString() }]
                    }, VDA5050Segment.SegState.Waiting);
                }
                else
                {
                    track = new(new edge()
                    {
                        edgeId = VDA5050Helper.GetVDA5050Id(trackId),
                        startNodeId = VDA5050Helper.GetVDA5050Id(srcId),
                        endNodeId = VDA5050Helper.GetVDA5050Id(dstId),
                        trackTypeInfo = trackTypeInfo,
                        orientation = reverse ? Math.PI : 0.0,
                        action = [new action() { actionDescription = $"{VDA5050Helper.GetVDA5050Id(trackId)}", actionId = Guid.NewGuid().ToString() }]
                    }, VDA5050Segment.SegState.Waiting);
                }

                dst = new(new node()
                {
                    nodeId = VDA5050Helper.GetVDA5050Id(dstId),
                    nodePosition = new() { x = SimpleLib.GetSite(dstId).x, y = SimpleLib.GetSite(dstId).y },
                    actions = [new action() { actionDescription = $"{VDA5050Helper.GetVDA5050Id(dstId)}", actionId = Guid.NewGuid().ToString() }]
                }, VDA5050Segment.SegState.Waiting);

                _car.RouteCache.Add(track);
                _car.RouteCache.Add(dst);
            }

            Queue(async () =>
            {
                while (!TryLock(dstId))
                    await Task.Delay(10);
                lock (_car.RouteCache)
                {
                    if (src.Item.sequenceId == 0)
                    {
                        src.State = VDA5050Segment.SegState.BaseSending;
                    }
                    track.State = VDA5050Segment.SegState.BaseSending;
                    dst.State = VDA5050Segment.SegState.BaseSending;
                    src.Item.released = true;
                    track.Item.released = true;
                    dst.Item.released = true;
                }
            }, async () =>
            {
                await src.FinishToken.Task;
                Leave(srcId);
            });
        }
        
        public void ChangeDI41Signal()
        {
            hc.GetStringAsync($"http://{_car.address}:8008/setValue?FieldName=DI41&Value=True");
            Diagnosis.Log("将DI41置为True", "VDA5050", true);
            Thread.Sleep(1000);
            hc.GetStringAsync($"http://{_car.address}:8008/setValue?FieldName=DI41&Value=False");
            Diagnosis.Log("1秒后将DI41置为False", "VDA5050", true);
        }

        public void Sleep(int SleepTime)
        {
            Diagnosis.Log("小车开始Sleep" + SleepTime + "ms", "VDA5050", true);
            Thread.Sleep(SleepTime);
            Diagnosis.Log("小车结束Sleep", "VDA5050", true);
        }

        public void WaitAO3Signal()
        {
            while(_car.AO3 == 0)
            {
                Thread.Sleep(200);
            }
            Diagnosis.Log($"AO3: " + _car.AO3, "VDA5050", true);
            Diagnosis.Log("获取到AO3信号为1，继续下个任务", "VDA5050", true);
        }

        private VDA5050Car _car;
    }
}