// UART FOR RS485 CONVERTER

#define RXD2 16
#define TXD2 17
// UART FOR HMI
#define RXD1 4
#define TXD1 2
// HAND WHEEL PULSE ENCODER
#define CLK_ENCODER 27
#define DT_ENCODER 25
// HOME SENSOR
#define HOME_SENSOR_J1 21 // 39
#define HOME_SENSOR_J2 22 // 36
#define HOME_SENSOR_J3 34 // 35
#define HOME_SENSOR_J4 35 // 34
#define HOME_SENSOR_J5 36 // 22
#define HOME_SENSOR_J6 39 // 21
// BUZZER
#define BUZZER 5
// RELAY
#define RELAY_5 32
#define RELAY_6 33
// BUTTON TEACH PENDANT
#define RED_BUTTON 15
#define GREEN_BUTTON 26
// VSPI FOR ETHERNET MODULE AND SD CARD
#define REASSIGN_PINS
int sck = 18;
int miso = 19;
int mosi = 23;
int cs = 14; // Chip Select Pin for SD CARD
int cs_ethernet = 13; // Chip Select Pin for ETHERNET W5500

// NO USE
// #define RELAY_1 13
// #define RELAY_2 18
// #define RELAY_3 19
// #define RELAY_4 23
// #define SW_ENCODER

//===========================================================================
// DEFINE SERVO ID
//===========================================================================
#define ID_1 1
#define ID_2 2
#define ID_3 3
#define ID_4 4
#define ID_5 5
#define ID_6 6
//===========================================================================
// DEFINE STATE
//===========================================================================
#define ON_SERVO 1
#define OFF_SERVO 0
#define DELAY_TICK_ENABLE_SERVO 100
#define DELAY_TICK_DISABLE_SERVO 100
#define ACCELERATION_TIME_PARAMETER 3
#define DECELERATION_TIME_PARAMETER 4
#define NULL_POS -1
#define RETURN_OK 0
#define CW 1
#define CCW 0
//===========================================================================
// DEFINE TCP STASK
//===========================================================================
#define TCP_TASK_POS_1 1
#define TCP_TASK_POS_2 2
#define TCP_TASK_POS_3 3
#define TCP_TASK_POS_4 4
#define TCP_TASK_POS_5 5
#define TCP_TASK_POS_6 6
#define TCP_TASK_POS_TG_1 7
#define TCP_TASK_POS_TG_2 8
#define TCP_TASK_HOME 9
#define TCP_TASK_CALIB 10
#define TCP_TASK_WAKEUP 11
#define TCP_TASK_SLEEP 12
#define TCP_TASK_ON_SERVO 13
#define TCP_TASK_OFF_SERVO 14
#define TCP_TASK_STOP 15
//===========================================================================
// DEFINE HARDWARE_SERIAL
//===========================================================================
HardwareSerial HMI(1);;
//===========================================================================
// DEFINE VARIABLES
//===========================================================================
struct StructRow
{
unsigned int Mark_Array[101] = {};
unsigned int rowNo;
String posName;
float EEx_Value;
float EEy_Value;
float EEz_Value;
String EE_Orientation;
unsigned int moveTimeValue;
unsigned int waitTimeValue;
String out1State;
String out2State;
struct EEC_R06
{
float r11, r12, r13;
float r21, r22, r23;
float r31, r32, r33;
} EEC_R06_Matrix;
};
StructRow Row_Data_T1;
StructRow Row_Data_T2;
StructRow Row_Data_T3;

struct Struct_Fixed_Modes
{
struct Home_Mode
{
// HOME THETA
const int Theta1 = 0, Theta2 = 155, Theta3 = -65, Theta4 = 0, Theta5 = -60,
Theta6 = 0;
} Home;
 struct Wakeup_Mode
{
// WAKE UP THETA
const int Theta1 = 0, Theta2 = 90, Theta3 = 0, Theta4 = 0, Theta5 = -90,
Theta6 = 0;
} Wakeup;
struct Sleep_Mode
{
// SLEEP THETA
const int Theta1 = 0, Theta2 = 123, Theta3 = -33, Theta4 = 0, Theta5 = -90,
Theta6 = 0;
} Sleep;
} Fixed_Modes;

