// Bbot motoro drive stuff

int Drive_Motor(int torque);
void resetEncoderCounts(void);
void rightEncoder();
void leftEncoder();


volatile int32_t Rcount = 0;
int32_t Rlast_count = 0;
double Rvel = 0;
volatile int32_t Lcount = 0;
int32_t Llast_count = 0;

void InitMotors(void)
{
	// Set pin modes
	pinMode(InA_R, OUTPUT);
	pinMode(InB_R, OUTPUT);
	pinMode(PWM_R, OUTPUT);
	pinMode(InA_L, OUTPUT);
	pinMode(InB_L, OUTPUT);
	pinMode(PWM_L, OUTPUT);
	Drive_Motor(0);
}

int Drive_Motor(int torque)
{
	if (torque >= 0)
	{  // drive motors forward
		digitalWrite(InA_R, HIGH);
		digitalWrite(InB_R, LOW);
		digitalWrite(InA_L, HIGH);
		digitalWrite(InB_L, LOW);
	}
	else
	{ // drive motors backward
		digitalWrite(InA_R, LOW);
		digitalWrite(InB_R, HIGH);
		digitalWrite(InA_L, LOW);
		digitalWrite(InB_L, HIGH);
		torque = abs(torque);
	}
	analogWrite(PWM_R,torque);
	analogWrite(PWM_L,torque);
}

void setupEncoder()
{
	/* Setup encoders */
	pinMode(leftEncoder1,INPUT);
	digitalWrite(leftEncoder1,0);
	pinMode(leftEncoder2,INPUT);
	digitalWrite(leftEncoder2,0);
	pinMode(rightEncoder1,INPUT);
	digitalWrite(rightEncoder1,0);
	pinMode(rightEncoder2,INPUT); 
	digitalWrite(rightEncoder2,0);
	attachInterrupt(digitalPinToInterrupt(leftEncoder1),leftEncoder,CHANGE);
	attachInterrupt(digitalPinToInterrupt(rightEncoder1),rightEncoder,CHANGE);
	resetEncoderCounts();
}

void resetEncoderCounts(void)
{
	Rcount = 0;
	Rlast_count = 0;
	Lcount = 0;
	Llast_count = 0;
}

void rightEncoder()
{  // pulse and direction, direct port reading to save cycles
	if (PIND & 0b00001000)
	{
		if (PINB & 0b00001000)
			Rcount--;  // if(digitalRead(encodPinB1)==HIGH)   count++   (on DI #2)
		else
			Rcount++;
	}
	else
	{
		if (PINB & 0b00001000)
			Rcount++;  // if(digitalRead(encodPinB1)==HIGH)   count++   (on DI #2)
		else
			Rcount--;
	}
}

void leftEncoder()
{  // pulse and direction, direct port reading to save cycles
	if (PIND & 0b00000100)
	{
		if (PINB & 0b00000100)
			Lcount++;  // if(digitalRead(encodPinB1)==HIGH)   count++   (on DI #2)
		else
			Lcount--;
	}
	else
	{
		if (PINB & 0b00000100)
			Lcount--;  // if(digitalRead(encodPinB1)==HIGH)   count++   (on DI #2)
		else
			Lcount++;
	}
}
