#include "StateProxy.hh"
#include "StateProxyStructure.hh"

#include "Hardware.hh"
#include "EncoderReader.hh"
#include "PositionControl.hh"
#include "RHexCom.hh"
#include "LogServer.hh"
#include "DBModule.hh"
#include "BSOCMonModule.hh"
#include "FizGyroModule.hh"
#include "CompassModule.hh"
#include "GpsModule.hh"
#include "AvgPower.hh"
#include "HeatSinkModule.hh"
#include "hardware/DCMotorHW.hh"
#include "hardware/AccelHW.hh"
#include "hardware/PowerHW.hh"

class StateProxy::EncoderA : public LogAccessor {
public:
  EncoderA(EncoderReader* (&e)[6]) { _encreaders = e; };
  ~EncoderA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++)
      *(f++) = _encreaders[i]->getPosition();
  }
private:
  EncoderReader **_encreaders;
};

class StateProxy::EncoderVelA : public LogAccessor {
public:
  EncoderVelA(EncoderReader* (&e)[6]) { _encreaders = e; };
  ~EncoderVelA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++)
      *(f++) = _encreaders[i]->getSpeed();
  }
private:
  EncoderReader **_encreaders;
};
  
class StateProxy::PoscontrolTarPosA : public LogAccessor {
public:
  PoscontrolTarPosA( PositionControl * (&pos)[6]) { _poscontrol = pos; };
  ~PoscontrolTarPosA() {};
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    MotorTarget_t tar;
    for (int i=0;i<6;i++)
    {
      _poscontrol[i]->getTarget( &tar );
      *(f++) = tar.pos;
    }
  }
private:
  PositionControl **_poscontrol;
};

class StateProxy::PoscontrolTarVelA : public LogAccessor {
public:
  PoscontrolTarVelA( PositionControl * (&pos)[6]) { _poscontrol = pos; };
  ~PoscontrolTarVelA() {};
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    MotorTarget_t tar;
    for (int i=0;i<6;i++)
    {
      _poscontrol[i]->getTarget( &tar );
      *(f++) = tar.vel;
    }
  }
private:
  PositionControl **_poscontrol;
};

class StateProxy::PoscontrolPosErrA : public LogAccessor {
public:
  PoscontrolPosErrA( PositionControl * (&pos)[6]) { _poscontrol = pos; };
  ~PoscontrolPosErrA() {};
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++)
      *(f++) = _poscontrol[i]->getPositionErr( );
  }
private:
  PositionControl **_poscontrol;
};

class StateProxy::PoscontrolVelErrA : public LogAccessor {
public:
  PoscontrolVelErrA( PositionControl * (&pos)[6]) { _poscontrol = pos; };
  ~PoscontrolVelErrA() {};
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++)
      *(f++) = _poscontrol[i]->getSpeedErr( );
  }
private:
  PositionControl **_poscontrol;
};

class StateProxy::PoscontrolTorqueOffsetA : public LogAccessor {
public:
  PoscontrolTorqueOffsetA( PositionControl * (&pos)[6]) { _poscontrol = pos; };
  ~PoscontrolTorqueOffsetA() {};
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++)
      *(f++) = _poscontrol[i]->getTorqueOffset( );
  }
private:
  PositionControl **_poscontrol;
};


class StateProxy::TemperatureA : public LogAccessor {
public:
  TemperatureA(DCMotorHW *dcmotors) { _dcmotors = dcmotors; }
  ~TemperatureA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++)
      *(f++) = _dcmotors->getTemperature(i);
  }
private:
  DCMotorHW* _dcmotors;
};
class StateProxy::AccelerationA : public LogAccessor {
public:
  AccelerationA(AccelHW* accels) { _accels = accels; };
  ~AccelerationA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    if (_accels) {
      *(f++) = _accels->read(AccelHW::AXIS_X);
      *(f++) = _accels->read(AccelHW::AXIS_Y);
      *(f++) = _accels->read(AccelHW::AXIS_Z);
    } else {
      memset(f, 0, 3*sizeof(float));
    }
  }
private:
  AccelHW* _accels;
};
class StateProxy::PowerA : public LogAccessor {
public:
  PowerA(PowerHW* power) { _power = power; };
  ~PowerA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    *(f++) = _power->voltage();
    *f = _power->current();
  }