// FOR KINEMATICS
float d0 = 228,
L1 = 55, L2 = 330, L3 = 62, d1 = 330, d2 = 201;
float POS = 90, NEG = -90;
float theta1 = 0, theta2 = 0, theta3 = 0, theta4 = 0, theta5 = 0, theta6 = 0;
float theta1_old = 0, theta2_old = 0, theta3_old = 0, theta4_old = 0, theta5_old =
0, theta6_old = 0;
float theta1_FK = 0, theta2_FK = 0, theta3_FK = 0, theta4_FK = 0, theta5_FK = 0,
theta6_FK = 0;
float theta1_IK = 0, theta2_IK = 0, theta3_IK = 0, theta4_IK = 0, theta5_IK = 0,
theta6_IK = 0;
float theta1_IK_T = 0, theta2_IK_1 = 0, theta2_IK_2 = 0, theta3_IK_1 = 0,
theta3_IK_2 = 0, theta5_IK_1 = 0, theta5_IK_2 = 0;
float tolerance = 2;
int Number_Of_Solutions = 0;
int i = 0;

// TEMPLATE MATRIX
template <int rows, int cols>
using MatrixSize = Eigen::Matrix<float, rows, cols>;

// HAND WHEEL PULSE ENCODER

long newPosition = 0;
long lastPosition = 0;
Encoder Teach_Pendant_Encoder;
int Encoder_Position_Diff = 0;

// SYSTEM STATUS [false: OFF | true: ON]

bool SYSTEM_STATUS = false;

// OUT RELAY STATUS [Out1 5V - Out2 24V] [false: OFF | true: ON]
bool OUT1_STATUS = false;
bool OUT2_STATUS = false;
// UART FOR HMI
// const int BUFFER_SIZE = 100; // Độ dài tối đa của chuỗi
// char HMI_BUFFER[BUFFER_SIZE];
// int bufferIndex = 0;
bool newDataReceived = false;
String HMI_BUFFER_DATA = "";
String CURRENT_PAGE = "HP";

// FOR MOVE SERVO

long POS_SERVO_1 = 0, POS_SERVO_2 = 0, POS_SERVO_3 = 0, POS_SERVO_4 = 0,
POS_SERVO_5 = 0, POS_SERVO_6 = 0;
long VEL_SERVO_1 = 0, VEL_SERVO_2 = 0, VEL_SERVO_3 = 0, VEL_SERVO_4 = 0,
VEL_SERVO_5 = 0, VEL_SERVO_6 = 0;
unsigned int Speed_Selection_Mode = 1;
unsigned int Speed = 1;
unsigned int Speed_1 = 1;
unsigned int Speed_2 = 1;
unsigned int Speed_3 = 1;
unsigned int Speed_4 = 1;
unsigned int Speed_5 = 1;
unsigned int Speed_6 = 1;
float Encoder_Pulse_Resolution = 0;
float Axis_Resolution = 0;
unsigned int ACCEL_TIME = 500; // Thời gian tăng tốc ms
unsigned int DECEL_TIME = 500; // Thời gian giảm tốc (ms);
unsigned int Speed_Find_Home = 8000; // Tốc độ tìm home (để quay về gần cảm
biến và tiến hành dò điểm giữa cảm biến home);
unsigned int Speed_Return_Origin = 12000; // Tốc độ quay về gốc khi xác định đã bị
quay ngược hướng lúc tìm home
unsigned int Speed_Find_Origin = 4000; // Tốc độ đi lấy vị trí ở 2 hướng tại cảm
biến home
unsigned int Speed_Move_Origin = 2000; // Tốc độ quay về điểm chính giữa của cảm
biến khi biết vị trí 2 chiều quay tại cảm biến
// FOR ROBOT HARDWARE
unsigned int Pulse_Per_Revolution_PPR = 10000;
float Gearbox_1_Ratio = 50; // Gearbox
float Gearbox_2_Ratio = 105.6; // 2.4*44 (Puly*Gearbox);
float Gearbox_3_Ratio = 47.2; // Gearbox
float Gearbox_4_Ratio = 100;
float Gearbox_5_Ratio = 90; // 2*45 (Puly*Gearbox);
float Gearbox_6_Ratio = 50; // Gearbox

