#include "SemiMotorDm.h"
#include "DmErrno.h"

#ifndef UNIX
# define Boolean MBool
# include "msgQLib.h"
# include "sio32.h"
# include "sio32Server.h"
# include "sio32Client.h"
# include "microDm.h"
# include "ibc_card.h"
# include "filter.h"
# include "gf_board.h"
# include "vxUtils.h"
#endif

#include "microcmd.h"
#include "ibcDm.h"
#include "pduMicro.h"
#include "microDm.h"
#include "ersDcon.h"
#include "lightingDcon.h"
#include "powerIbc.h"
#include "vmeIbc.h"
#include "cameraDcon.h"
#include "sensorDcon.h"
#include "sonarDcon.h"
#include "gf_5vDm.h"
#include "thrusterDM.h"

#define dprintf if (debug) fprintf


DmMotorFaultObject::DmMotorFaultObject(const char *name)
  : DmObject(name, 1)
{

    Errno status = 
      dm_create((char *)name, 1, &_handle,

		DM_NAT16, 1, DM_NAT32, 1, DM_NAT16, 1, 
		DM_NAT16, 1, DM_INT16, 1, 
		DM_INT16, 1,  // Pad

		DM_NAT16, 1, DM_NAT32, 1, DM_NAT16, 1, 
		DM_NAT16, 1, DM_INT16, 1, 
		DM_INT16, 1,  // Pad

		DM_NAT16, 1, DM_NAT32, 1, DM_NAT16, 1, 
		DM_NAT16, 1, DM_INT16, 1,
		DM_INT16, 1,  // Pad

		DM_NAT16, 1, DM_NAT32, 1, DM_NAT16, 1, 
		DM_NAT16, 1, DM_INT16, 1,
		DM_INT16, 1,  // Pad

		DM_ENDT);
 

  char buf[100];
  char errorBuf[errorMsgSize];
    
  switch (status)
  {
    case SUCCESS:
    case EDM_NAME_EXISTS:
    break;
    
    default:
    sprintf(buf, "DmMotorFaultObject::DmMotorFaultObject(%s)", name);
    sprintDmError(status, buf, errorBuf);
    setError(errorBuf);
    return;
  }

  // Get structure info
  dm_item_status(_handle, &_status);
  MBool debug = FALSE;

  dprintf(stderr, "DmMotorFaultObject struct info:\n");
  char *type;
  // Check structure items
  for (int i = 0; i < _status.is_num_compounds; i++) {
    switch (_status.is_compound[i].elt_type) {
    case DM_EMPTY:
      type = "empty";
      break;

    case DM_CHAR:
      type = "char";
      break;

    case DM_UCHAR:
      type = "uchar";
      break;

    case DM_INT16:
      type = "int16";
      break;

    case DM_NAT16:
      type = "nat16";
      break;

    case DM_INT32:
      type = "int32";
      break;

    case DM_NAT32:
      type = "nat32";
      break;

    case DM_MBOOL:
      type = "bool";
      break;

    case DM_FLT32:
      type = "flt32";
      break;

    case DM_FLT64:
      type = "flt64";
      break;

    case DM_PTR:
      type = "ptr";
      break;

    case DM_ENUM:
      type = "enum";
      break;

    case DM_ARRAY:
      type = "array";
      break;

    case DM_STRUCT:
      type = "struct";
      break;

    default:
      type = "Unknown";
    }

    dprintf(stderr, "Element %d: %s [%d]\n", 
	    i, type, _status.is_compound[i].elt_num);
  }
}


DmMotorFaultObject::~DmMotorFaultObject()
{
  dm_item_status_free(&_status);
}



Errno DmMotorFaultObject::read(faultQueueEntry *entries, DM_Time *t)
{
  MBool debug = TRUE;

  char *debugPtr;
  Errno err;
  err = dm_read(_handle, entries, 
		sizeof(faultQueueEntry) * FAULT_QUEUE_LENGTH, 
		t);

  debugPtr = (char *)entries;

  return err;
}


Errno DmMotorFaultObject::write(faultQueueEntry *entries, DM_Time *t)
{
  return dm_write(_handle, entries, 
		  sizeof(faultQueueEntry) * FAULT_QUEUE_LENGTH, 
		  t);
}



