/*-global variables---------------------------------------------------------------------------------------------------------*/
float skew = 0.0f, step = 0.0f;
/*--------------------------------------------------------------------------------------------------------------------------*/

DWORD WINAPI zozStepMonitor(LPDWORD lpData)
{
  HANDLE hStepperSerial;
  int delay;
  char readbuf[64];
  int cmin, cmax, cpos, ctim;
  int bbak;
  float camp, csku;
  int cstp, cdir;
  const int DIR_UP = 1;
  const int DIR_DOWN = 2;
  const float full_step_amp = 54.0f;
  const float full_skew = 24.0f;
  const float skew_centre = 162.0f;
  const float skew_thresh = 9.0f;
  const int pos_thresh = 3;
  const int reverse_thresh = 128;
  int lastpos = 0;
  float sku = 0.0f, lastsku = 0.0f;
  float decel = 0.0f;
  float skudecel = 0.0f;

  delay = 40; /* 25Hz update rate of communication with stepper */
  cdir = DIR_UP;
  csku = skew_centre;

  hStepperSerial = (HANDLE)zozSerialOpen(commport, baudrate);
  if ((int)hStepperSerial < 0)
  {
    printf("Stepper comms failed, exiting...\n");
    return(-1);
  }

  zozWriteChar(hStepperSerial, 'R');
  zozRead(hStepperSerial, readbuf, sizeof(readbuf));

  while (TRUE)
  {
    WaitForSingleObject(hStepMutex, INFINITE);
    zozWriteChar(hStepperSerial, 'C');
    Sleep(delay/2);
    if (zozCanRead(hStepperSerial))
    {
      zozRead(hStepperSerial, readbuf, sizeof(readbuf));
      readbuf[0] = ' '; /* blank out echoed command */
      sscanf(readbuf, "%x%x%x%x%x", &cmin, &cmax, &cpos, &ctim, &bbak);
    }
    zozWriteChar(hStepperSerial, 'B');
    Sleep(delay/2);
    if (zozCanRead(hStepperSerial))
    {
      zozRead(hStepperSerial, readbuf, sizeof(readbuf));
      readbuf[0] = ' '; /* blank out echoed command */
      sscanf(readbuf, "%x", &bbak);
    }
    cstp = 0;
    if (cmax > cmin)
    {
      camp = (float)(cmax - cmin);
      csku = (float)((cmin + cmax) / 2.0f);
      if ((cdir == DIR_UP) && ((float)cpos >= (0.95f * (float)cmax)) && (cpos > lastpos))
      /* register an up step if we're on the up swing, within 5% of the last max and have just moved up */
      {
        cstp = 1;
        cdir = DIR_DOWN;
      }
      else if ((cdir == DIR_DOWN) && ((float)cpos <= (1.05f * (float)cmin)) && (cpos < lastpos))
      /* register a down step if we're on the down swing, within 5% of the last min and have just moved down */
      {
        cstp = 1;
        cdir = DIR_UP;
      }
    }
    if (fabs(csku - skew_centre) >= skew_thresh)
    /* it's a skew step if we're the skew threshold away from the midpoint */
    {
      sku = csku - skew_centre;
    }
    if (fabs(sku) != fabs(lastsku))
    /* an active full skew step - accelerate by the normalised skew, and don't decelerate */
    {
      skew += sku / full_skew;
      skudecel = 0.0f;
    }
    else if (((cdir == DIR_UP) && ((cpos - lastpos) > pos_thresh)) || ((cdir == DIR_DOWN) && ((lastpos - cpos) > pos_thresh)))
    /* not a full step, but we're still moving - don't decelerate */
    {
      skudecel = 0.0f;
    }
    else if ((skew != 0.0f) && (skudecel == 0.0f))
    /* not moving on stepper but in an active turn, decelerate turn */
    {
      skudecel = (skew * (float)delay) / 100.0f; /* come to a halt in approx. 0.1s */
    }
    else /* not turning */
    {
      if (cstp)
      /* accelerate the walking speed by the normalised step amplitude, and don't decelerate */
      {
        if (bbak < back_thresh)
        /* if we're walking backwards, speed is negative */
        {
          step -= camp / full_step_amp;
        }
        else
        {
          step += camp / full_step_amp;
        }
        decel = 0.0f;
      }
      else if (((cdir == DIR_UP) && ((cpos - lastpos) > pos_thresh)) || ((cdir == DIR_DOWN) && ((lastpos - cpos) > pos_thresh)))
      /* not a full step, but we're still moving - don't decelerate */
      {
        decel = 0.0f;
      }
      else if ((decel == 0.0f) && (step != 0.0f))
      /* not moving on stepper but in an active step, decelerate */
      {
        decel = (step * (float)delay) / 250.0f; /* come to a halt in approx. 0.25s */
      }
    }
    /* apply movement and turn decelerations if applicable */
    if (step > 0.0f)
    {
      step -= (step>decel)?decel:step;
    }
    else if (step < 0.0f)
    {
      step -= (step<decel)?decel:step;
    }
    if (skew > 0.0f)
    {
      skew -= (skew>skudecel)?skudecel:skew;
    }
    else if (skew < 0.0f)
    {
      skew -= (skew<skudecel)?skudecel:skew;
    }
    lastpos = cpos;
    lastsku = sku;
    ReleaseMutex(hStepMutex);
  }
  return(-1);
}