#include<stdio.h>
#include<fcntl.h>
#include<string.h>
#include<unistd.h>
#include<string.h>

#define BASEPATH "/sys/class/gpio/"
#define GPIO_FOLDER "gpio%s/"

#define PIN_MOTOR1_FORWARD "20"
#define PIN_MOTOR1_REVERSE "13"
#define PIN_MOTOR2_FORWARD "19"
#define PIN_MOTOR2_REVERSE "26"

#define MOTOR_ON 1
#define MOTOR_OFF 0

void writeFile(char *filename, char *buffer, size_t count)
{
	int fd = open(filename, O_WRONLY);
	if(fd == -1) {
		perror("Fehler bei open ");
	}
	write(fd, buffer, count);
	close(fd);
}

void registerPin(char *pin)
{
	writeFile(BASEPATH "export", pin, strlen(pin));
}

void freePin(char *pin)
{
	writeFile(BASEPATH "unexport", pin, strlen(pin));
}

void setDirection(char *pin, char *direction, int dirlen)
{
	char path[50];
	sprintf(path, BASEPATH GPIO_FOLDER "direction", pin);
	writeFile(path, direction, dirlen);
}

void registerOutput(char *pin)
{
	registerPin(pin);
	setDirection(pin, "out", 3);
}

void writeOutput(char *pin, int state)
{
	char path[50];
	sprintf(path, BASEPATH GPIO_FOLDER "value", pin);
	FILE *fd = fopen(path, "w");
	fprintf(fd, "%i", state);
	fclose(fd);
}

void forward(int motorNumber)
{
	if(motorNumber == 1)
	{
		writeOutput(PIN_MOTOR1_FORWARD, MOTOR_ON);		
	}
	else if(motorNumber == 2)
	{
		writeOutput(PIN_MOTOR2_FORWARD, MOTOR_ON);
	}
	else if(motorNumber == 3)
	{
		writeOutput(PIN_MOTOR1_FORWARD, MOTOR_ON);
		writeOutput(PIN_MOTOR2_FORWARD, MOTOR_ON);
	}		
}

void reverse(int motorNumber)
{
	if(motorNumber == 1)
	{
		writeOutput(PIN_MOTOR1_REVERSE, MOTOR_ON);
	}
	else if(motorNumber == 2)
	{
		writeOutput(PIN_MOTOR2_REVERSE, MOTOR_ON);
	}
	else if(motorNumber == 3)
	{
		writeOutput(PIN_MOTOR1_REVERSE, MOTOR_ON);
		writeOutput(PIN_MOTOR2_REVERSE, MOTOR_ON);
	}
}

int main(int argc, char* argv[])
{
	registerOutput(PIN_MOTOR1_FORWARD);
	registerOutput(PIN_MOTOR1_REVERSE);
	registerOutput(PIN_MOTOR2_FORWARD);
	registerOutput(PIN_MOTOR2_REVERSE);
	if(strcmp(argv[1],"1")==0)
	{
		forward(1);
	} 
	else if(strcmp(argv[1],"2")==0)
	{
		forward(2);
	} 
	else if(strcmp(argv[1],"3")==0)
	{
		reverse(1);
	} 
	else if(strcmp(argv[1],"4")==0)
	{
		reverse(2);
	} 
	else if(strcmp(argv[1],"0")==0)
	{
		freePin(PIN_MOTOR1_FORWARD);
		freePin(PIN_MOTOR1_REVERSE);
		freePin(PIN_MOTOR2_FORWARD);
		freePin(PIN_MOTOR2_REVERSE);
	}
	
	return 0;
}