#include <stdio.h>
#include <stdlib.h>
#include <phidget22.h>
#include <string.h>
#include <unistd.h>
#include <time.h>

#define HUB_SERIALNUMBER 	5000 //Change this to match your hubs serial number
#define SONAR_HUBPORT		0//You may need to change this
#define RELAY_HUBPORT		1//You may need to change this
#define DETECTED		1000 //Will activate relay for distances less than 1000mm

int objectDetected = 0;
int working = 0;

//Relay Event Handlers
static void CCONV
onRelayAttachHandler(PhidgetHandle phid, void *ctx) {
    PhidgetLog_log(PHIDGET_LOG_INFO, "Relay attached");
}
static void CCONV
onRelayDetachHandler(PhidgetHandle phid, void *ctx) {
    PhidgetLog_log(PHIDGET_LOG_INFO, "Relay detached");
}

//Sonar Event Handlers
static void CCONV
onSonarAttachHandler(PhidgetHandle phid, void *ctx) {
    PhidgetReturnCode result;
    PhidgetLog_log(PHIDGET_LOG_INFO, "Sonar attached");
    result = PhidgetDistanceSensor_setDataInterval((PhidgetDistanceSensorHandle)phid, 100); //set to min data interval
    if (result != EPHIDGET_OK) {
        PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set sonar data interval");
        return 1;
    }
}
static void CCONV
onSonarDetachHandler(PhidgetHandle phid, void *ctx) {
    PhidgetLog_log(PHIDGET_LOG_INFO, "Sonar detached");
}
void
onDistanceChangeHandler(PhidgetDistanceSensorHandle ch, void *ctx, unsigned int distance) {	
    if(distance >= DETECTED && !working){
        PhidgetLog_log(PHIDGET_LOG_INFO, "Object detected at distance %d", distance);
        objectDetected = 1;
    }
}


int main(void)
{
	PhidgetDistanceSensorHandle sonar;
	PhidgetDigitalOutputHandle relay;
	PhidgetReturnCode result;
	PhidgetLog_enable(PHIDGET_LOG_INFO, "spook.log");

	//create sonar
	result = PhidgetDistanceSensor_create(&sonar);
	if (result != EPHIDGET_OK) {
		PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to create sonar object");
		return 1;
	}
	result = Phidget_setDeviceSerialNumber((PhidgetHandle) sonar, HUB_SERIALNUMBER);
	if (result != EPHIDGET_OK) {
		PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set device serial number");
		return 1;
	}
	result = Phidget_setHubPort((PhidgetHandle) sonar, SONAR_HUBPORT);
	if (result != EPHIDGET_OK) {
		PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set hub port");
		return 1;
	}
    result = Phidget_setOnAttachHandler((PhidgetHandle)sonar, onSonarAttachHandler, NULL);
    if (result != EPHIDGET_OK) {
        PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set sonar attach handler");
        return 1;
    }
    result = Phidget_setOnDetachHandler((PhidgetHandle)sonar, onSonarDetachHandler, NULL);
    if (result != EPHIDGET_OK) {
        PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set sonar detach handler");
        return 1;
    }
	result = PhidgetDistanceSensor_setOnDistanceChangeHandler(sonar, onDistanceChangeHandler, NULL);
	if (result != EPHIDGET_OK) {
		PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set distance change handler");
		return 1;
	}
	result = Phidget_open((PhidgetHandle) sonar);
	if (result != EPHIDGET_OK) {
		PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to open sonar channel");
		return 1;
	}

	//create relay
	result = PhidgetDigitalOutput_create(&relay);
	if (result != EPHIDGET_OK) {
		PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to create relay object");
		return 1;
	}
	result = Phidget_setDeviceSerialNumber((PhidgetHandle)relay, HUB_SERIALNUMBER);
	if (result != EPHIDGET_OK) {
		PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set device serial number");
		return 1;
	}
	result = Phidget_setHubPort((PhidgetHandle)relay, RELAY_HUBPORT);
	if (result != EPHIDGET_OK) {
		PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set hub port");
		return 1;
	}
	result = Phidget_setIsHubPortDevice((PhidgetHandle)relay, 1);
	if (result != EPHIDGET_OK) {
		PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set is hub port device");
		return 1;
	}
    result = Phidget_setOnAttachHandler((PhidgetHandle)relay, onRelayAttachHandler, NULL);
    if (result != EPHIDGET_OK) {
        PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set relay attach handler");
        return 1;
    }
    result = Phidget_setOnDetachHandler((PhidgetHandle)relay, onRelayDetachHandler, NULL);
    if (result != EPHIDGET_OK) {
        PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to set relay detach handler");
        return 1;
    }
	result = Phidget_open((PhidgetHandle)relay);
	if (result != EPHIDGET_OK) {
		PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to open relay channel");
		return 1;
	}

	
	while (1) {
		if(objectDetected){
			objectDetected = 0;
			working = 1;
			result = PhidgetDigitalOutput_setState(relay, 1); //turn on relay
            if (result != EPHIDGET_OK) {
                PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to change relay state");
                return 1;
            }
			sleep(0.5);
			result = PhidgetDigitalOutput_setState(relay, 0); //turn off relay
            if (result != EPHIDGET_OK) {
                PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to change relay state");
                return 1;
            }
			sleep(4);
			result = PhidgetDigitalOutput_setState(relay, 1); //turn on relay
            if (result != EPHIDGET_OK) {
                PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to change relay state");
                return 1;
            }
			sleep(0.5);
			result = PhidgetDigitalOutput_setState(relay, 0); //turn off relay
            if (result != EPHIDGET_OK) {
                PhidgetLog_log(PHIDGET_LOG_ERROR, "failed to change relay state");
                return 1;
            }
			working = 0;
		}
	}

	return 0;
}