using UnityEngine;
using System.Collections.Generic;

public class CollisionDetector : MonoBehaviour
{
    public ROSCollissionDetectionPublisher publisher;

    [Header("Gizmo Settings")]
    public float gizmoRadius = 0.02f;
    public Color gizmoColor = Color.red;
    private Vector3 collision_point;
    private Transform collision_obj;

    private bool selfCollisionStatus = false;

    private List<Vector3> collisionPoints = new List<Vector3>();

    private void OnCollisionEnter(Collision collision)
    {
        // Filter by the tag of the object you hit
        if (collision.gameObject.CompareTag("obstacles"))
        {
            collision_point = collision.contacts[^1].point; //last elem
            //Debug.Log($"Collision {transform.name} with {collision.gameObject.name} {transform.position} occurred at point: {collision_point}");
            selfCollisionStatus = transform.name.Contains("wheel") ? collision_point.y >= transform.position.y : true;
            if (selfCollisionStatus)
            {
                collisionPoints.Add(collision_point);
                //Debug.Log($"{transform.name} collide with {collision.gameObject.name} occurred at point: {collision_point}");
            }
            publisher.result.data = publisher.result.data ? publisher.result.data : selfCollisionStatus;
            //publisher.ros.Publish(publisher.topicName, publisher.result);
        }
    }

    void OnCollisionStay(Collision collision)
    {
        if (collision.gameObject.CompareTag("obstacles"))
        {
            collision_point = collision.contacts[^1].point; //last elem
            //Debug.Log($"Collision {transform.name} with {collision.gameObject.name} {transform.position} occurred at point: {collision_point}");
            selfCollisionStatus = transform.name.Contains("wheel") ? collision_point.y >= transform.position.y : true;
            publisher.result.data = publisher.result.data ? publisher.result.data : selfCollisionStatus;
            //publisher.ros.Publish(publisher.topicName, publisher.result);
        }
    }

    void OnCollisionExit(Collision collisionInfo)
    {
        publisher.result.data = false;
    }

    private void OnDrawGizmos()
    {
        Gizmos.color = gizmoColor;
        foreach (Vector3 point in collisionPoints)
            Gizmos.DrawSphere(point, gizmoRadius);
    }
}