using UnityEngine;
using Unity.Robotics.Core;
using RosMessageTypes.Geometry;
using Unity.Robotics.ROSTCPConnector;
using System.Collections.Generic;

using RosMessageTypes.GenLocationsOnMesh;

public class CampfireRenderer : MonoBehaviour
{
    public GameObject objectCollector, objectToSpawn;
    public string topic_name = "/campfire_status";

    protected string _namespace;
    public bool useNamespace = true;

    ROSConnection ros;

    private Dictionary<int, GameObject> campfire;
    private GameObject newSpawnedObject;

    private void Awake()
    {
        if (useNamespace)
        {
            _namespace = "/" + gameObject.name;
            topic_name = topic_name[0] == '/' ? _namespace + topic_name : _namespace + "/" + topic_name;
        }
    }

    // Start is called once before the first execution of Update after the MonoBehaviour is created
    void Start()
    {
        ros = ROSConnection.GetOrCreateInstance();
        ros.Subscribe<LocationsOnMeshMsg>(topic_name, CampfireHandler);

        campfire = new Dictionary<int, GameObject>();
    }

    void CampfireHandler(LocationsOnMeshMsg capmfireInfo)
    {
        for (int id = 0; id < capmfireInfo.location_id.Length; id++)
        {
            Vector3 pose = new Vector3(
                (float)capmfireInfo.x[id],
                (float)capmfireInfo.y[id],
                (float)capmfireInfo.z[id]);
            if(campfire.TryGetValue(id, out GameObject go))
            {
                go.transform.position = pose;
                go.SetActive(true);
            } else
            {
                SpawnCampfire(id, pose);
            }
        }

        for (int id = capmfireInfo.location_id.Length; id < campfire.Count; id++)
        {
            campfire[id].SetActive(false);
        }
    }

    void SpawnCampfire(int id, Vector3 pos)
    {
        newSpawnedObject = Instantiate(objectToSpawn, pos, transform.rotation);
        newSpawnedObject.name = "Campfire_" + id;
        newSpawnedObject.transform.SetParent(objectCollector.transform);
        campfire.Add(id, newSpawnedObject);
    }
}