//callback that will run if the Spatial is detached from the computer
int imuDetach(CPhidgetHandle spatial, void *userptr)
{
	int serialNo;
	CPhidget_getSerialNumber(spatial, &serialNo);
	printf("IMU %10d detached!\n", serialNo);
	return 0;
}

int display_properties(CPhidgetAccelerometerHandle phid)
{
    int serial_number, version, num_axes;
    const char* ptr;

    CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
    CPhidget_getSerialNumber((CPhidgetHandle)phid,
							 &serial_number);
    CPhidget_getDeviceVersion((CPhidgetHandle)phid,
							  &version);

    CPhidgetAccelerometer_getAxisCount((CPhidgetAccelerometerHandle)phid,
									   &num_axes);

    // initialise acceleration values
    ros::Time begin = ros::Time::now();
    acc.acceleration.clear();
    acc.stamp.clear();
    for (int i = 0; i < num_axes; i++) {
        acc.acceleration.push_back(0);
        acc.stamp.push_back(begin);
        acc.stamp.push_back(begin);
        acc.stamp.push_back(begin);
    }

    ROS_INFO("%s", ptr);
    ROS_INFO("Serial Number: %d", serial_number);
    ROS_INFO("Version: %d", version);
    ROS_INFO("Number of axes %d", num_axes);

    return 0;
}

//Display the properties of the attached phidget to the screen.  We will be displaying the name, serial number and version of the attached device.
//Will also display the number of inputs, outputs, and analog inputs on the interface kit as well as the state of the ratiometric flag
//and the current analog sensor sensitivity.
int IKDisplayProperties(CPhidgetInterfaceKitHandle phid)
{
	int serialNo, version, numInputs, numOutputs, numSensors, triggerVal, ratiometric, i;
	const char* ptr;

	CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
	CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
	CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);

	CPhidgetInterfaceKit_getInputCount(phid, &numInputs);
	CPhidgetInterfaceKit_getOutputCount(phid, &numOutputs);
	CPhidgetInterfaceKit_getSensorCount(phid, &numSensors);
	CPhidgetInterfaceKit_getRatiometric(phid, &ratiometric);

	SetupLog("%s", ptr);
	SetupLog("Serial Number: %10d\nVersion: %8d", serialNo, version);
	SetupLog("# Digital Inputs: %d\n# Digital Outputs: %d", numInputs, numOutputs);
	SetupLog("# Sensors: %d", numSensors);
	SetupLog("Ratiometric: %d", ratiometric);

	for(i = 0; i < numSensors; i++)
	{
		CPhidgetInterfaceKit_getSensorChangeTrigger (phid, i, &triggerVal);

		SetupLog("Sensor#: %d > Sensitivity Trigger: %d\n", i, triggerVal);
	}

	return 0;
}

// print the sensor values for a given ifkit device (by serial)
// -1 to print them all!
void PhidgetConnector::print(int serial_in) {
    cout << "Models: " << endl;
    if(ifKitModels.size()>0) {

        for(int i=0; i<ifKitModels.size(); i++) {
            IFKitModel * thisKit = (IFKitModel *)ifKitModels.at(i);
            if(serial_in == -1 || serial_in == thisKit->getSerial()) thisKit->print();
        }
    } else {
        cout << "there doesn't seem to be any devices successfully connected with which to print." << endl;
    }
    cout << "hardware devices: " << endl;
    for(int j=0; j<ifkits.size(); j++) {
        int serialNo;
        CPhidget_getSerialNumber((CPhidgetHandle)*ifkits.at(j), &serialNo);
        int whichKit = getIFKitModelID(serialNo);
        if(whichKit>-1) {
            IFKitModel * desiredKit = ifKitModels.at(whichKit);
            cout << "serial: " << desiredKit->getSerial() << endl;
            for(int i=0; i<desiredKit->getNumSensors(); i++) {
                int sensorVal;
                CPhidgetInterfaceKit_getSensorValue(*ifkits.at(j), i, &sensorVal);
                cout << i << ": " << sensorVal << endl;
                // desiredKit->setSensorVal(i,sensorVal);
            }
        }
    }

}

static int CCONV AttachHandler (CPhidgetHandle device, void *userptr) {

    int serialNumber;
    const char *name;

    LocalErrorCatcher(
        CPhidget_getDeviceName(device, &name));
    LocalErrorCatcher(
        CPhidget_getSerialNumber(device, &serialNumber));

    PhidgetsDeviceManager* obj = (PhidgetsDeviceManager*)userptr;

    QString devname(name);


    // If the device name contains motor control in its name then emit a motor control card signal
    if(devname.contains("Motor Control"))
    {
        obj->pushToMotorControlCardSerials(serialNumber);
       // emit obj->motorCard(serialNumber);

    }
    else if(devname.contains("InterfaceKit"))
    {
        obj->setInterfaceKitSerial(serialNumber);
    }



    printf("Hello Device %s, Serial Number: %d\n", name, serialNumber);

    return 0;
}

