// ROBOTH.h
// We declare all variables(properties) and functions(methods) of the ROBOT object inside this header file

#ifndef _ROBOTH_h
#define _ROBOTH_h

#include "Arduino.h"

// Class for MOTOR
class MOTOR
{
public:
	// Motor Type:
	// "D" - DC Motor
	// "E" - EV3 Motor
	String gstrMotorType = "";

	// bolVSpeed = true means: Use analogWrite(pin, value) to enable setting the speed of the DC motor
	// On Arduino UNO, the following PINs support analogWrite: 3, 5, 6, 9, 10, 11
	// On Arduino MEGA, the following PINS support analogWrite: 2-13, 44-46
	// If all PINS support analogWrite are occupied, turn gbolVSpeed = false, which means that the Motor will run in FULL speed.
	bool gbolVSpeed = true;

	// Direction of the Motor, 1 or -1
	int gintDir = 1;

	// PIN No. for Input PIN 1
	int gintInput1PIN = 0;

	// PIN No. for Input PIN 2
	int gintInput2PIN = 0;

	// Initialize a Motor
	void Init(int intMotorNo, String strMotorType, bool bolVSpeed, int intDir, int intInput1PIN, int intInput2PIN);

	// Move a Motor
	void Move(int intMotorNo, int intDir, int intSpeed);

	// Stop a Motor
	void Stop(int intMotorNo);
};

// Class for ROBOT
class ROBOT
{
public:
	// Part A - Public vars

	// A.1	LED Related vars

	// Total No. of LED
	int gintNoOfLED = 0;
	// Pin No. of the LEDs, a Maximum of 10 LED is allowed
	int garyLED[10];

	// A.11	Motor Related vars

	// Total No. of Motor, 0 - 10
	int gintNoOfMotor = 0;

	// Type of Each Motor
	// "D" = DC Motor
	// "S" = Step Motor
	// "V" = Servo Motor
	// "E" = EV3 Motor
	String gaMotorType[10];

	// Allow vary speed for each Motor
	bool gaBolVSpeed[10];

	// Direction for each Motor
	int gaMDir[10];

	// Input X PIN for each Motor
	int gaInput1PIN[10];
	int gaInput2PIN[10];

	// Set Left / Right for each motor
	// "L" - means this motor use in turn Left
	// "R" - means this motor use in turn right
	// "N" - means this is a special purpose motor, it is not connected to a wheel, and won't be used to turn left / turn right / MoveForward / MoveBackward the vehicle.
	String gaLRN[10];

	// Motor Object
	MOTOR gaM[10];

	// Part B - Init
	void Init();

	// Part C - Functions

	// C1 - LED Related functions

	// Turn On an LED
	void LEDOn(int intLedNo);

	// Turn Off an LED
	void LEDOff(int intLedNo);

	// C11 - Motor Related functions

	// Move a Single Motor
	// intDir: 1 or -1
	// intSpeed: 0 to 255
	void Move(int intMotorNo, int intDir, int intSpeed);

	// Stop a Single Motor
	void Stop(int intMotorNo);

	// Move Robot
	// MoveRobot only move motors with gaLRN = L or R
	// floPowerL / floPowerR: -1.0 to 1.0, % of speed of the L/R wheel, if those wheels support variable speed, i.e. gbolVSpeed = true
	// for example:
	// floPowerL = 1, floPowerR = 1 means: Move Forward at full speed
	// floPowerL = 1, floPowerR = -1 means: Left Wheel go forward at full speed, Right Wheel go backward at full speed, i.e. Turn Right at full speed.
	// floPowerL = 1, floPowerR = 0.5 means: Left Wheel go forward at full speed, Right Wheel go forward at half speed, i.e. Go Forward And Right at the same time.
	void MoveRobot(float floPowerL, float floPowerR);

	// Stop Robot
	void StopRobot();

private:
	// Part D - Private Functions

	// D1 - LED Related Private Functions

	// Init. LED
	void InitLED();

	// D11 - Motor Related Private Functions

	// Init. Motor
	void InitMotor();
};
#endif

#include "ROBOTH.h"
#include "Arduino.h"

// Update the five pin assignments in the constructor below.
// The arguments are: enPin, dirPin, pwmPin, encoderPin1, encoderPin2
// There are a few considerations for pin assignments:
// A. pwmPin needs to be a pin with PWM capabilities (that is, it supports analogWrite)
//      Uno:       pins 3, 5, 6, 9, 10, and 11
//      Mega 2560: pins 2 - 13 and 44 - 46
// B. There are three ways to connect the encoder pins (labeled T1/T2 on the board).
// ** Best performance: Both signals are connected to true interrupt pins (listed below).
// ** Good performance: The FIRST signal (T1) is connected to an interrupt pin, the second signal is a regular pin. This is the mode used for the Bricktronics Shield/Megashield. For this mode it is CRITICAL that the true interrupt pin is used for T1 and not T2.
// ** Low performance: Both signals are connected to non-interrupt pins.
// Regardless of which performance mode used, you MUST list the pin T1 before T2 in
//   the constructor, otherwise the encoder will be connected backwards and the
//   PID algorithm will get all confused and freak out.
// Location of true interrupt pins:
//      Uno:       pins 2 and 3
//      Mega 2560: pins 2, 3, 21, 20, 19, and 18