private:
  PowerHW* _power;
};
class StateProxy::VoltageA : public LogAccessor {
public:
  VoltageA(PowerHW* power) { _power = power; };
  ~VoltageA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    *f = _power->voltage();
  }
private:
  PowerHW* _power;
};
class StateProxy::CurrentA : public LogAccessor {
public:
  CurrentA(PowerHW *power) { _power = power; };
  ~CurrentA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    *f = _power->current();
  }
private:
  PowerHW *_power;
};

// Motor Hardwarw related variables
class StateProxy::MotorVoltageA : public LogAccessor {
public:
  MotorVoltageA(DCMotorHW* dcmotors) { _dcmotors = dcmotors; };
  ~MotorVoltageA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++)
      *(f++) = _dcmotors->getVoltage(i);
  }
private:
  DCMotorHW* _dcmotors;
};
class StateProxy::MotorCurrentA : public LogAccessor {
public:
  MotorCurrentA(DCMotorHW *dcmotors) { _dcmotors = dcmotors; };
  ~MotorCurrentA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++)
      *(f++) = _dcmotors->getCurrent(i);
  }
private:
  DCMotorHW *_dcmotors;
};

class StateProxy::MotorBackEMFA : public LogAccessor {
public:
  MotorBackEMFA(DCMotorHW *dcmotors) { _dcmotors = dcmotors; };
  ~MotorBackEMFA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++)
      *(f++) = _dcmotors->getBackEMF(i);
  }
private:
  DCMotorHW *_dcmotors;
};

class StateProxy::MotorCommandA : public LogAccessor {
public:
  MotorCommandA(DCMotorHW *dcmotors) { _dcmotors = dcmotors; };
  ~MotorCommandA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++)
      *(f++) = _dcmotors->getCommand(i);
  }
private:
  DCMotorHW *_dcmotors;
};

class StateProxy::MotorAnalogA : public LogAccessor {
public:
  MotorAnalogA(DCMotorHW *dcmotors) { _dcmotors = dcmotors; };
  ~MotorAnalogA() { };
  void getVar(int size, unsigned char *p) {
    float *f = (float*)p;
    for (int i=0;i<6;i++) {
#ifdef _LINUX_
      *(f++) = 0;
#endif
#ifdef _QNX_
      *(f++) = 0;
#endif
    }
  }
private:
  DCMotorHW *_dcmotors;
};

class StateProxy::BatteryDODA : public LogAccessor {
public:
  BatteryDODA(  BSOCMonModule* batt_mon ) { _b_mon = batt_mon; };
  ~BatteryDODA() {};
  void getVar( int size, unsigned char*p) 
    {
      float *f = (float*)p;
      *f = _b_mon->getPercentRemaining();
    }
private:
  BSOCMonModule* _b_mon;
};

StateProxy::StateProxy( void ) : Module( STATEPROXY_NAME, 0, MULTI_USER ) {

  memset( &_rhex_state, 0, sizeof( _rhex_state ) );
  memset( _encreaders, 0, sizeof( _encreaders ) );
  memset( _poscontrol, 0, sizeof( _poscontrol ) );
  _dbmodule = NULL;
}

StateProxy::~StateProxy( ) {

}

void StateProxy::init( void ) {
  
  _gyro_calib = true;

  // Find the modules that we will use 
  for ( int i = 0; i < 6; i++) {
    if ((_encreaders[i] =
         (EncoderReader*) MMFindModule(ENCODERREADER_NAME, i)) == NULL) 
      MMFatalError ("StateProxy::init", "Can not find an encoder reader!" );

    if ((_poscontrol[i] =
         (PositionControl *) MMFindModule(POSITIONCONTROL_NAME, i)) == NULL)
      MMFatalError ("StateProxy::init", "Can not find an encoder reader!");
  }
  
  if ((_batt_mon = (BSOCMonModule*) MMFindModule(BSOC_NAME, 0)) == NULL)
    MMFatalError( "StateProxy::init", "Can not find battery monitor!");
  if ((_ap = (APModule*) MMFindModule(APMODULE_NAME, 0)) == NULL)
    MMFatalError( "StateProxy::init", "Can not find average power!");
  if ((_hs_mon = (HeatSinkModule *) MMFindModule(HSMOD_NAME, 0)) == NULL)
    MMFatalError( "StateProxy::init","Can not find HeatSink Monitor");
  if (( _gyro_mod = (FizGyroModule*) MMFindModule(FIZGYRO_NAME, 0)) == NULL) 
    MMWarning( "StateProxy::init", "Can not find the Gyro Module!" ); 
  if (( _compass_mod 
        = (CompassModule*) MMFindModule(COMPASS_IF_NAME, 0)) == NULL) 
    MMWarning ("StateProxy::init", "Can not find the Compass Module!" ); 
  if (( _gps_mod = (GpsModule*) MMFindModule(GPS_IF_NAME, 0)) == NULL) 
    MMWarning ("StateProxy::init", "Can not find the GPS Module!" ); 

  if ((_dbmodule = (DBModule*) MMFindModule(DBMODULE_NAME, 0)) == NULL) 
    MMFatalError( "StateProxy::init", "Can not find the database module!" );
  if ((_logger =  (LogServer*) MMFindModule(LOGSERVER_NAME, 0)) == NULL)
    MMWarning( "StateProxy::init", "Can not find the logging module!" );

  DBManager* mgr = _dbmodule->getManager();
  if ( !mgr ) 
    MMFatalError( "StateProxy::init", "Invalid database manager!" );

  _state_entry = mgr->ENTER( STATE_PROXY_NAME, StateProxyStructure_t );
  if ( !_state_entry )
    MMFatalError( "StateProxy::init", "Can not create state proxy entry!" );
}

