#include <cctk.h>
#include <cctk_Arguments.h>
#include <cctk_Parameters.h>
#include <cctk_Functions.h>

void NSTracker(CCTK_ARGUMENTS)
{
  DECLARE_CCTK_ARGUMENTS
  DECLARE_CCTK_PARAMETERS

  const int tracked_location_idx = CCTK_FirstVarIndex(NSTracker_tracked_location);
  const CCTK_REAL *tracked_location_ptr = CCTK_VarDataPtrI(cctkGH, 0, tracked_location_idx);
  if (tracked_location_idx < 0  || tracked_location_ptr == NULL)
  {
    CCTK_VWarn(CCTK_WARN_ABORT, __LINE__, __FILE__, CCTK_THORNSTRING,
               "Could not obtain data pointer for group '%s' index '%d'.",
               NSTracker_tracked_location, tracked_location_idx);
  }

  // Only update, if there was a surface index set
  const int sn = sf_IdFromName(NSTracker_SF_Index, NSTracker_SF_Name);
  if ( (sn >= 0) && (*Hydro_Analysis_rho_max > 0.0) )
  {
    // Stop after some time, if requested
    if ((NSTracker_stop_time < 0) ||
      (cctk_time < NSTracker_stop_time))
    {
      CCTK_REAL dist2 = 0.0;
      CCTK_REAL loc[3];
      for (int i=0; i<3; i++)
        loc[i] = tracked_location_ptr[i];
      if (sf_valid[sn])
      {
        dist2 = (sf_centroid_x[sn]-loc[0])*
                (sf_centroid_x[sn]-loc[0])+
                (sf_centroid_y[sn]-loc[1])*
                (sf_centroid_y[sn]-loc[1])+
                (sf_centroid_z[sn]-loc[2])*
                (sf_centroid_z[sn]-loc[2]);
        // Try to handle two equal maxima
        const int sno = sf_IdFromName(NSTracker_SF_Index_Opposite,
                                      NSTracker_SF_Name_Opposite);
        if (sno >= 0)
        {
          // Choose the closest (one of the two) maxima to the old location
          CCTK_REAL dist2_2 =
                  (sf_centroid_x[sn]+loc[0])*
                  (sf_centroid_x[sn]+loc[0])+
                  (sf_centroid_y[sn]+loc[1])*
                  (sf_centroid_y[sn]+loc[1])+
                  (sf_centroid_z[sn]-loc[2])*
                  (sf_centroid_z[sn]-loc[2]);
          if (dist2_2 < dist2)
          {
            loc[0] = -loc[0];
            loc[1] = -loc[1];
            dist2 = dist2_2;
          }
        }
      }
      // Only set if new location is within NSTracker_max_distance
      // if this was set
      if ((NSTracker_max_distance < 0) ||
          (dist2 <= NSTracker_max_distance*NSTracker_max_distance))
      {
        sf_valid     [sn] = 1;
        sf_active    [sn] = 1;
        sf_centroid_x[sn] = loc[0];
        sf_centroid_y[sn] = loc[1];
        sf_centroid_z[sn] = loc[2];
        const int sno = sf_IdFromName(NSTracker_SF_Index_Opposite,
                                      NSTracker_SF_Name_Opposite);
        if (sno >= 0)
        {
          sf_valid     [sno] = 1;
          sf_active    [sno] = 1;
          sf_centroid_x[sno] = -loc[0];
          sf_centroid_y[sno] = -loc[1];
          sf_centroid_z[sno] = loc[2];
        }
        if (NSTracker_verbose)
          CCTK_VInfo(CCTK_THORNSTRING, "Found star at (%g,%g,%g)",
            sf_centroid_x[sn],
            sf_centroid_y[sn],
            sf_centroid_z[sn]);
      }
      else
        if (NSTracker_verbose)
          CCTK_VInfo(CCTK_THORNSTRING, "New star location not set (%g,%g,%g) "
                                       "because too far from old (%g,%g,%g).",
            tracked_location_ptr[0],
            tracked_location_ptr[1],
            tracked_location_ptr[2],
            sf_centroid_x[sn],
            sf_centroid_y[sn],
            sf_centroid_z[sn]);
    }
    else
      if (NSTracker_verbose)
        CCTK_VInfo(CCTK_THORNSTRING, "New star location not set anymore.");
  }
  else
    if (NSTracker_verbose)
      CCTK_VInfo(CCTK_THORNSTRING, "New star location not set because no got info.");
}