// Since there are only 6 true interrupt pins, use Good Performance if there are 4 motors

// Bugs for a particular Arduino Mega 2560
// If mB use 34 as Dir pin and mC use 35 as Dir pin, mB always turn in same direction!!!!!
BricktronicsMotor BM[4] = {BricktronicsMotor(32, 38, 44, 2, 3), BricktronicsMotor(33, 35, 45, 20, 21), BricktronicsMotor(32, 38, 44, 2, 3), BricktronicsMotor(32, 38, 44, 2, 3)};

// Init Motor
void MOTOR::Init(int intMotorNo, String strMotorType, bool bolVSpeed, int intDir, int intInput1PIN, int intInput2PIN) {
	gstrMotorType = strMotorType;
	gbolVSpeed = bolVSpeed;
	gintDir = intDir;
	gintInput1PIN = intInput1PIN;
	gintInput2PIN = intInput2PIN;

	if (strMotorType == "D") {
		// Set Pins Output
		pinMode(gintInput1PIN, OUTPUT);
		pinMode(gintInput2PIN, OUTPUT);
	}
	else if (strMotorType == "E") {
		BM[intMotorNo - 1].begin();
	}
}

// Motor.Move
void MOTOR::Move(int intMotorNo, int intDir, int intSpeed) {
	// Calculate Final Direction of this Motor
	int intFinalDir = gintDir * intDir;
	
	if (gstrMotorType == "D") {
		// Move for DC Motor

		// Reset Speed if needed, since allowed value = 0 - 255
		if (intSpeed < 0) {
			intSpeed = 0 - intSpeed;
		}
		if (intSpeed > 255) {
			intSpeed = 255;
		}

		// Move Motor
		if (gbolVSpeed) {
			// Allow Different Speed
			if (intFinalDir == 1) {
				analogWrite(gintInput1PIN, intSpeed);
				digitalWrite(gintInput2PIN, LOW);
			}
			else {
				digitalWrite(gintInput1PIN, LOW);
				analogWrite(gintInput2PIN, intSpeed);
			}
		}
		else {
			// Use Maximum Speed;
			if (intFinalDir == 1) {
				digitalWrite(gintInput1PIN, HIGH);
				digitalWrite(gintInput2PIN, LOW);
			}
			else {
				digitalWrite(gintInput1PIN, LOW);
				digitalWrite(gintInput2PIN, HIGH);
			}
		}
	}
	else if (gstrMotorType == "S") {
		// Move for Step Motor
	}
	else if (gstrMotorType == "V") {
		// Move for Servo Motor
	}
	else if (gstrMotorType == "E") {
		// Move for EV3 Motor
		BM[intMotorNo-1].setFixedDrive(int(intSpeed * intFinalDir));
	}
}

// Motor.Stop
void MOTOR::Stop(int intMotorNo) {
	if (gstrMotorType == "D") {
		// Stop for DC Motor
		digitalWrite(gintInput1PIN, LOW);
		digitalWrite(gintInput2PIN, LOW);
	}
	else if (gstrMotorType == "S") {
		// Move for Step Motor
	}
	else if (gstrMotorType == "V") {
		// Move for Servo Motor
	}
	else if (gstrMotorType == "E") {
		// Move for EV3 Motor
		BM[intMotorNo - 1].brake();
	}
}

// Init Robot
void ROBOT::Init() {
	// Init LED, if any
	InitLED();
	// Init Motor
	InitMotor();
}

// Init LED, if any
void ROBOT::InitLED() {
	for (int i = 1; i <= gintNoOfLED; i++) {
		pinMode(garyLED[i - 1], OUTPUT);
	};
}

// Init Motor, if any
void ROBOT::InitMotor() {
	for (int i = 1; i <= gintNoOfMotor; i++) {
		if (gaMotorType[i - 1] == "D") {
			// Init DC Motor
			gaM[i - 1].Init(i, gaMotorType[i - 1], gaBolVSpeed[i - 1], gaMDir[i - 1], gaInput1PIN[i - 1], gaInput2PIN[i - 1]);
		}
		else if (gaMotorType[i - 1] == "E") {
			gaM[i - 1].Init(i, gaMotorType[i - 1], gaBolVSpeed[i - 1], gaMDir[i - 1], gaInput1PIN[i - 1], gaInput2PIN[i - 1]);
		}
	};
}

// LED Related functions

// LED On
void ROBOT::LEDOn(int intLedNo) {
	digitalWrite(garyLED[intLedNo - 1], HIGH);
}