int display_properties(CPhidgetPHSensorHandle phid)
{
    int serialNo, version;
    double trigger, potential;
    double min, max;
    const char* ptr;

    CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
    CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
    CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);

    CPhidgetPHSensor_getPHChangeTrigger(phid, &trigger);
    CPhidgetPHSensor_getPotential(phid, &potential);

    ROS_INFO("%s\n", ptr);
    ROS_INFO("Serial Number: %10d\nVersion: %8d\n",
			 serialNo, version);
    ROS_INFO("Trigger Sensitivity: %f\nPotential: %f\n",
			 trigger, potential);
    CPhidgetPHSensor_getPHMax(phid, &max);
    CPhidgetPHSensor_getPHMin(phid, &min);
    ROS_INFO("Max PH: %0.4lf, Min PH: %0.4lf\n", max, min);
    CPhidgetPHSensor_getPotentialMax(phid, &max);
    CPhidgetPHSensor_getPotentialMin(phid, &min);
    ROS_INFO("Max Potential: %0.2lfmV, " \
			 "Min Potential: %0.2lfmV\n", max, min);

    return 0;
}

//Display the properties of the attached phidget to the screen.  We will be displaying the name, serial number and version of the attached device.
int display_properties(CPhidgetTextLCDHandle phid)
{
	int serialNo, version, numRows, numColumns, backlight, cursor, contrast, cursor_blink, numScreens;
	const char* ptr;
	CPhidget_DeviceID id;
	
	CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
	CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
	CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);
	CPhidget_getDeviceID((CPhidgetHandle)phid, &id);

	CPhidgetTextLCD_getRowCount (phid, &numRows);
	CPhidgetTextLCD_getColumnCount (phid, &numColumns);
	CPhidgetTextLCD_getBacklight (phid, &backlight);
	CPhidgetTextLCD_getContrast (phid, &contrast);
	CPhidgetTextLCD_getCursorOn (phid, &cursor);
	CPhidgetTextLCD_getCursorBlink (phid, &cursor_blink);

	printf("%s\n", ptr);
	printf("Serial Number: %10d\nVersion: %8d\n", serialNo, version);
	if(id == PHIDID_TEXTLCD_ADAPTER){
		CPhidgetTextLCD_getScreenCount (phid, &numScreens);
		printf("# Screens: %d\n", numScreens);
		CPhidgetTextLCD_setScreen(phid, 0);
		CPhidgetTextLCD_setScreenSize(phid, PHIDGET_TEXTLCD_SCREEN_2x16);
		CPhidgetTextLCD_initialize(phid);
	}

	printf("# Rows: %d\n# Columns: %d\n", numRows, numColumns);
	printf("Current Contrast Level: %d\nBacklight Status: %d\n", contrast, backlight);
	printf("Cursor Status: %d\nCursor Blink Status: %d\n", cursor, cursor_blink);

	return 0;
}

//Display the properties of the attached phidget(s) to the screen.  We will be displaying the name, serial number and version of the attached device(s).
int display_devices(CPhidgetManagerHandle MAN)
{
	int serialNo, version, numDevices, i;
	const char* ptr;
	CPhidgetHandle *devices;

	CPhidgetManager_getAttachedDevices (MAN, &devices, &numDevices);

	printf("|-   # -|-              Type              -|- Serial No. -|-  Version -|\n");
	printf("|-------|----------------------------------|--------------|------------|\n");


	for(i = 0; i < numDevices; i++)
	{
		CPhidget_getDeviceType(devices[i], &ptr);
		CPhidget_getSerialNumber(devices[i], &serialNo);
		CPhidget_getDeviceVersion(devices[i], &version);

		printf("|- %3d -|- %30s -|- %10d -|- %8d -|\n", i, ptr, serialNo, version);
		printf("|-------|----------------------------------|--------------|------------|\n");
	}

	CPhidgetManager_freeAttachedDevicesArray(devices);

	return 0;
}