// OTHER
String Theta_Selection = "";
String Axis_Selection = "";
String EE_Selection = "";
MatrixSize<4, 4> FK_Jog;
MatrixSize<3, 3> R_0_6_Temp;
MatrixSize<3, 1> EE_Target_Matrix;
float THETA2_OFFSET = 90;
float theta1_Init = 0, theta2_Init = 0, theta3_Init = 0, theta4_Init = 0,
theta5_Init = 0, theta6_Init = 0;
float slider1_target = 0, slider2_target = 0, slider3_target = 0, slider4_target =
0, slider5_target = 0, slider6_target = 0;
bool EN_READY = false;
bool DONE_ALM = false;
bool FIND_NEAREST_SOLUTION = false;
float Theta1_IK_READY = 0, Theta2_IK_READY = 0, Theta3_IK_READY = 0,
Theta4_IK_READY = 0, Theta5_IK_READY = 0, Theta6_IK_READY = 0;
unsigned int Run_Time_Target = 10;
String IK_Result = "";
float EEx_Target = 0;
float EEy_Target = 0;
float EEz_Target = 0;
unsigned int row_begin = 1;
unsigned int Table_No = 1;
unsigned int Max_Table_No = 10;
String Table_Name = "Jog Program";

bool ETH_Connect = false, L_State = false, D_State = false;

unsigned long interval = 500, previous_Time = 0, previous_Time_D = 0, current_Time
= 0;

const int HREG1_DATA_WRITE = 0;

const int HREG2_DATA_READ = 1;
int16_t TCP_DATA = 0;
unsigned int Run_Mark = 0;
#define MODBUSIP_PORT 502
byte MAC_Address[] = {0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED};
IPAddress IP_Address(192, 168, 0, 20);;
IPAddress Gateway_Address(192, 168, 0, 10);;
IPAddress Subnet_Mask(255, 255, 255, 0);;
IPAddress DNS_Address = Gateway_Address;

// ModbusIP object
ModbusIP Modbus;
EthernetClient client;

String splitString(String str, String delim, uint16_t pos);

String roundFloatToString(float value, unsigned int decimalPlaces);
String convertBytesToHexString(byte bytes[]);
float roundToNearestHalfDeg(float degree_target);

void Read_HMI();
void Write_End_Byte_HMI();
void Write_HMI(String page, String object, String para, String content);
void Write_HMI(String page, String object, String para, unsigned int value);
void Write_HMI(String object, String para, String content);
void Write_HMI(String object, String para, unsigned int value);
void Write_HMI_Val(String object, String para, float value);
void Alarm_HMI(String alarm_Content);
void Update_Row_Data_HMI(unsigned int row_update);
void Update_Row_Null_HMI(unsigned int row_update);
void Clear_Data_Table();
void Update_Data_HMI();
void clearBufferData();
void TCP_Led();
void LINK_Led();
void Update_Data_Table(unsigned int row_begin, unsigned int row_end);
void Mark_Row(unsigned int row_update, bool runState);

void ENABLE_ALL_SERVO();
void DISABLE_ALL_SERVO();
void CLEAR_ALL_POSITION();
float GET_THETA_1();
float GET_THETA_2();
float GET_THETA_3();
float GET_THETA_4();
float GET_THETA_5();
float GET_THETA_6();
float GET_SPEED_1();
float GET_SPEED_2();
float GET_SPEED_3();
float GET_SPEED_4();
float GET_SPEED_5();
float GET_SPEED_6();
void SET_ALL_ACCELERATION_TIME(unsigned int accel_time);
void SET_ALL_DECELERATION_TIME(unsigned int decel_time);
void RESET_ALL_ALARM();
void SAVE_ALL_PARAMETER();

template <int rows, int cols>