void StateProxy::uninit ( void ) {
}

void StateProxy::activate( void ) {

  MMGrabModule( _ap, this );
  MMGrabModule( _hs_mon, this );
  MMGrabModule( _gyro_mod, this );
  MMGrabModule( _batt_mon, this );

  for ( int i = 0; i < 6; i++) 
    MMGrabModule( _encreaders[i], this);

  MMGrabModule(_dbmodule, this);

  if ( _logger )
    MMGrabModule( _logger, this );

  if ( ( _motors = DCMotorHW::instance() ) == NULL )
    MMFatalError( "StateProxy::init",
                  "DC Motor hardware component is not supported!" );
  
  if ( ( _accels = AccelHW::instance() ) == NULL )
    MMFatalError( "StateProxy::init",
                  "Accelerometer hardware component is not supported!" );
  
  if ( ( _power = PowerHW::instance() ) == NULL )
    MMFatalError( "StateProxy::init",
                  "Power hardware component is not supported!" );

  if (_logger) {

    _log_id[0] =
      _logger->registerVar( LOG_FLOAT, 6, "Leg", "position",
                            new EncoderA(_encreaders));
    _log_id[1] =
      _logger->registerVar(LOG_FLOAT, 6, "Leg", "velocity",
                           new EncoderVelA(_encreaders));

    _log_id[2] =
      _logger->registerVar( LOG_FLOAT, 1, "Battery", "voltage",
                            new VoltageA(_power));
    _log_id[3] =
      _logger->registerVar( LOG_FLOAT, 1, "Battery", "current",
                            new CurrentA(_power));
    _log_id[4] =
      _logger->registerVar( LOG_FLOAT, 2, "Battery", "power",
                            new PowerA(_power));
    _log_id[5] =
      _logger->registerVar( LOG_FLOAT, 1, "Battery", "DOD",
                            new BatteryDODA(_batt_mon));


    _log_id[6] =
      _logger->registerVar( LOG_FLOAT, 6, "Leg", "motor_voltage",
                            new MotorVoltageA(_motors));
    _log_id[7] =
      _logger->registerVar( LOG_FLOAT, 6, "Leg", "motor_current",
                            new MotorCurrentA(_motors));
    _log_id[8] =
      _logger->registerVar( LOG_FLOAT, 6, "Leg", "motor_backemf",
                            new MotorBackEMFA(_motors));
    _log_id[9] =
      _logger->registerVar( LOG_FLOAT, 6, "Leg", "motor_command",
                            new MotorCommandA(_motors));
    _log_id[10] =
      _logger->registerVar( LOG_FLOAT, 6, "Leg", "apex_command",
                            new MotorAnalogA(_motors));
    _log_id[11] =
      _logger->registerVar( LOG_FLOAT, 6, "Leg", "motor_temperature",
                            new TemperatureA(_motors));

    _log_id[12] =
      _logger->registerVar( LOG_FLOAT, 6, "Leg", "targetpos",
                            new PoscontrolTarPosA(_poscontrol));
    _log_id[13] =
      _logger->registerVar(LOG_FLOAT, 6, "Leg", "targetvel",
                           new PoscontrolTarVelA(_poscontrol));

    _log_id[14] =
      _logger->registerVar( LOG_FLOAT, 6, "Leg", "poserror",
                            new PoscontrolPosErrA(_poscontrol));
    _log_id[15] =
      _logger->registerVar(LOG_FLOAT, 6, "Leg", "velerror",
                           new PoscontrolVelErrA(_poscontrol));

    _log_id[16] =
      _logger->registerVar(LOG_FLOAT, 6, "Leg", "torqueoffset",
                           new PoscontrolTorqueOffsetA(_poscontrol));

    _log_id[17] =
      _logger->registerVar( LOG_FLOAT, 3, "Other", "acceleration",
                            new AccelerationA(_accels));

  }
}

