using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class DriveBlock : MonoBehaviour
{
    [SerializeField]
    float m_accelRate = 25f;
    [SerializeField]
    float m_thickness = 0.5f;
    [SerializeField]
    float m_antiGravityRate = 0.1f;

    HashSet<Rigidbody> m_managedBodies = new HashSet<Rigidbody>();


    Vector3 m_offset;
    Vector3 m_drive;

    Vector3 m_antiGravity;

    public void Init()
    {
        m_offset = transform.up * m_thickness;
        m_drive = transform.forward;
        m_antiGravity = Vector3.Project(Physics.gravity, transform.forward) * m_antiGravityRate;
    }
    public void Drive(float speed)
    {
        var drive = (m_drive + m_antiGravity) * speed;

        foreach (var body in m_managedBodies)
        {

            var currentDrive = Vector3.ProjectOnPlane(body.velocity, transform.up);

            if (body.position.y > transform.position.y)
            {
                var accel = m_accelRate * (drive - currentDrive);
                var position = transform.position + Vector3.ProjectOnPlane(body.position - transform.position, transform.up) + m_offset;
                body.AddForceAtPosition(accel, position, ForceMode.Acceleration);
            }
            else
            {
                var accel = -m_accelRate * (drive + currentDrive);
                var position = transform.position + Vector3.ProjectOnPlane(body.position - transform.position, transform.up) - m_offset;
                body.AddForceAtPosition(accel, position, ForceMode.Acceleration);
            }
        }
    }
    private void OnCollisionStay(Collision collision)
    {
        var body = collision.body as Rigidbody;
        if (!body) return;
        if (!m_managedBodies.Contains(body))
        {
            m_managedBodies.Add(body);
        }
    }
    private void OnCollisionExit(Collision collision)
    {
        var body = collision.body as Rigidbody;
        if (body)
        {
            m_managedBodies.Remove(body);
        }
    }
}
