%
% PURPOSE: Implement the Moog Micro controller in matlab
%
function [desiredVelocity, filtAcceleration, torque] = ...
         velocityMode( velocity, desiredThrust )
% Int16 torque;                   /* Torque value calculated              */
% Int16 velocity;                 /* Motor velocity adjusted for divide   */
% Int16 absVelocity;              /* Absolute value of velocity           */
% Int32 desiredVelocity;          /* Desired velocity - calc from thrust  */
% Int32 temp;                     /* Temporary stores intermediate result */
  MAX_TORQUE_CMD         =  127; %/* Maximum motor torque command bits*/
  MAX_MOTOR_POWER        = 3728; %/* Maximum motor power in watts     */
  MAX_MOTOR_RPM          = 1630; %/* Motor RPM expected at max power  */
  MAX_MOTOR_THRUST       = 1245; %/* Maximum Motor Thrust Command     */

  MAX_CMD_THRUST         = 1245; %/* Maxiumum thrust value sent to motor  */

  MAX_MOTOR_TORQUE       =   22; %/* Max motor torque in Newton.metres*/
  VELOCITY_DEADBAND      =   10; %/* Velocity deadband region RPM     */
  MOTOR_TORQUE_DEADBAND  =    4; %/* Motor torque deadband in bits    */
  VELOCITY_UPDATE_PERIOD =   10; %/* 10 millisecond = 100 Hz update   */
  VEL_FILTER_TIME_CONST  =   20; %/* 20 Milliseconds = 0.02 seconds   */
  VEL_PROP_GAIN      = 300;
  VEL_DERIV_GAIN     = 4000;
  VEL_POWER_GAIN     = 57;
  dynamicModel_oneOnK4 = 2400;

  NEWTONS_TO_MOTOR_UNITS =  MAX_TORQUE_CMD / MAX_MOTOR_THRUST;
  ctrlLoopPeriod =   VELOCITY_UPDATE_PERIOD;

  sz = size(velocity);
  N = sz(1);

% if (velocity < 0)              %/* Derive absolute value of velocity    */
%       absVelocity = -velocity;
% else
%       absVelocity = velocity;
  absVelocity = abs(velocity);

  lastVelocity = 0;
  lastVelDeriv = 0;
  lastFiltAcceleration = 0;

  for i = 1:N,
                                   %/* Adjust to prevent divide by zero     */
  if ((velocity(i) > -VELOCITY_DEADBAND) & (velocity(i) < VELOCITY_DEADBAND) )
       velocity(i)    = 0;
       absVelocity(i) = 1;
  end %/* if */

  %/* Calculate filtered motor acceleration*/
  % calcMotorAcceleration(velocity);

  velDeriv = (velocity(i) - lastVelocity) /  ctrlLoopPeriod;

% Debug printouts
  if 0
  disp(' ')
  disp(['i = ' num2str(i)])
  disp(['lastFiltAcc = ' num2str(size(lastFiltAcceleration))] )
  disp(['VEL TIME CONST = ' num2str(size(VEL_FILTER_TIME_CONST))])
  disp(['ctrlLoopPrd = ' num2str(size(ctrlLoopPeriod))])
  disp(['velocity = ' num2str(size(velocity))])
  disp(['lastVelocity = '  num2str(size(lastVelocity))])
  disp(['lastVelDeriv = ' num2str(size(lastVelDeriv)) ])
  end


  filtAcceleration(i,1) = (lastFiltAcceleration * ...
      (2 * VEL_FILTER_TIME_CONST -  ctrlLoopPeriod) + ...
      (velocity(i) - lastVelocity) + ctrlLoopPeriod * lastVelDeriv) / ...
      (VEL_FILTER_TIME_CONST +  ctrlLoopPeriod);

  lastVelocity = velocity(i);
  lastVelDeriv = velDeriv;
  lastFiltAcceleration = filtAcceleration(i);

  end %for

  %/* Calc desired velocity from thrust    */

  desiredVelocity = ( desiredThrust * dynamicModel_oneOnK4) ./ absVelocity;

% if (desiredVelocity > MAX_MOTOR_RPM)
%     desiredVelocity = MAX_MOTOR_RPM;
% else if (desiredVelocity < -MAX_MOTOR_RPM)
%          desiredVelocity = -MAX_MOTOR_RPM;
  desiredVelocity = min(desiredVelocity,  MAX_MOTOR_RPM);
  desiredVelocity = max(desiredVelocity, -MAX_MOTOR_RPM);

  if 1  % debug printouts
  disp(['desiredVelocity = ' num2str(size(desiredVelocity))] )
  disp(['desiredThrust = ' num2str(size(desiredThrust))] )
  disp(['absVelocity = ' num2str(size(absVelocity))] )
  disp(['velocity = ' num2str(size(velocity))] )
  disp(['filtAcceleration = ' num2str(size(filtAcceleration))] )
  end    

  temp = ...
   (( (desiredVelocity -  velocity) * MAX_TORQUE_CMD / VEL_PROP_GAIN) ...
  - ( filtAcceleration * MAX_TORQUE_CMD / VEL_DERIV_GAIN) ...
  + ( desiredThrust * MAX_TORQUE_CMD / VEL_POWER_GAIN) );

  torque = (temp /  MAX_MOTOR_TORQUE);
% 
% The last calc above completes the conversion from N-m to counts. The
% conversion is N-m * MAX_TORQUE_CMD / MAX_MOTOR_TORQUE = counts
%
% torque = motorDeadbandCompensate(torque, desiredThrust);
%                                /* Compensate for thruster deadband         */
%                                /* By adding an offset to non zero commands */
% if (command > 0)
%     torque += MOTOR_TORQUE_DEADBAND;
% else if (command < 0)
%     torque -= MOTOR_TORQUE_DEADBAND;
  torque = torque + sign(torque)*MOTOR_TORQUE_DEADBAND; 

% setMotorTorque(torque);                    /* Send desired torque to motor */
% if (torque > (Int16) MAX_TORQUE_CMD)
%     torque = (Int16) MAX_TORQUE_CMD;
% else if (torque < - (Int16) MAX_TORQUE_CMD)
%     torque = - (Int16) MAX_TORQUE_CMD;
  torque = min( torque,  MAX_TORQUE_CMD );
  torque = max( torque, -MAX_TORQUE_CMD );
%
% Now, convert back to Newton-meters;
  torque = torque * MAX_MOTOR_TORQUE / MAX_TORQUE_CMD;


%                                    /* Convert torque to bi-polar PWM value */
% torque += NVRamData.torqueZeroOffset;
% if (torque > MAX_PWM_VALUE)        /* Keep PWM control value within limits */
%     torque = MAX_PWM_VALUE;
% else if (torque < 0)               /* Values outside deadband are zero     */
%     torque = 0;
%                                    /* Convert torque to a byte wide value  */

return