StreetLight/Assets/StreetLight/Scripts/PersonManager.cs

using Assets.StreetLight.Adapters;
using Assets.StreetLight.Interfaces;
using Assets.StreetLight.Poco;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Globalization;
using System.IO;
using System.Linq;
using UnityEngine;

namespace Assets.StreetLight.Scripts
{
    public class PersonManager : MonoBehaviour
    {
        public ObservableCollection<Person> Persons { get; private set; }
        private PositionCalculator PositionCalculator;

        private IPersonDetector personDetector;

        public event EventHandler DetectionReady;

        void Start()
        {
            try
            {
                var homographyArray = GetHomographyArrayFromFile();
                PositionCalculator = new PositionCalculator(homographyArray);
            }
            catch (Exception ex)
            {
                Debug.LogWarning($"Could not load homography: {ex.Message} Calculating the unity position of a person will not be possible. (This is expected during calibration.)");
            }

            Persons = new ObservableCollection<Person>();
            personDetector = new ZedPersonDetector(FindObjectOfType<ZEDManager>());
            personDetector.PersonsDetected += InvokeDetectionReady;
            personDetector.PersonsDetected += PersonDetector_PersonsDetected;
        }

        private double[,] GetHomographyArrayFromFile()
        {
            var homographyString = File.ReadAllLines(Configuration.Instance.HomographyFilePath);

            var values = homographyString.Select(l => l.Split(' ').Select(s => double.Parse(s, CultureInfo.InvariantCulture)).ToArray()).ToArray();

            double[,] array = new double[3, 3];

            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    array[i, j] = values[i][j];
                }
            }

            return array;
        }

        private void InvokeDetectionReady(object sender, IEnumerable<Person> e)
        {
            DetectionReady?.Invoke(this, EventArgs.Empty);
            personDetector.PersonsDetected -= InvokeDetectionReady;
        }

        private void PersonDetector_PersonsDetected(object sender, IEnumerable<Person> e)
        {
            var detectedIds = new HashSet<int>();
            foreach (var detectedPerson in e)
            {
                var person = Persons.SingleOrDefault(p => p.Id == detectedPerson.Id);
                if (person == null)
                {
                    person = new Person(detectedPerson.Id, PositionCalculator);
                    Persons.Add(person);
                }
                person.WorldPosition = detectedPerson.WorldPosition;
                detectedIds.Add(detectedPerson.Id);
            }

            var personsToBeRemoved = Persons.Where(p => !detectedIds.Contains(p.Id)).ToArray();
            foreach (var person in personsToBeRemoved)
            {
                Persons.Remove(person);
            }
        }

        void Update()
        {
        }
    }
}