SemiMotorDm::SemiMotorDm(const char *name, const char *dmPrefix) :
  ComponentDmObjects(name, dmPrefix)
{
  char dmItemName[MaxDmNameLen];
  char errorBuf[errorMsgSize];

  strcpy(dmItemName, _dmPrefix);
  char *namePtr = dmItemName + strlen(dmItemName);
  
  strcpy(namePtr, MOTOR_VELOCITY_DM);
  velocity = new DmInt16Object(dmItemName);
  if (velocity->error())
  {
    velocity->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  strcpy(namePtr, MOTOR_CTRL_MODE_DM);
  cntrlMode = new DmEnumObject(dmItemName);
  if (cntrlMode->error())
  {
    cntrlMode->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  strcpy(namePtr, MOTOR_CMD_THRUST_DM);
  cmdThrust = new DmInt16Object(dmItemName);
  if (cmdThrust->error())
  {
    cmdThrust->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  strcpy(namePtr, MOTOR_CURRENT_GAIN_DM);
  currentGain = new DmInt16Object(dmItemName);
  if (currentGain->error())
  {
    currentGain->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  strcpy(namePtr, MOTOR_VELOCITY_GAINS_DM);
  velocityGains = new DmInt16Object(dmItemName, VELOCITY_GAIN_SIZE);
  if (velocityGains->error())
  {
    velocityGains->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  strcpy(namePtr, MOTOR_MODEL_PARAM_K4_DM);
  modelParamK4 = new DmInt16Object(dmItemName);
  if (modelParamK4->error())
  {
    modelParamK4->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  strcpy(namePtr, INPUT_VOLTAGE_DM);
  inputVoltage = new DmNat16Object(dmItemName);
  if (inputVoltage->error()) {
    inputVoltage->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  strcpy(namePtr, OUTPUT_POWER_DM);
  outputPower = new DmNat16Object(dmItemName);
  if (outputPower->error()) {
    outputPower->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  strcpy(namePtr, ELAPSED_HOURS_DM);
  elapsedHours = new DmNat16Object(dmItemName);
  if (elapsedHours->error()) {
    elapsedHours->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  strcpy(namePtr, HEATSINK_TEMPERATURE_DM);
  heatSinkTemp = new DmInt16Object(dmItemName);
  if (heatSinkTemp->error()) {
    heatSinkTemp->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  strcpy(namePtr, HUMIDITY_DM);
  humidity = new DmNat16Object(dmItemName);
  if (humidity->error()) {
    humidity->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  strcpy(namePtr, MOTOR_ENABLE_REQ_DM);
  enable = new DmEnumObject(dmItemName);
  if (enable->error()) {
    enable->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  strcpy(namePtr, MOTOR_ENABLE_STATUS_DM);
  enableStatus = new DmEnumObject(dmItemName);
  if (enableStatus->error()) {
    enableStatus->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  strcpy(namePtr, FAULT_QUEUE_STRUCT_DM);
  faultQueue = new DmMotorFaultObject(dmItemName);
  if (faultQueue->error()) {
    faultQueue->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }  

  strcpy(namePtr, CLEAR_FAULT_QUEUE_DM);
  clearFaultQueue = new DmEmptyObject(dmItemName);
  if (clearFaultQueue->error()) {
    clearFaultQueue->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  strcpy(namePtr, READ_FAULT_QUEUE_DM);
  readFaultQueue = new DmEmptyObject(dmItemName);
  if (readFaultQueue->error()) {
    readFaultQueue->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  strcpy(namePtr, MOTOR_CURRENT_DM);
  current = new DmNat16Object(dmItemName);
  if (current->error()) {
    current->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  strcpy(namePtr, MOTOR_FAULT_CODE_DM);
  faultCode = new DmEnumObject(dmItemName);
  if (faultCode->error()) {
    faultCode->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  strcpy(namePtr, MOTOR_TORQUE_DM);
  torque = new DmInt16Object(dmItemName);
  if (torque->error()) {
    torque->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }  
}


SemiMotorDm::~SemiMotorDm()
{
  delete faultQueue;
}