// servoexample1.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <winsock2.h>
#include "..\phidget21.h"

HANDLE m_EventHandle;

int __stdcall AttachHandler(CPhidgetHandle Servo, void *userptr)
{
	SetEvent(m_EventHandle);
	printf("Attach handler ran!\n");
	return 0;
}

int __stdcall DetachHandler(CPhidgetHandle Servo, void *userptr)
{
	printf("Detach handler ran!\n");
	return 0;
}

int __stdcall ErrorHandler(CPhidgetHandle Servo, void *userptr, long ErrorCode, void *unknown)
{
	printf("Error handler ran!\n");
	return 0;
}

void display_generic_properties(CPhidgetHandle phid)
{
	long sernum, version;
	const char *deviceptr;
	CPhidget_getDeviceType((CPhidgetHandle)phid, &deviceptr);
	CPhidget_getSerialNumber((CPhidgetHandle)phid, &sernum);
	CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);

	printf("%s\n", deviceptr);
	printf("Version: %8d SerialNumber: %10d\n", version, sernum);
	return;
}

//int SRV_MotorPositionChangeHandler(struct __CPhidgetServoInfo *IFK, void *userptr, int Index, double Value)
int __stdcall SRV_MotorPositionChangeHandler(CPhidgetServoHandle IFK, void *userptr, int Index, double Value)
{
	printf("Motor Position %d is %lf\n", Index, Value);
	return 0;
}

int main(int argc, char* argv[])
{
	int result;

	CPhidgetServoHandle Servo = 0;

	m_EventHandle = CreateEvent(NULL, FALSE, FALSE, NULL);

	CPhidgetServo_create(&Servo);

	CPhidget_set_OnAttach_Handler((CPhidgetHandle)Servo, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)Servo, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)Servo, ErrorHandler, NULL);
	CPhidgetServo_set_OnMotorPositionChange_Handler(Servo, SRV_MotorPositionChangeHandler, NULL);

	result = CPhidget_open((CPhidgetHandle)Servo, -1);
	if (result)
	{
		printf("Cannot find PhidgetServo\n");
		return 0;
	}
	result = WaitForSingleObject(m_EventHandle, 100000); 

	display_generic_properties((CPhidgetHandle)Servo);

	result = CPhidgetServo_setMotorPosition(Servo, 0,-23.0);
	Sleep(500);
	result = CPhidgetServo_setMotorPosition(Servo, 0,40.0);
	Sleep(500);
	result = CPhidgetServo_setMotorPosition(Servo, 0,80.0);
	Sleep(500);
	result = CPhidgetServo_setMotorPosition(Servo, 0,120.0);
	Sleep(500);

	CPhidget_close((CPhidgetHandle)Servo);
	CPhidget_delete((CPhidgetHandle)Servo);
	return 0;
}