void StateProxy::deactivate ( void ) {

  if ( _logger ) {

    for( int i = 0; i < STATEPROXY_NUM_VARS; i++)
      _logger->deleteVar( _log_id[i] );

    if ( _logger ) 
      MMReleaseModule( _logger, this );
  }

  for ( int i = 0; i < 6; i++) 
    MMReleaseModule( _encreaders[i], this);

  MMReleaseModule( _ap, this );
  MMReleaseModule( _hs_mon, this );
  MMReleaseModule( _gyro_mod, this );
  MMReleaseModule( _batt_mon, this );

  MMReleaseModule(_dbmodule, this);
}

void StateProxy::update ( void ) {

  Message* msg = _state_entry->createData();
  msg->setSize( sizeof( StateProxyStructure_t ) );
  StateProxyStructure_t* state = ( StateProxyStructure_t* ) msg->getData();

  int i;
  for ( i = 0; i < 6; i++) {
    state->positions[i] = _encreaders[i]->getPosition();
    state->temperatures[i] = _motors->getTemperature(i);
  }
  if ( _accels ) {
    state->accelerations[0] = _accels->read(AccelHW::AXIS_X);
    state->accelerations[1] = _accels->read(AccelHW::AXIS_Y);
    state->accelerations[2] = _accels->read(AccelHW::AXIS_Z);
  } else {
    state->accelerations[0] = 0.0;
    state->accelerations[1] = 0.0;
    state->accelerations[2] = 0.0;
  }
  if ( _batt_mon ) state->battery_dod = _batt_mon->getPercentRemaining();

  for(i=0;i<NUM_HS_SENSORS;i++)
	state->hs_temp[i] = _hs_mon->getHSTemp( i );

  state->avgvoltage = _ap->getVoltage();
  state->avgcurrent = _ap->getCurrent();
  state->avgpower = _ap->getPower();
  state->voltage = _power->voltage();
  state->current = _power->current();

  if (_logger) state->log_buffer = _logger->getBufSize();
  
  if( _compass_mod ) {
    state->heading = _compass_mod->getHeading();
    state->pitch   = _compass_mod->getPitch();
    state->roll    = _compass_mod->getRoll();
    state->dip     = _compass_mod->getDip();
  } else {
    state->heading = 0.0;
    state->pitch   = 0.0;
    state->roll    = 0.0;
    state->dip     = 0.0;
  }

  if( _gps_mod ) {
    state->latitude  = _gps_mod->getLatitude();
    state->longitude = _gps_mod->getLongitude();
    state->utc       = _gps_mod->getUtc();
    state->ns        = _gps_mod->getNS();
    state->ew        = _gps_mod->getEW();
    state->hdop     = 0;
    state->altitude = 0;
    state->dgpstime = 0;
    state->sats     = 0;
    state->fix      = 0;
    state->dgpsid   = 0;
  } else {
    state->latitude  = 0.0;
    state->longitude = 0.0;
    state->utc       = 0.0;
    state->ns        = 'N';
    state->ew        = 'E';
    state->hdop      = 99;
    state->altitude  = 0;
    state->dgpstime  = 99;
    state->sats      = 0;
    state->fix       = 0;
    state->dgpsid    = 0;
  }

  // update the state in the DB
  _state_entry->set(msg);

  // check on gyro calibration
  if ( _gyro_mod ) {
    if ( _gyro_calib && _gyro_mod->calibRunning() ) {
      _gyro_calib = false;
      MMMessage( "Gyro calibrating started\n", 240 );
    } else if ( !_gyro_calib && !_gyro_mod->calibRunning() ) {
      _gyro_calib = true;
      MMMessage( "Gyro calibrating completed\n", 240 );
    }
  }

}