int display_properties(CPhidgetSpatialHandle phid)
{
    int serialNo, version;
    const char* ptr;
    int numAccelAxes, numGyroAxes, numCompassAxes;
	int dataRateMax, dataRateMin;

    CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
    CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
    CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);
    CPhidgetSpatial_getAccelerationAxisCount((CPhidgetSpatialHandle)phid,
											 &numAccelAxes);
    CPhidgetSpatial_getGyroAxisCount((CPhidgetSpatialHandle)phid,
									 &numGyroAxes);
    CPhidgetSpatial_getCompassAxisCount((CPhidgetSpatialHandle)phid,
										&numCompassAxes);
    CPhidgetSpatial_getDataRateMax((CPhidgetSpatialHandle)phid,
								   &dataRateMax);
    CPhidgetSpatial_getDataRateMin((CPhidgetSpatialHandle)phid,
								   &dataRateMin);

    ROS_INFO("%s\n", ptr);
    ROS_INFO("Serial Number: %10d\nVersion: %8d\n",
			 serialNo, version);
    ROS_INFO("Number of Accel Axes: %i\n", numAccelAxes);
    ROS_INFO("Number of Gyro Axes: %i\n", numGyroAxes);
    ROS_INFO("Number of Compass Axes: %i\n", numCompassAxes);
    ROS_INFO("datarate> Max: %d  Min: %d\n",
			 dataRateMax, dataRateMin);

    return 0;
}

int display_properties(CPhidgetInterfaceKitHandle phid)
{
    int serial_number, version, ratiometric, num_sensors, num_inputs, num_outputs, triggerVal;
    const char* ptr;

    CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
    CPhidget_getSerialNumber((CPhidgetHandle)phid, &serial_number);
    CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);

    CPhidgetInterfaceKit_getInputCount(phid, &num_inputs);
    CPhidgetInterfaceKit_getOutputCount(phid, &num_outputs);
    CPhidgetInterfaceKit_getSensorCount(phid, &num_sensors);
    CPhidgetInterfaceKit_getRatiometric(phid, &ratiometric);

    ROS_INFO("%s", ptr);
    ROS_INFO("Serial Number: %d", serial_number);
    ROS_INFO("Version: %d", version);
    ROS_INFO("Number of digital inputs %d", num_inputs);
    ROS_INFO("Number of digital outputs %d", num_outputs);
    ROS_INFO("Number of sensors %d", num_sensors);
    ROS_INFO("Ratiometric %d", ratiometric);

    for (int i = 0; i < num_sensors; i++)
    {       
        CPhidgetInterfaceKit_getSensorChangeTrigger (phid, i, &triggerVal);
        //CPhidgetInterfaceKit_setSensorChangeTrigger (phid, i, 10);
	ROS_INFO("Sensor %d Sensitivity Trigger %d", i, triggerVal);
    }

    return 0;
}

// display the properties and create the ifkit model. Should really refactor...
int PhidgetConnector::display_properties(CPhidgetInterfaceKitHandle phid) {

    int serialNo, version, numInputs, numOutputs, numSensors, triggerVal, ratiometric, i;
    const char* ptr;

    CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
    CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
    CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);

    CPhidgetInterfaceKit_getInputCount(phid, &numInputs);
    CPhidgetInterfaceKit_getOutputCount(phid, &numOutputs);
    CPhidgetInterfaceKit_getSensorCount(phid, &numSensors);
    CPhidgetInterfaceKit_getRatiometric(phid, &ratiometric);
    ifKitModels.push_back(new IFKitModel(serialNo, numSensors));



    printf("%s\n", ptr);
    printf("Serial Number: %10d\nVersion: %8d\n", serialNo, version);
    printf("# Digital Inputs: %d\n# Digital Outputs: %d\n", numInputs, numOutputs);
    printf("# Sensors: %d\n", numSensors);
    printf("Ratiometric: %d\n", ratiometric);

    for(i = 0; i < numSensors; i++) {
        CPhidgetInterfaceKit_setSensorChangeTrigger(phid, i, 0);
        CPhidgetInterfaceKit_getSensorChangeTrigger (phid, i, &triggerVal);
        printf("Sensor#: %d > Sensitivity Trigger: %d\n", i, triggerVal);
    }

    return 0;
}

void imuPrintInfo( void* handle )
{
	if( !handle )
		return;

	CPhidgetHandle phid = (CPhidgetHandle)handle;

	int serialNo = 0;
	int version = 0;
	const char* ptr;
	int numAccelAxes = 0, numGyroAxes = 0, numCompassAxes = 0;
	int dataRateMax = 0, dataRateMin = 0, dataRateCurr = 0;

	//CPhidget_getDeviceType(phid, &ptr);
	//printf("IMU  %s\n", ptr);

	CPhidget_getSerialNumber(phid, &serialNo);
	CPhidget_getDeviceVersion(phid, &version);
	CPhidgetSpatial_getAccelerationAxisCount((CPhidgetSpatialHandle)phid, &numAccelAxes);
	CPhidgetSpatial_getGyroAxisCount((CPhidgetSpatialHandle)phid, &numGyroAxes);
	CPhidgetSpatial_getCompassAxisCount((CPhidgetSpatialHandle)phid, &numCompassAxes);
	CPhidgetSpatial_getDataRateMax((CPhidgetSpatialHandle)phid, &dataRateMax);
	CPhidgetSpatial_getDataRateMin((CPhidgetSpatialHandle)phid, &dataRateMin);
	CPhidgetSpatial_getDataRate((CPhidgetSpatialHandle)phid, &dataRateCurr);
	
	
	printf("IMU  Serial Number: %10i\n", serialNo);
	printf("IMU  Version: %8i\n", version);
	printf("IMU  Number of Accel Axes: %i\n", numAccelAxes);
	printf("IMU  Number of Gyro Axes: %i\n", numGyroAxes);
	printf("IMU  Number of Compass Axes: %i\n", numCompassAxes);
	printf("IMU  datarate>   Min: %d  Max: %d  Current: %d\n", dataRateMin, dataRateMax, dataRateCurr);
}

