#include <stdio.h>
#include <stdlib.h>
#include "TailCone.h"

typedef char Byte;
typedef short Int16;
typedef long Int32;
typedef void Void;

Int16 cmdSpeed = 0;
Void motorStop(Void);
Int16 KmotorInv = 758;

int main(int argc, char **argv)
{
  if (argc != 2) {
    fprintf(stderr, "Usage: %s <speed>\n", argv[0]);
    return 1;
  }

  cmdSpeed = atoi(argv[1]);

  fprintf(stderr, "cmdSpeed = %d; call motorStop()\n", cmdSpeed);

  motorStop();

  fprintf(stderr, "Done.\n");

  return 0;
}


Void motorStop( Void )
{
  Int16 speed;
  Int16 pwmCounts;
  Int16 signFlip = 1;
  Int32 pwm;
  const Int16 mcDeadband  = 2;        /* Motor controller board deadband    */
  const Int32 scale       = 10000;
  const Int16 speedIncr = 50;

  speed = cmdSpeed;
  if( speed < 0 )
  {
    signFlip = -1;
  }
  else
  {
    signFlip = 1;
  }


  while( (signFlip*speed) > 0)
  {
    if (abs(speed) < speedIncr) 
      speed = 0;
    else
      /* subtract 50 RPM from Speed */
      speed = speed - (signFlip*speedIncr);

           /* Set motor Command to Stop */
    pwm  = ( (Int32) KmotorInv * (Int32) speed );
    pwmCounts =  MOTOR_OFFSET -  ( (Int16) ( pwm / scale) );

    if( speed  >=  1 ) pwmCounts = pwmCounts - mcDeadband;
    if( speed  <= -1 ) pwmCounts = pwmCounts + mcDeadband;

    /** Don't allow out-of-range commands.
      */
    if( pwmCounts <= 1 )   pwmCounts = 1;
    if( pwmCounts >= 255 ) pwmCounts = 255;

    /*
    ** Write to the Pontech motor controller board
    */
    fprintf(stderr, "motorStop() - setting speed to %d\n", speed);
    // sv203SetPWM(&propMotor, (Byte) pwmCounts);

    // taskDelay(10);/* Delay 1 second*/
  } 
  cmdSpeed = speed;
}  /* motorStop() */