/*
	Tims_Electronic_Pantograph.ino
	By Tim Jackson.1960

	Creadits:
		Arduino.
		LiquidCrystal_I2C based on work by DFRobot.

	This is code for: Tim's Electronic Pantograph.
	The code is written for ATMEGA based boards, where the double is only 4 bytes.
	I am using 3D Printed Parts with two 49E Linear Hall Effect Sensor and two Magnets on each sensor.
	I am using an Arduino NANO to calculate the postion of the sylus from the values recived from the two 49E Linear Hall Effect Sensors.
	The sensor is linear: 3 mV/GS, but to get the angle a Sine of the value is calculated.

  Note!
  I am using pin A6 and A7 on the Arduino NANO.
  If using other boards, the Pin Assignment may need changing.

	S=O/H C=A/H T=O/A
	Degrees to Radians = degrees * (PI / 180)
	Radians to Degrees = radians * 180.0 / pi

*/

#include <Wire.h> 
#include <LiquidCrystal_I2C.h>

//#define DEBUG														//	Activates Serial Printing. (Comment out to turn off DBUG)
/*
	Pins
*/
#define ARM_LENGTH				200									//	Length between centres of Arms.
#define ENCODER_B				14									//	Pin A0	Encoder A Pin.
#define ENCODER_A				15									//	Pin A1	Encoder B Pin.
#define ENCODER_S				16									//	Pin A2	Encoder S Pin.
#define SYLUS_BUTTON			17									//	Pin A3	Sylus Button Pin.
//	18	A4 = SDA
//	19	A5 = SCL
#define Hall_49R_Pin_01			20									//	Pin A6	define Hall-Effect sensor Pin.
#define Hall_49R_Pin_02			21									//	Pin A7	define Hall-Effect sensor Pin.
/*
	Encoder/buttons
	To keep track of the state of switches.
*/
volatile byte seqA = 0;
volatile byte seqB = 0;
volatile byte cnt1 = 0;
volatile byte cnt2 = 0;
volatile bool positive = false;
volatile bool negative = false;
volatile bool button = false;
volatile bool sylus = false;
/*
	Sensors

	Note!
		During Calibration, if Values are the wrong way around (small / large), then magnets are the wrong way around.
*/
#define CAL_HALL_0D_01			107									//	Value created by NANO.
#define CAL_HALL_45D_01			237									//	Small Value of hall-effect sensor 1 at 45 angle. Move 1st Arm Up to the Right with stop on.
#define CAL_HALL_135D_01		834									//	Large Value of hall-effect sensor 1 at 135 angle. Move 1st Arm Down to Bottom Left with stop on.
#define CAL_HALL_180D_01		960									//	Value created by NANO.
#define CAL_HALL_RANGE_01		(CAL_HALL_180D_01 - CAL_HALL_0D_01)	//	Highest CAL - Lowest CAL.
#define CAL_HALL_RAD_01			((double)CAL_HALL_RANGE_01 / 2)		//	Used in the sine H value to get angle.
#define ANGLE_BIOS_01			1.4107								//	Angle correction at calibration point, after by NANO correction.
#define X_BIOS_01				0									//	X correction at calibration point.	163.830
#define Y_BIOS_01				0									//	Y correction at calibration point.	114.715
unsigned int Sensor_Value_01 = CAL_HALL_0D_01;						//	Variable to hold Sensor value.
double Hall_Angle_01 = 0;											//	Variable to hold Hall 1 Angle value.
double Hall_X_01 = 0;												//	Variable to hold Hall 1 X value.
double Hall_Y_01 = 0;												//	Variable to hold Hall 1 Y value.