void printMatrix(const MatrixSize<rows, cols> &matrix);
MatrixSize<4, 4> T_0_1(float a1, float alpha1, float d1, float theta1);
MatrixSize<4, 4> T_1_2(float a2, float alpha2, float d2, float theta2);
MatrixSize<4, 4> T_2_3(float a3, float alpha3, float d3, float theta3);
MatrixSize<4, 4> T_3_4(float a4, float alpha4, float d4, float theta4);
MatrixSize<4, 4> T_4_5(float a5, float alpha5, float d5, float theta5);
MatrixSize<4, 4> T_5_6(float a6, float alpha6, float d6, float theta6);
MatrixSize<4, 4> Forward_Kinematics(float theta1_FK, float theta2_FK, float
theta3_FK, float theta4_FK, float theta5_FK, float theta6_FK);
bool Check_T_0_3(float theta1, float theta2, float theta3, float Wx, float Wy,
float Wz);
bool Check_T_0_6(float theta1, float theta2, float theta3, float theta4, float
theta5, float theta6, float EEx, float EEy, float EEz);
MatrixSize<3, 3> R_0_3_Inverse(float theta1, float theta2, float theta3);
String Inverse_Kinematics(const MatrixSize<3, 1> &EE_Pos_Matrix, const
MatrixSize<3, 3> &R_0_6_Matrix);
String Inverse_Kinematics_No_Debug(const MatrixSize<3, 1> &EE_Pos_Matrix, const
MatrixSize<3, 3> &R_0_6_Matrix);

void MOVE_ORIGIN_SERVO_1();
void MOVE_ORIGIN_SERVO_2();
void MOVE_ORIGIN_SERVO_3();
void MOVE_ORIGIN_SERVO_4();
void MOVE_ORIGIN_SERVO_5();
void MOVE_ORIGIN_SERVO_6();
void Joint_1_Calibration();
void Joint_2_Calibration();
void Joint_3_Calibration();
void Joint_4_Calibration();
void Joint_5_Calibration();
void Joint_6_Calibration();
void Robot_Calibration();

void Update_Home_Sensor_State();
void Update_Encoder_State();
void Update_Speed_Selection();
void Update_Slider_State();
void Update_Current_Page_Fcns();
void Update_System_Status();
void Update_Out_Relay_Status();

void Add_Row_Data_To_File(unsigned int rowNo);

void Add_HMI_Data_To_Struct();
void Add_SD_Data_To_Struct(String rowData);
void Edit_Data_To_Struct();
long DEG_TO_POS_1_CONV(float degree_target);
long DEG_TO_POS_2_CONV(float degree_target);
long DEG_TO_POS_3_CONV(float degree_target);
long DEG_TO_POS_4_CONV(float degree_target);
long DEG_TO_POS_5_CONV(float degree_target);
long DEG_TO_POS_6_CONV(float degree_target);
unsigned int SPEED_SERVO_1(float degree_target, float degree_target_old, unsigned
int time_target);
unsigned int SPEED_SERVO_2(float degree_target, float degree_target_old, unsigned
int time_target);
unsigned int SPEED_SERVO_3(float degree_target, float degree_target_old, unsigned
int time_target);
unsigned int SPEED_SERVO_4(float degree_target, float degree_target_old, unsigned
int time_target);
unsigned int SPEED_SERVO_5(float degree_target, float degree_target_old, unsigned
int time_target);
unsigned int SPEED_SERVO_6(float degree_target, float degree_target_old, unsigned
int time_target);

void Run_Robot_Position_Table();
void Run_Robot_TCP_Mode(int16_t table_target);
template <typename Fixed_Modes_Fcns>
void Move_Robot_Fixed_Modes(const Fixed_Modes_Fcns &fixed_modes, unsigned int
speed);
void Check_And_Run_Robot_IK_ENC();
void Check_And_Run_Robot_IK_SLI();
void Check_Stop_Robot_TCP_Mode();
void Run_Robot_To_Target_Base_Mode(String move_Mode);
void Move_Change_EE_Orientatation_Base_Mode_Page();
void Check_Move_Change_EE_Orientation_Base_Mode();

void Check_Change_Page_Fcns();
void Home_Page();
void Kinematic_Slider_Mode_Page();
void Kinematic_Encoder_Mode_Page();
void Select_EE_Orientation_Page();
void Base_Mode_Page();
void Position_Table_Page();
void TCPIP_Mode_Page();
void Debug_Page();

void SET_ALL_POS_INIT(float theta1_init, float theta2_init, float theta3_init,

float theta4_init, float theta5_init, float theta6_init);
void Initialization_Fcns();
void setup();

void loop();