/*
	This is a little sketch for using the new Servo Controller LU9685-20CU
	This is controling the LU9685-20CU via I2C via Serial.

	By Tim Jackson.1960 https://www.timsnet.co.uk/

	Use with Tim's LU9685-20CU Controller.
	Commands are sent to the Arduino via Serial, then passed on to the LU9685-20CU via I2C.

	Below is what I use for Visual Studio 2022
	It is for the Inteli-sence to be able to read Arduino Libraries.
	It is not needed for the Arduino IDE.
#include <Tims_Arduino_NANO.h>

	Below this is DEBUG and HAS_LCD
	Comment out HAS_LCD if you don't have an I2C LCD connected.
	DEBUG sends back Feedback to my controller.

	I2C_CONNECTION or UART_CONNECTION
	Only one ov thes should be active.


*/

#define DEBUG
#define HAS_LCD
#define I2C_CONNECTION
//#define UART_CONNECTION

#include <Wire.h>

#ifdef HAS_LCD
#include <LiquidCrystal_I2C.h>
#endif /*	HAS_LCD	*/

#ifdef UART_CONNECTION
#include <SoftwareSerial.h>
#endif /*	UART_CONNECTION	*/



/*  Serial  */
#define MAX_BYTES 24
byte buffer[MAX_BYTES];
byte i2cAddress = 128;
unsigned int no_data = 0;
unsigned int sofar = 0;
bool processingCommand = false;

/*	Soft Serial	*/
#ifdef UART_CONNECTION
const byte UART_RX = 7;
const byte UART_TX = 6;
SoftwareSerial UART_SoftSerial(UART_RX, UART_TX);
#endif /*	UART_CONNECTION	*/


/*	I2C LCD	*/
#ifdef HAS_LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);
#endif /*	HAS_LCD	*/


/*  One Time Run    */
void setup() {

	/* Start the serial port */
#ifdef UART_CONNECTION
	Serial.begin(9600);
#else
	Serial.begin(115200);
#endif /*	UART_CONNECTION	*/



	/* Start the Wire Comunication */
	Wire.begin();

	/*	Soft Serial for UART Connection	*/
#ifdef UART_CONNECTION
	/*	Define pin modes for TX and RX	*/
	pinMode(UART_RX, INPUT);
	pinMode(UART_TX, OUTPUT);

	/*	Start the SoftwareSerial object at 9600 baud rate	*/
	UART_SoftSerial.begin(9600);
#endif /*	UART_CONNECTION	*/


	/*	I2C LCD	*/
#ifdef HAS_LCD
	lcd.init();
	lcd.backlight();
	lcd.print("  LU9685-20CU   ");
#endif /*	HAS_LCD	*/

}

/*  Constant Run    */
void loop() {

	if (!processingCommand) {
		ReadSerial();
	}

}

/* Read from Serial */
void ReadSerial() {
	byte c;

	if (Serial.available() > 0) {
		c = Serial.read();
		no_data = 0;

		if (c != '\n' && c != '\r') {
			buffer[sofar] = c;
			sofar++;
		}
	}
	else {
		no_data++;
		delayMicroseconds(100);

		if (sofar && (c == '\n' || c == '\r' || no_data > 100)) {
			no_data = 0;

			processingCommand = true;
#ifdef I2C_CONNECTION
			Send_I2C();
#endif /*	I2C_CONNECTION	*/

#ifdef UART_CONNECTION
			Send_UART();
#endif /*	UART_CONNECTION	*/


			Initiate_Serial();
		}
	}
}

/* Initialize Serial */
void Initiate_Serial() {
	for (byte i = 0; i < MAX_BYTES; i++) {
		buffer[i] = 0;
	}
	sofar = 0;
	Serial.println("ok");
	Serial.flush();
	processingCommand = false;
}

/* Write the data bytes to the I2C bus */
#ifdef I2C_CONNECTION
void Send_I2C() {

	i2cAddress = buffer[0];

#ifdef HAS_LCD
	lcd.setCursor(0, 1);/* bottom line	*/
	lcd.print("                ");
	lcd.setCursor(0, 1);/* bottom line	*/
#endif // HAS_LCD

#ifdef DEBUG
	for (size_t i = 1; i < sofar; i++) {
		Serial.print(buffer[i], DEC);
		Serial.print(" ");

#ifdef HAS_LCD
		lcd.print(buffer[i], DEC);
		lcd.print(" ");
#endif // HAS_LCD

	}

#endif // DEBUG


	if (sofar) {
		Wire.beginTransmission(i2cAddress);
		Wire.write(buffer + 1, sofar - 1);  /*  Skip the first element in buffer    */
		Wire.endTransmission();
	}
}
#endif /*	I2C_CONNECTION	*/

/* Write the data bytes to the UART Soft Serial */
#ifdef UART_CONNECTION
void Send_UART() {

#ifdef HAS_LCD
	lcd.setCursor(0, 1);/* bottom line	*/
	lcd.print("                ");
	lcd.setCursor(0, 1);/* bottom line	*/
#endif /*	HAS_LCD	*/

	if (sofar) {

		for (size_t i = 0; i < sofar; i++) {

#ifdef DEBUG
			Serial.print(buffer[i], DEC);
			Serial.print(" ");
#endif /*	DEBUG	*/

#ifdef HAS_LCD
			lcd.print(buffer[i], DEC);
			lcd.print(" ");
#endif /*	HAS_LCD	*/

			UART_SoftSerial.write(buffer[i]);

		}
	}
}
#endif /*	UART_CONNECTION	*/