#define CAL_HALL_0D_02			140									//	Value created by NANO.
#define CAL_HALL_45D_02			258									//	Small Value of hall-effect sensor 2 at 45 angle. Move away from 1st Arm with stop on.
#define CAL_HALL_135D_02		825									//	Large Value of hall-effect sensor 2 at 135 angle. Move close to 1st Arm with stop on.
#define CAL_HALL_180D_02		942									//	Value created by NANO.
#define CAL_HALL_RANGE_02		(CAL_HALL_180D_02 - CAL_HALL_0D_02)	//	Highest CAL - Lowest CAL.
#define CAL_HALL_RAD_02			((double)CAL_HALL_RANGE_02 / 2)		//	Used in the sine H value to get angle.
#define ANGLE_BIOS_02			1.5719								//	Angle correction at calibration point, after by NANO correction.
#define X_BIOS_02				0									//	X correction at calibration point.	114.715
#define Y_BIOS_02				0									//	Y correction at calibration point.	163.830
unsigned int Sensor_Value_02 = CAL_HALL_0D_02;						//	Variable to hold Sensor value.
double Hall_Angle_02 = 0;											//	Variable to hold Hall 2 Angle value.
double Hall_X_02 = 0;												//	Variable to hold Hall 2 X value.
double Hall_Y_02 = 0;												//	Variable to hold Hall 2 Y value.
/*
	Output
*/
double Sylus_X = 0;													//	Variable to hold Sylus X Coordinate.
double Sylus_Y = 0;													//	Variable to hold Sylus Y Coordinate.
double Length = 0;													//	Variable to hold Length value.
bool Pen_Down = true;												//	State of Pen.
#define CALL_X					93.572								//	Off-set of output X
#define CALL_Y					-33.339								//	Off-set of output Y

LiquidCrystal_I2C lcd(0x27, 16, 2);									//	(0x20 to 0x27)	Set to 0x27, 16 chars and 2 line display.
/*
	Timing
*/
unsigned long NowTime = millis();
unsigned long IntevalTime = 200;
unsigned long NextTime = NowTime + IntevalTime;

