/*

   Turns are detected in a manner similar to speed jumps.
   If the speed at both ends of the reference array exceeds
   a threshold, and if the direction differs by more than a
   threshold, then a turn is detected.

   CALLED BY:  TIMSLIP

*/
#include <math.h> /* sqrt(), atan2() */
#include "cal.h"  /* VELOCITY_TYPE, sqr() */

#if PROTOTYPE_ALLOWED
int is_turn(VELOCITY_TYPE refs[], int n_refs, double turn_speed, double turn_thresh)
#else
int is_turn(refs, n_refs, turn_speed, turn_thresh)
VELOCITY_TYPE refs[];
int n_refs;
double turn_speed,  /* m/s, minimum through turn */
       turn_thresh; /* degrees, plus or minus for turn */
#endif
{
   static int left, right;
   double cross, dot, turn;
   double speed0, speed1;   /* first, last speeds in ref array */

   speed0 = sqrt(sqr(refs[0].u) + sqr(refs[0].v));
   speed1 = sqrt(sqr(refs[n_refs-1].u) + sqr(refs[n_refs-1].v));

   if (speed0 < turn_speed || speed1 < turn_speed)
   {
      left = 0;
      right = 0;
      return(0);
   }

   cross =   refs[0].u * refs[n_refs-1].v
           - refs[0].v * refs[n_refs-1].u;
   dot   =   refs[0].u * refs[n_refs-1].u
           + refs[0].v * refs[n_refs-1].v;
   if (dot == 0.0) dot = 1e-6;
   turn = atan2(cross, dot) * 180 / M_PI;

   if ( turn > turn_thresh )
      left++;
   else
      left = 0;

   if ( turn < -turn_thresh )
      right++;
   else
      right = 0;

   if (left == n_refs/2)        return(TURN_LEFT);     /* speed increasing */
   else if (right == n_refs/2) return(TURN_RIGHT);    /* speed decreasing */
   else                       return(0);     /* no centered speed jump */
}                       /* is_turn() */