//================================================================================
// Pin Assignments
//================================================================================
//Encoder
#define enkoderPinA  2
#define enkoderPinB  3
#define enkoderIndexPin 21
//Motor
#define kjørPin 4
#define retningPin 10
#define hurtighetPin 5
//Bryter
#define startknappPin 9



//================================================================================
// Faster DigitalRead/Write
//================================================================================
//setup
#define portOfPin(P)\
  (((P)>=0&&(P)<8)?&PORTD:(((P)>7&&(P)<14)?&PORTB:&PORTC))
#define ddrOfPin(P)\
  (((P)>=0&&(P)<8)?&DDRD:(((P)>7&&(P)<14)?&DDRB:&DDRC))
#define pinOfPin(P)\
  (((P)>=0&&(P)<8)?&PIND:(((P)>7&&(P)<14)?&PINB:&PINC))
#define pinIndex(P)((uint8_t)(P>13?P-14:P&7))
#define pinMask(P)((uint8_t)(1<<pinIndex(P)))
//funksjoner som brukes
#define pinAsInput(P) *(ddrOfPin(P))&=~pinMask(P)
#define pinAsInputPullUp(P) *(ddrOfPin(P))&=~pinMask(P);digitalHigh(P)
#define pinAsOutput(P) *(ddrOfPin(P))|=pinMask(P)
#define digitalLow(P) *(portOfPin(P))&=~pinMask(P)
#define digitalHigh(P) *(portOfPin(P))|=pinMask(P)
#define isHigh(P)((*(pinOfPin(P))& pinMask(P))>0)
#define isLow(P)((*(pinOfPin(P))& pinMask(P))==0)

//================================================================================
// Debug
//================================================================================

#ifdef DEBUG
#define DEBUG_PRINT(x)     Serial.print (x)
#define DEBUG_PRINTDEC(x)     Serial.print (x, DEC)
#define DEBUG_PRINTLN(x)  Serial.println (x)
#else
#define DEBUG_PRINT(x)
#define DEBUG_PRINTDEC(x)
#define DEBUG_PRINTLN(x)
#endif


//================================================================================
// Variabler
//================================================================================

//Encoder
volatile int enkoder0Pos = 0;
volatile int enkoder0PosTemp = 0;
int runder = 0;
int runderCW = 0;
int runderCCW = 0;
//Motor
boolean motorStart;
unsigned int fart = 10;
//Misc
#define DEBUG 0

//================================================================================
// Main
//================================================================================
void setup()
{
	///////setup Pinns///////
	//Bryter
	pinAsInput(startknappPin);
	//Motor
	pinAsOutput(kjørPin);
	pinAsOutput(retningPin);
	pinAsOutput(hurtighetPin);
	//Encoder
	pinAsInput(enkoderPinA);
	pinAsInput(enkoderPinB);
	pinAsInput(enkoderIndexPin);
	//interupts
	attachInterrupt(0, doEnkoderA, RISING);
	attachInterrupt(1, doEnkoderB, RISING);
	attachInterrupt(2, doEnkoderI, RISING);
	attachInterrupt(3, doBryter, CHANGE);

	//Serial Setup
	Serial.begin(115200);

}

//Main loop
void loop()
{

	if (enkoder0Pos != enkoder0PosTemp) {
		DEBUG_PRINTLN(enkoder0Pos);
		enkoder0PosTemp = enkoder0Pos;
	}


	if (motorStart){
		beregnVei();
	}
}

//================================================================================
// Funksjoner
//================================================================================

void beregnVei(){
	//kjør høyre
	if (enkoder0Pos > 100){
		digitalLow(retningPin);
		analogWrite(hurtighetPin, 10);
		return;
	}
	//kjør venstre
	if (enkoder0Pos == 0){
		digitalHigh(retningPin);
		analogWrite(hurtighetPin,10);
	}
}


//================================================================================
// Interupts
//================================================================================
/////Encoder/////
//Interrupt på stigandeflanke A
void doEnkoderA()
{
	//Agammal++;
	if (isHigh(enkoderPinB))
	{
		enkoder0Pos--;
	}
}

//Interrupt på stigande flanke B
void doEnkoderB()
{
	//Bny++;
	if (isHigh(enkoderPinA))
	{
		enkoder0Pos++;
	}
}

void doEnkoderI()
{
	runder += 1;

	if (enkoder0Pos++)
	{
		runderCW++;
	}

	if (enkoder0Pos--)
	{
		runderCCW++;
	}
}

/////Bryter/////
void doBryter(){
	if (isHigh(startknappPin)){
		motorStart = true;
		return;
	}
	motorStart = false;
}