#include<stdio.h>
#include<fcntl.h>
#include<string.h>
#include<unistd.h>
#include<string.h>
#include<stdbool.h>
#include "gpio.h"

#define PIN_MOTOR_RECHTS_FORWARD "20"
#define PIN_MOTOR_RECHTS_REVERSE "13"
#define PIN_MOTOR_LINKS_REVERSE "19"
#define PIN_MOTOR_LINKS_FORWARD "26"

#define MOTOR_ON 1
#define MOTOR_OFF 0

bool motor_rechts_forward_on = false;
bool motor_rechts_reverse_on = false;
bool motor_links_forward_on = false;
bool motor_links_reverse_on = false;

void forward(int motorNumber)
{
	if(motorNumber == 1)
	{
		writeOutput(PIN_MOTOR_RECHTS_FORWARD, MOTOR_ON);		
		motor_rechts_forward_on = true;
	}
	else if(motorNumber == 2)
	{
		writeOutput(PIN_MOTOR_LINKS_FORWARD, MOTOR_ON);
		motor_links_forward_on = true;
	}
	else if(motorNumber == 3)
	{
		writeOutput(PIN_MOTOR_RECHTS_FORWARD, MOTOR_ON);
		writeOutput(PIN_MOTOR_LINKS_FORWARD, MOTOR_ON);
		motor_rechts_forward_on = true;
		motor_links_forward_on = true;
	}		
}

void reverse(int motorNumber)
{
	if(motorNumber == 1)
	{
		writeOutput(PIN_MOTOR_RECHTS_REVERSE, MOTOR_ON);
		motor_rechts_reverse_on = true;
	}
	else if(motorNumber == 2)
	{
		writeOutput(PIN_MOTOR_LINKS_REVERSE, MOTOR_ON);
		motor_links_reverse_on = true;
	}
	else if(motorNumber == 3)
	{
		writeOutput(PIN_MOTOR_RECHTS_REVERSE, MOTOR_ON);
		writeOutput(PIN_MOTOR_LINKS_REVERSE, MOTOR_ON);
		motor_rechts_reverse_on = true;
		motor_links_reverse_on = true;
		
	}
}

void stopMotor(int motorNumber)
{
	if(motorNumber == 1 && motor_rechts_forward_on)
	{
		writeOutput(PIN_MOTOR_RECHTS_FORWARD, MOTOR_OFF);
		motor_rechts_forward_on = false;
	}
	else if(motorNumber == 2 && motor_links_forward_on)
	{
		writeOutput(PIN_MOTOR_LINKS_FORWARD, MOTOR_OFF);
		motor_links_forward_on = false;
	}
	else if(motorNumber == 3 && motor_rechts_reverse_on)
	{
		writeOutput(PIN_MOTOR_RECHTS_REVERSE, MOTOR_OFF);
		motor_rechts_reverse_on = false;
	}
	else if(motorNumber == 4 && motor_links_reverse_on)
	{
		writeOutput(PIN_MOTOR_LINKS_REVERSE, MOTOR_OFF);
		motor_links_reverse_on = false;
	}
	
}

int main(int argc, char* argv[])
{
	registerOutput(PIN_MOTOR_RECHTS_FORWARD);
	registerOutput(PIN_MOTOR_RECHTS_REVERSE);
	registerOutput(PIN_MOTOR_LINKS_FORWARD);
	registerOutput(PIN_MOTOR_LINKS_REVERSE);
	

	if(argc > 0)
	{
		if(strcmp(argv[1],"1")==0)
		{
			//motor_rechts_forward
			forward(1);
		} 
		else if(strcmp(argv[1],"2")==0)
		{
			//motor_links_forward
			forward(2);
		} 
		else if(strcmp(argv[1],"3")==0)
		{
			//motor_rechts_reverse
			reverse(1);
		} 
		else if(strcmp(argv[1],"4")==0)
		{
			//motor_links_reverse
			reverse(2);
		} 
		else if(strcmp(argv[1],"5")==0)
		{

			forward(3);
		}
		else if(strcmp(argv[1],"6")==0)
		{
			//motor_rechts_forward stop
			stopMotor(1);
		}
		else if(strcmp(argv[1],"7")==0)
		{
			//motor_links_forward stop
			stopMotor(2);
		}
		else if(strcmp(argv[1],"8")==0)
		{
			//motor_rechts_reverse stop
			stopMotor(3);
		}
		else if(strcmp(argv[1],"9")==0)
		{
			//motor_links_reverse stop
			stopMotor(4);
		}
		else if(strcmp(argv[1],"0")==0)
		{
			freePin(PIN_MOTOR_RECHTS_FORWARD);
			freePin(PIN_MOTOR_RECHTS_REVERSE);
			freePin(PIN_MOTOR_LINKS_FORWARD);
			freePin(PIN_MOTOR_LINKS_REVERSE);
		}
	}
	return 0;
}