int CCONV DetachHandler(CPhidgetHandle ENC, void *userptr)
{
	int serialNo;
	CPhidget_getSerialNumber(ENC, &serialNo);
	printf("Encoder %10d detached! \n", serialNo);

	return 0;
}

//callback that will run if the Accelerometer is detached from the computer
int CCONV DetachHandler(CPhidgetHandle ir, void *userptr)
{
	int serialNo;
	CPhidget_getSerialNumber(ir, &serialNo);
	printf("PhidgetIR %10d detached! \n", serialNo);

	return 0;
}

int AttachHandlerIR(CPhidgetHandle ir, void *userptr)
{
	int serialNo;
	CPhidget_getSerialNumber(ir, &serialNo);
	printf("PhidgetIR %10d attached!", serialNo);

	return 0;
}

//callback that will run if the Spatial is detached from the computer
int CCONV DetachHandler(CPhidgetHandle spatial, void *userptr)
{
    int serialNo;
    CPhidget_getSerialNumber(spatial, &serialNo);
    printf("Spatial %10d detached! \n", serialNo);

    return 0;
}

//callback that will run if the Accelerometer is detached from the computer
int DetachHandler(CPhidgetHandle IFK, void *userptr)
{
	int serialNo;
	CPhidget_getSerialNumber(IFK, &serialNo);
	printf("Accelerometer %10d detached! \n", serialNo);

	return 0;
}

//callback that will run if the Spatial is attached to the computer
int imuAttach(CPhidgetHandle spatial, void *userptr)
{
	int serialNo;
	CPhidget_getSerialNumber(spatial, &serialNo);
	printf("IMU %10d attached!\n", serialNo);
	imuPrintInfo(spatial);
	return 0;
}

Wt::WContainerWidget* WidgetsCommon::CreateWidget()
{
	PhidgetsInfo* item = ::GetPhidgetManager()->FindPhidgetBySerial(GetSerial());
	if (!item)
		return NULL;
	
	CPhidgetHandle handle = item->m_phidget->GetHandle();

	Wt::WContainerWidget* tab_container = new Wt::WContainerWidget();
  Wt::WHBoxLayout* hbox = new Wt::WHBoxLayout(tab_container);
	
	Wt::WTable* table = new Wt::WTable();
	hbox->addWidget(table);
	
	table->columnAt(0)->setWidth(GetLeftColumnWidth());
	table->columnAt(1)->setWidth(Wt::WLength::Auto);

	int row = 0;
	const char* string_value;
	int int_value;
	if (EPHIDGET_OK == CPhidget_getDeviceName(handle, &string_value))
	{
		table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("DeviceName")));
		table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString(string_value, Wt::UTF8)));
	}
	
	if (EPHIDGET_OK == CPhidget_getSerialNumber(handle, &int_value))
	{
		table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("SerialNumber")));
		table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString("{1}").arg(int_value)));
	}

	if (EPHIDGET_OK == CPhidget_getDeviceVersion(handle, &int_value))
	{
		table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("DeviceVersion")));
		table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString("{1}").arg(int_value)));
	}

	if (EPHIDGET_OK == CPhidget_getDeviceStatus(handle, &int_value))
	{
		table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("DeviceStatus")));
		table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString("{1}").arg(int_value)));
	}

	if (EPHIDGET_OK == CPhidget_getDeviceType(handle, &string_value))
	{
		table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("DeviceType")));
		table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString(string_value, Wt::UTF8)));
	}

	if (EPHIDGET_OK == CPhidget_getDeviceLabel(handle, &string_value))
	{
		table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("DeviceLabel")));
		table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString(string_value, Wt::UTF8)));
	}

	return tab_container;
}

int CCONV DetachHandler( CPhidgetHandle ADVSERVO, void *userptr )
{
	int serialNo;
	const char *name;
	CPhidget_getDeviceName( ADVSERVO, &name );
	CPhidget_getSerialNumber( ADVSERVO, &serialNo );
	printf( "%s %10d detached!\n", name, serialNo );
	return 0;
}