void  setup()
{

	Serial.begin(115200);					//	Start Serial.

	pinMode(Hall_49R_Pin_01, INPUT);		//	Define as Input.
	pinMode(Hall_49R_Pin_02, INPUT);		//	Define as Input.
	//pinMode(SYLUS_BUTTON, INPUT_PULLUP);  //  Define as Input, pulled high.
  pinMode(SYLUS_BUTTON, INPUT);  //  Define as Input, pulled high.
	pinMode(ENCODER_S, INPUT);				//	Define as Input.
	pinMode(ENCODER_B, INPUT);				//	Define as Input.
	pinMode(ENCODER_A, INPUT);				//	Define as Input.

	//00000|PCIE2|PCIE1|PCIE0
	PCICR = 0b00000010; // Pin Change Interrupt Enable.
	//PCMSK0=PCINT 7,6,5,4,3,2,1,0.(Pins= crystal, crystal, 13, 12, 11, 10, 9, 8)
	//PCMSK1=PCINT 14,13,12,11,10,9,8.(Pins= reset, A5,A4, A3, A2, A1, A0)
	//PCMSK2=PCINT 23,22,21,20,19,18,17,16.(Pins= 7, 6, 5, 4, 3, 2, 1, 0)
	PCMSK1 = 0b00001111; // Enable Pin Change Interrupt for A0, A1, A2, A3.
	sei();//enable interupts

	lcd.init();								//	Start LCD.
	lcd.backlight();						//	Turn on back light.
	lcd.setCursor(0, 0);					//	Set Cursor at the begining of line 0 (Top Line).
	lcd.print("X:");						//	Display Label for X on top line.
	lcd.setCursor(0, 1);					//	Set Cursor at the begining of line 1 (Bottom Line).
	lcd.print("Y:");						//	Display Label for Y on bottom line.
	Do_Stylus();							//	Display Pen State.	

#ifdef DEBUG
	lcd.setCursor(11, 0);					//	Set Cursor near the end of line 0 (top Line).
	lcd.print("DEBUG");						//	Display DEBUG on top line.
#endif // DEBUG

}
void  loop() {

	NowTime = millis();

	if (button || positive || negative) { Do_Menu(); }
	if (sylus) { Do_Stylus(); }

	Read_Sensor_Values();
	Calc_Angle_Hall_01();								//	Do sub routeen for angle first arm.
	Calc_Angle_Hall_02();								//	Do sub routeen for angle second arm.
	Calc_XY_Hall_01();									//	Do sub routeen for first Cooardinates.
	Calc_XY_Hall_02();									//	Do sub routeen for second Cooardinates.
	Calculate_Coordinates();

#ifdef DEBUG
	Calc_0_180();
#endif // DEBUG

	/*
		Send data at intervals, don't want to clog the Serial.
		Don't want it send this data when in DEBUG mode.
	*/
	if (NowTime > NextTime) {
		NextTime = NowTime + IntevalTime;
		Send_Data();
	}

}
ISR(PCINT1_vect) {

	// If interrupt is triggered by the button
	if (!digitalRead(ENCODER_S)) {
		button = true;
	}
	// If interrupt is triggered by the sylus
	else if (!digitalRead(SYLUS_BUTTON)) {
		sylus = true;
	}
	// Else if interrupt is triggered by encoder signals
	else {

		// Read A and B signals
		bool A_val = digitalRead(ENCODER_A);
		bool B_val = digitalRead(ENCODER_B);

		// Record the A and B signals in seperate sequences
		seqA <<= 1;
		seqA |= A_val;

		seqB <<= 1;
		seqB |= B_val;

		// Mask the MSB four bits
		seqA &= 0b00001111;
		seqB &= 0b00001111;

		// Compare the recorded sequence with the expected sequence
		if (seqA == 0b00001001 && seqB == 0b00000011) {
			cnt1++;
			positive = true;
		}

		if (seqA == 0b00000011 && seqB == 0b00001001) {
			cnt2++;
			negative = true;
		}
	}

}
/*
	Calculate 0 and 180 from Sector Quadrants

	H=O/S
	Degrees to Radians = degrees * (PI / 180)

	Segment_Chord = CAL_HALL_135D - CAL_HALL_45D
	Opp = Segment_Chord / 2
	Rad = Opp / Sin(45 * (PI / 180))
	Adjustment = Opp - Rad

	0_Deg = CAL_HALL_45D - Adjustment
	180_Deg = CAL_HALL_135D + Adjustment


*/
void Calc_0_180() {

	double Opp = (double)(CAL_HALL_135D_01 - CAL_HALL_45D_01) / 2.0;
	double Rad = (double)(Opp / sin(45 * (PI / 180)));
	double Adjustment = (double)(Rad - Opp);

	Serial.println();									//	Send a new line to serial to seperate values.
	Serial.print("Sensor 1 Calculated 0 value: ");		//	Send value to serial.
	Serial.println(CAL_HALL_45D_01 - Adjustment);		//	Send value to serial.
	Serial.print("Sensor 1 Calculated 180 value: ");	//	Send value to serial.
	Serial.println(CAL_HALL_135D_01 + Adjustment);		//	Send value to serial.

	Opp = (double)(CAL_HALL_135D_02 - CAL_HALL_45D_02) / 2.0;
	Rad = (double)(Opp / sin(45 * (PI / 180)));
	Adjustment = (double)(Rad - Opp);

	Serial.println();									//	Send a new line to serial to seperate values.
	Serial.print("Sensor 2 Calculated 0 value: ");		//	Send value to serial.
	Serial.println(CAL_HALL_45D_02 - Adjustment);		//	Send value to serial.
	Serial.print("Sensor 2 Calculated 180 value: ");	//	Send value to serial.
	Serial.println(CAL_HALL_135D_02 + Adjustment);		//	Send value to serial.

	Serial.println();									//	Send a new line to serial to seperate values.

}
/*
	Read Sensor Values

*/
void Read_Sensor_Values() {

	Sensor_Value_01 = 0;
	Sensor_Value_02 = 0;

	for (size_t i = 0; i < 10; i++)
	{
		Sensor_Value_01 += analogRead(Hall_49R_Pin_01);		//	Read the value from sensor.
		Sensor_Value_02 += analogRead(Hall_49R_Pin_02);		//	Read the value from sensor.
	}

	Sensor_Value_01 = Sensor_Value_01 / 10;
	Sensor_Value_02 = Sensor_Value_02 / 10;

#ifdef DEBUG
	Serial.println();									//	Send a new line to serial to seperate values.
	Serial.println(Sensor_Value_01);					//	Send value to serial.
	Serial.println(Sensor_Value_02);					//	Send value to serial.
	delay(1000);										//	Wait a little for things to happen and be able to read results.
#endif // DEBUG

}
/*
	Calculate the Hall Angle using Value from Sensor.

		S=O/H
		Radians to Degrees = radians * 180.0 / pi

		O = SensorValue - CAL_HALL_0D - CAL_HALL_RAD
		H = CAL_HALL_RAD
		Hall_Angle in radians = asin(O / H)
		Hall_Angle in degrees = Hall_Angle in radians * 180.0 / pi;

*/
void Calc_Angle_Hall_01() {

	double O = (double)Sensor_Value_01 - CAL_HALL_0D_01 - CAL_HALL_RAD_01;
	double H = (double)O / CAL_HALL_RAD_01;
	Hall_Angle_01 = 90.0 + (asin(H) * 180.0 / PI);

#ifdef DEBUG
	Serial.print("Hall_Angle_01 ");
	Serial.println(Hall_Angle_01 + ANGLE_BIOS_01, 4);
#endif // DEBUG

}
void Calc_Angle_Hall_02() {

	double O = (double)Sensor_Value_02 - CAL_HALL_0D_02 - CAL_HALL_RAD_02;
	double H = (double)O / CAL_HALL_RAD_02;
	Hall_Angle_02 = 90.0 + (asin(H) * 180.0 / PI);

#ifdef DEBUG
	Serial.print("Hall_Angle_02 ");
	Serial.println(Hall_Angle_02 + ANGLE_BIOS_02, 4);
#endif // DEBUG

}
/*
	Calculate the XY Position of the end of First Arm.
	Note!
		55 degrees is the amount I have off-set the first arm.
		This is to use the more accurate area in the real world.

	S=O/H C=A/H T=O/A
	Degrees to Radians = degrees * (PI / 180)

	S=O/H
	O = S*H
	Y = sin((Hall_Angle_01 - 55) * (PI / 180)) * ARM_LENGTH

	C=A/H
	A = C*H
	X = cos((Hall_Angle_01 - 55) * (PI / 180)) * ARM_LENGTH

 */
