using AGXUnity;
using AGXUnity.Model;
using AGXUnity.Utils;
using System.Linq;
using UnityEngine;

public class TerrainBrush : MonoBehaviour
{
  public DeformableTerrain TargetTerrain = null;
  [Range(1,20)]
  public float Radius = 10.0f;
  [Range(1,10)]
  public float Amplitude = 10.0f;

  struct BrushRect
  {
    public Vector2Int Center { get; private set; }
    public Vector2Int Max { get; private set; }
    public Vector2Int Min { get; private set; }
    public Vector2Int Size => Max - Min;

    public BrushRect( Vector2Int center, Vector2Int halfSize )
    {
      Center = center;
      Max = center + halfSize;
      Min = center - halfSize;
    }

    public void ClampToTerrain( Vector2Int terrSize )
    {
      Min = new Vector2Int( Mathf.Max( Min.x, 0 ), Mathf.Max( Min.y, 0 ) );
      Max = new Vector2Int( Mathf.Min( Max.x, terrSize.x - 1 ), Mathf.Min( Max.y, terrSize.y - 1 ) );
    }
  }

  private Vector3? MouseTerrainIntersection()
  {
    var mouseray = Camera.main.ScreenPointToRay(Input.mousePosition);

    var pos = mouseray.origin.ToHandedVec3();
    var dir = mouseray.direction.ToHandedVec3();

    agxCollide.Geometry g = new agxCollide.Geometry(new agxCollide.Line(pos,pos + dir * 100));
    var geoms = new agxCollide.GeometryPtrVector(1);
    geoms.Add( g );
    var contacts = new agxCollide.LocalGeometryContactVector();
    var partContacts = new agxCollide.LocalParticleGeometryContactVector();
    Simulation.Instance.Native.getSpace().testGeometryOverlap( geoms, contacts, partContacts );

    if ( contacts.Count == 0 )
      return null;

    var native = TargetTerrain.Native.getGeometry();
    var contact = contacts
      .Where(c => c.geometry(0).getUuid().EqualWith(native.getUuid()) || c.geometry( 1 ).getUuid().EqualWith(native.getUuid()) )
      .FirstOrDefault();

    if ( contact == null )
      return null;

    var point = contact.points()[0];
    return point.getPoint().ToHandedVector3();
  }

  private Vector2Int WorldPosToTerrainIndex( Vector3 worldPos )
  {
    var localPos = worldPos - TargetTerrain.transform.position;

    var x = (int)(localPos.x / TargetTerrain.TerrainData.size.x * (TargetTerrain.TerrainDataResolution - 1));
    var y = (int)(localPos.z / TargetTerrain.TerrainData.size.z * (TargetTerrain.TerrainDataResolution - 1));
    return new Vector2Int( x, y );
  }
  private float BrushShape( BrushRect rect, int x, int y )
  {
    int gx = rect.Min.x + x;
    int gy = rect.Min.y + y;

    int dx = rect.Center.x - gx;
    int dy = rect.Center.y - gy;

    float dist = Mathf.Sqrt( dx * dx + dy * dy );
    if ( dist > Radius )
      return 0.0f;

    return Mathf.Cos( dist / Radius * Mathf.PI / 2 ) * Amplitude;
  }

  // Update is called once per frame
  void Update()
  {
    if ( TargetTerrain == null )
      return;

    if ( Input.GetKeyDown( KeyCode.R ) )
      TargetTerrain.ResetHeights();

    var brushPoint = MouseTerrainIntersection();
    if ( brushPoint == null )
      return;

    var brushIndex = WorldPosToTerrainIndex(brushPoint.Value);

    if ( Input.GetKeyDown( KeyCode.G ) ) {
      var h = TargetTerrain.GetHeight( brushIndex.x, brushIndex.y );

      Debug.Log( $"Terrain at ({brushIndex.x}, {brushIndex.y}) has height {h}m" );
    }

    bool left = Input.GetMouseButton( 0 );
    bool right = Input.GetMouseButton( 1 );
    bool middle = Input.GetMouseButton( 2 );

    if ( left || right || middle ) {
      var rect = new BrushRect(brushIndex,new Vector2Int((int)Radius,(int)Radius));
      rect.ClampToTerrain( new Vector2Int( TargetTerrain.TerrainDataResolution, TargetTerrain.TerrainDataResolution ) );
      var heights = TargetTerrain.GetHeights(rect.Min.x,rect.Min.y,rect.Size.x,rect.Size.y);
      if ( right || left ) {
        for ( int y = 0; y < rect.Size.y; y++ )
          for ( int x = 0; x < rect.Size.x; x++ )
            heights[ y, x ] = heights[ y, x ] + BrushShape( rect, x, y ) * Time.deltaTime * ( left ? 1 : -1 );
      }
      else {
        float tot = 0;
        for ( int y = 0; y < rect.Size.y; y++ )
          for ( int x = 0; x < rect.Size.x; x++ )
            tot += heights[ y, x ];

        float avg = tot / (rect.Size.y * rect.Size.x);

        for ( int y = 0; y < rect.Size.y; y++ )
          for ( int x = 0; x < rect.Size.x; x++ ) {
            var h = heights[ y, x ];
            heights[ y, x ] = h + ( avg - h ) * BrushShape( rect, x, y ) * Time.deltaTime;
          }
      }

      TargetTerrain.SetHeights( rect.Min.x, rect.Min.y, heights );
    }
  }
}