// LED Off
void ROBOT::LEDOff(int intLedNo) {
	digitalWrite(garyLED[intLedNo - 1], LOW);
}

// Motor Related functions

// Move a Single Motor
void ROBOT::Move(int intMotorNo, int intDir, int intSpeed) {
	gaM[intMotorNo - 1].Move(intMotorNo, intDir, intSpeed);
}

// Stop a Single Motor
void ROBOT::Stop(int intMotorNo) {
	gaM[intMotorNo - 1].Stop(intMotorNo);
}

void ROBOT::MoveRobot(float floPowerL, float floPowerR) {
	int intSpeedL = abs((int)(255 * floPowerL));
	int intSpeedR = abs((int)(255 * floPowerR));
	int intDirL;
	int intDirR;
	if (floPowerL > 0)
		intDirL = 1;
	else
		intDirL = -1;
	if (floPowerR > 0)
		intDirR = 1;
	else
		intDirR = -1;

	for (int i = 1; i <= gintNoOfMotor; i++) {
		if (gaLRN[i - 1] == "L") {
			gaM[i - 1].Move(i, intDirL, intSpeedL);
		}
		else if (gaLRN[i - 1] == "R") {
			gaM[i - 1].Move(i, intDirR, intSpeedR);
		}
	}
}

void ROBOT::StopRobot() {
	for (int i = 1; i <= gintNoOfMotor; i++) {
		if (gaLRN[i - 1] == "L" || gaLRN[i - 1] == "R") {
			gaM[i - 1].Stop(i);
		}
	}
}

/*
Name:		lesson5.ino
Created:	11/27/2017 8:39:28 PM
Author:	Administrator
*/

// the setup function runs once when you press reset or power the board
#include "ROBOTH.h"

// Declare the ROBOT rbt1
ROBOT rbt1;

void setup() {
	// Set vars for LED

	// Set the no. of LED in this robot, if any
	rbt1.gintNoOfLED = 0;

	// Set the PIN numbers of those LEDs, if any
	// rbt1.garyLED[0] = 13;

	// Set vars for Motor

	// Set No. of Motor
	rbt1.gintNoOfMotor = 2;

	// Set Motor Type for each Motor
	// "D" for DC Motor
	// "E" for EV3 Motor
	rbt1.gaMotorType[0] = "E";
	rbt1.gaMotorType[1] = "E";

	// Set Vary Speed for each Motor
	rbt1.gaBolVSpeed[0] = false;
	rbt1.gaBolVSpeed[1] = false;

	// Set Direction for each Motor
	// If the direction of the Motor is not as expected, turn that value to -1
	rbt1.gaMDir[0] = -1;
	rbt1.gaMDir[1] = -1;

	// Set Input PINs for each DC Motor
	// For EV3 Motor, Set PINS in ROBOTH.cpp
	//rbt1.gaInput1PIN[0] = 32;
	//rbt1.gaInput2PIN[0] = 38;
	//rbt1.gaInput1PIN[1] = 33;
	//rbt1.gaInput2PIN[1] = 35;

	// Set which motor(s) for turn left/right/forward/backward
	rbt1.gaLRN[0] = "L";
	rbt1.gaLRN[1] = "R";

	// Init Robot
	rbt1.Init();
}

// the loop function runs over and over again until power down or reset
void loop() {
	// Both Motor Move forward
	rbt1.Move(1, 1, 128);
	rbt1.Move(2, 1, 128);
	// For 1 second
	delay(1000);
	// Both Motor Stop
	rbt1.Stop(1);
	rbt1.Stop(2);
	// For 1 second
	delay(1000);
	// Both Motor Move backward
	rbt1.Move(1, -1, 128);
	rbt1.Move(2, -1, 128);
	// For 1 second
	delay(1000);
	// Both Motor Stop
	rbt1.Stop(1);
	rbt1.Stop(2);
	// For 1 second
	delay(1000);
	// Robot Move Forward
	rbt1.MoveRobot(1.0, 1.0);
	// For 1 second
	delay(1000);
	// Robot Stop
	rbt1.StopRobot();
	// For 1 second
	delay(1000);
	// Robot Move Backward
	rbt1.MoveRobot(-1.0, -1.0);
	// For 1 second
	delay(1000);
	// Robot Stop
	rbt1.StopRobot();
	// For 1 second
	delay(1000);
	// Robot Turn Left at full speed
	rbt1.MoveRobot(-1.0, 1.0);
	// For 1 second
	delay(1000);
	// Robot Stop
	rbt1.StopRobot();
	// For 1 second
	delay(1000);
	// Robot Turn Right at half speed
	rbt1.MoveRobot(0.5, -0.5);
	// For 1 second
	delay(1000);
	// Robot Stop
	rbt1.StopRobot();
	// For 1 second
	delay(1000);
}