void Calc_XY_Hall_01() {

	Hall_X_01 = cos((Hall_Angle_01 + ANGLE_BIOS_01 - 55) * (PI / 180)) * ARM_LENGTH;
	Hall_Y_01 = sin((Hall_Angle_01 + ANGLE_BIOS_01 - 55.0) * (PI / 180)) * ARM_LENGTH;

#ifdef DEBUG
	Serial.print("Hall_X_01 ");
	Serial.println(Hall_X_01, 4);
	Serial.print("Hall_Y_01 ");
	Serial.println(Hall_Y_01, 4);
#endif // DEBUG

}
void Calc_XY_Hall_02() {

	Hall_X_02 = cos((Hall_Angle_02 + ANGLE_BIOS_02 + Hall_Angle_01 + ANGLE_BIOS_01 - 55) * (PI / 180)) * ARM_LENGTH;
	Hall_Y_02 = sin((Hall_Angle_02 + ANGLE_BIOS_02 + Hall_Angle_01 + ANGLE_BIOS_01 - 55.0) * (PI / 180)) * ARM_LENGTH;

#ifdef DEBUG
	Serial.print("Hall_X_02 ");
	Serial.println(Hall_X_02, 4);
	Serial.print("Hall_Y_02 ");
	Serial.println(Hall_Y_02, 4);
#endif // DEBUG

}
/*
	Calculate from bottom left of Drawing Area.

	First Arm pivot cooardinates:
		X = 100
		Y = 345

		X = 100 + Hall_X_01 + Hall_X_02
		Y = 345 - Hall_X_01 - Hall_X_02

*/
void Calculate_Coordinates() {

	Sylus_X = 100.0 + Hall_X_01 + Hall_X_02;
	Sylus_Y = 345.0 - Hall_Y_01 - Hall_Y_02;

	lcd.setCursor(3, 0);
	lcd.print("  ");
	lcd.setCursor(Xpos(3, Sylus_X), 0);
	lcd.print(Sylus_X, 3);

	lcd.setCursor(3, 1);
	lcd.print("  ");
	lcd.setCursor(Xpos(3, Sylus_Y), 1);
	lcd.print(Sylus_Y, 3);

#ifdef DEBUG
	Serial.print("Sylus_X ");
	Serial.println(Sylus_X, 4);
	Serial.print("Sylus_Y ");
	Serial.println(Sylus_Y, 4);
#endif // DEBUG

}
/*
	Calculate the Length using degrees calculated from Sensor.

		C=A/H
		A=C*H
		Degrees to Radians = degrees * (PI / 180)

		C = (180 - Hall_Angle_01 in degrees) / 2
		H = ARM_LENGTH
		A = C * H
		Length = A * 2

*/
void CalcLength() {

	//double _angle = (180.0 - Hall_Angle_01) / 2;
	//double _rad = _angle * (PI / 180);
	//double C = cos(_rad);
	//double H = ARM_LENGTH;
	//Length = C * H * 2;

	//Serial.print("Length ");
	//Serial.println(Length + ANGLE_BIOS_01, 4);

	//lcd.setCursor(3, 1);
	//lcd.print("    ");
	//lcd.setCursor(Xpos(Length + ANGLE_BIOS_01), 1);
	//lcd.print(Length + ANGLE_BIOS_01, 3);
	//lcd.print("    ");

}
/*
	This a function to calculate the position of value displayed on LCD.

		Check to see if value is hundreds, tens or single.
		To keep numbers alighned.
*/
byte Xpos(byte pos, byte number) {
	byte val = pos;
	if (number < 100) { val = pos + 1; }
	if (number < 10) { val = pos + 2; }
	if (number < 0) { val = pos - 1; }
	return val;
}
/*
	Menu when I do it.
*/
void Do_Menu() {

}
/*
	When Stylus button is pressed.
*/
void Do_Stylus() {

	//delay(200);	//	Uncomment if you don't have de-Bounce on the Sylus Switch
	sylus = false;
	Pen_Down = !Pen_Down;

	lcd.setCursor(12, 1);
	if (Pen_Down) { lcd.print("DOWN"); }
	else { lcd.print("UP  "); }

#ifdef DEBUG
	Serial.print("Pen_Down ");
	Serial.println(Pen_Down);
#endif // DEBUG

}
/*
	Send data to Serial Port.
	Want to send it fast, so it will be sent as raw bytes.

	Code is for ATMEGA based boards, where the double is only 4 bytes.

	Multiply by 1000 to get 3 decimal places as whole numbers.
	Max Dimention is 300mm
	so 300000 as binary = 00000100 10010011 11100000
	Needs 3 bytes for each dimention, total 6.
	Need another byte for thee bits, -X, -y and pen state.

*/
void Send_Data() {

	byte _switch = 0;
	if (Pen_Down) { bitSet(_switch, 0); }
	if (Sylus_X < 0) { bitSet(_switch, 1); }
	if (Sylus_Y < 0) { bitSet(_switch, 2); }

	unsigned long _sylus_X = abs(Sylus_X * 1000);
	unsigned long _sylus_Y = abs(Sylus_Y * 1000);

	byte _buff[7];
	_buff[0] = _switch;					//	0 0 0 0 0 -X -Y PenDown
	_buff[1] = (byte)(_sylus_X >> 0);	//	X LSB
	_buff[2] = (byte)(_sylus_X >> 8);	//	
	_buff[3] = (byte)(_sylus_X >> 16);	//	X MSB
	_buff[4] = (byte)(_sylus_Y >> 0);	//	Y LSB
	_buff[5] = (byte)(_sylus_Y >> 8);	//	
	_buff[6] = (byte)(_sylus_Y >> 16);	//	Y MSM

#ifdef DEBUG
	for (size_t i = 0; i < 9; i++) {

		Serial.print("_buff ");
		Serial.println(_buff[i]);

	}
#else
	Serial.write(_buff, 9);
	Serial.println();
#endif // DEBUG

}