using System;
using System.Net;
using Autolabor.Robobase;
using Autolabor.Robobase.Component.Algorithm.LidarDetection;
using Autolabor.Robobase.Component.Algorithm.LidarDetection.DataType;
using Autolabor.Robobase.Component.Business.Benz;
using Autolabor.Robobase.Component.Driver;
using Autolabor.Robobase.Component.Driver.SerialRelay;
using Autolabor.Robobase.Component.Tools;
using Autolabor.Robobase.Geometry;
using Autolabor.Robobase.Tools.DataRender;

namespace Autolabor.Benz.ObstacleDetection
{
    [Serializable]
    public class Detection : GroupComponent
    {
        // 驱动层组件
        private Ld19DriverNew _lidarDriver;
        private SerialRelayDriver _serialRelay;
        
        // 算法层组件
        private LidarRoiFilter _lidarRoiFilter;
        private ObstacleDetector _obstacleDetector;
        private BenzObstacleControl _benzObstacleController;
        
        // 显示组件
        private DataRender<Pose2D[]> _lidarRender;
        private DataRender<Pose2D[]> _obstacleRender;
        
        // 模拟器配置
        private int _FrontLidarRpcPort = 50120;
        private int _FrontLidarListenPort = 40120;
        private IPAddress _serverIp = IPAddress.Loopback;
        
        // 配置过滤区域
        private LidarRoi[] _lidarRois;

        public Detection()
        {
            // 2. 创建各个功能组件
            // 驱动组件
            _lidarDriver = new Ld19DriverNew();
            _serialRelay = new SerialRelayDriver();
        
            // 算法组件
            _obstacleDetector = new ObstacleDetector();
            _benzObstacleController = new BenzObstacleControl();
            _lidarRoiFilter = new LidarRoiFilter();
            
            // 显示组件
            _lidarRender = DataRenderFactory.Make<Pose2D[]>("Benz","lidar",false,true);
            _obstacleRender = DataRenderFactory.Make<Pose2D[]>("Benz","obstacle",false,true);
            
            // 3. 添加组件到容器
            Add(_lidarDriver);
            Add(_serialRelay);
            Add(_lidarRoiFilter);
            Add(_obstacleDetector);
            Add(_benzObstacleController);
            Add(_lidarRender);
            Add(_obstacleRender);
        
            // 4. 配置组件
            // 激光雷达配置
            _lidarDriver.SetProperty("lidar.port", "/dev/tty.usbserial-2120");
            _lidarDriver.SetProperty("lidar.x", 0.0);
            _lidarDriver.SetProperty("lidar.y", 0.0);
            _lidarDriver.SetProperty("lidar.originName", "frontLidarOrigin");
            _lidarDriver.SetProperty("lidar.rotation", Math.PI / 2.0);
            _lidarDriver.SetProperty("lidar.startRad", -Math.PI);        // 要提取的数据范围
            _lidarDriver.SetProperty("lidar.rangeRad", 2.0 * Math.PI);
            _lidarDriver.SetProperty("lidar.display.interval", 1000);
            _lidarDriver.SetProperty("lidar.intensityLevel", 200.0);
            _lidarDriver.SetProperty("sim.lidar.server.ip", _serverIp);
            _lidarDriver.SetProperty("sim.lidar.rpc.port", _FrontLidarRpcPort);
            _lidarDriver.SetProperty("sim.lidar.listen.port", _FrontLidarListenPort);
            
            // 串口继电器配置
            _serialRelay.SetProperty("serial.relay.port", "/dev/tty.usbserial-AC012VIB");
            
            // 创建感兴趣区
            _lidarRois = new[]
            {
                LidarRoiFactory.Make(0.6, 0.5, 0.1, -0.5),
            };
            // 配置过滤器
            _lidarRoiFilter.SetProperty("roiArray", _lidarRois);
            // 障碍物检测器配置
            _obstacleDetector.SetProperty("MinClusterDistance", 0.05);
            _obstacleDetector.SetProperty("MinClusterSize", 3);
            _obstacleDetector.SetProperty("MaxPrjDistance", 0.05);
            _obstacleDetector.SetProperty("GuessDistance", 1.0);
            // 配置发现和丢失次数
            _benzObstacleController.SetProperty("FoundObstacleThreshold", 3);
            _benzObstacleController.SetProperty("LostObstacleThreshold", 5);
        
            // 5. 连接组件
            // 雷达数据流向避障检测
            _lidarDriver.OutputLocalLidarPose2D.Connect(_lidarRoiFilter.InputLidarPoints);
            _lidarRoiFilter.OutputFilteredPoints.Connect(_obstacleDetector.InputLidarPoints);
            // 雷达诊断信号连接到奔驰障碍物控制器
            _lidarDriver.OutputLidarDiagnostic.Connect(_benzObstacleController.InputDiagnostics);
            // 避障检测结果流向奔驰避障控制器
            _obstacleDetector.OutputObstacles.Connect(_benzObstacleController.InputObstacles);
            // 奔驰避障控制器输出到继电器
            _benzObstacleController.OutputRelayControl.Connect(_serialRelay.InputRelayControl);
            _benzObstacleController.OutputRelayQuery.Connect(_serialRelay.InputRelayQuery);
            _serialRelay.OutputRelayFeedback.Connect(_benzObstacleController.InputRelayStatus);
            
            // 连接到绘图
            _lidarDriver.OutputLocalLidarPose2D.Connect(_lidarRender.Input);
            _obstacleDetector.OutputObstaclePoses.Connect(_obstacleRender.Input);
        }
    }
}