#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include <termios.h>
#include <fcntl.h>

#define LOOP_RATE 20 //Hz
#define QUEUE_SIZE 64 //subscriber buffer size


// -------------- Serial Port -----------------
int fd;
bool isConnected = false;
const char *port0 = "/dev/ttyACM0"; //red
//const char *port1 = "/dev/ttyACM1"; //yellow

int connectit(const char *dev, int baud);
static  void initTermios(int fd, uint32_t baud);
struct termios sTermios;
int writecommand(char speed, char dir, bool mode);


// --------------- Fish ID --------------------
const unsigned char prefix[3]   = { 0x21, 0x43, 0x00};
const unsigned char fish_ID[9]    = { 0x52, 0x71, 0xa7, 0x29, 0xbc, 0xd6, 0xd9, 0x00, 0xe5}; //red
//const unsigned char fish_ID[9] = { 0x52, 0x71, 0x63, 0x4c, 0xb9, 0xd6, 0xda, 0x00, 0xe5}; //yellow


// global vars
float lin_vel = 0;
float ang_vel = 0;


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
*
* Twist message Callback
*
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void velCallback (const geometry_msgs::Twist::ConstPtr& vel_msg) {

	lin_vel = vel_msg->linear.x;	//Get linear velocity from Twist message.
	ang_vel = vel_msg->angular.z;	//Get the direction from the Twist message. This is not a speed in reality, but steering action.
}




/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
*
* initTermios.	Serial I/O
*
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
static void initTermios(int fd, uint32_t baud) 
{
    tcgetattr(fd, &sTermios); /*Get the attributes set by default*/
     
    /* ---------- Serial Port Config ------- */
    sTermios.c_cflag |=(CLOCAL|CREAD);  
    sTermios.c_cflag &= ~(CSTOPB|CSIZE); 
    sTermios.c_cflag &= ~(PARENB);
    sTermios.c_cflag |= CS8;       
    sTermios.c_lflag &= ~(ICANON|ECHO|ECHOE|ISIG); 
    sTermios.c_cc[VMIN]  = 0;     
    sTermios.c_cc[VTIME] = (unsigned int)(100);
    sTermios.c_cc[VSTART] = 0x13;  
    sTermios.c_cc[VSTOP] = 0x19; 
 
    sTermios.c_iflag &= ~(IXON|IXOFF|IXANY|ICRNL|INLCR);
    sTermios.c_oflag &= ~OPOST;    
    cfsetispeed(&sTermios, baud);  
    cfsetospeed(&sTermios, baud);  
    /* -------- Serial Port Config END---- */
 
    tcsetattr(fd, TCSANOW, &sTermios);
}
 



/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
*
* connectit.	Open serial port
*
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
int connectit(const char *dev, int baud) {
  int fd;
  fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
  if (fd == -1) { perror("serconnect "); return fd; }
  initTermios(fd, baud);
  return fd;
}



/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
*
* writecommand
*
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
int writecommand(char speed, char dir, bool mode) {

	char msg[15];	
	if(!isConnected) return -1;

	if(dir > 8)  dir = 8;
	if(dir < -7) dir = -7;

	if(speed > 15)  speed = 15;
	if(speed < 0)   speed = 0;

	char direction = dir + 7;
	char modeH = mode ? 0x10 : 0x00;
	char dirMode = direction + modeH; 
	char checksum = 0xff - speed - direction - modeH;
	for(int i=0;i<3;i++){
		msg[i] = prefix[i];
	}
	msg[3] = speed;
	msg[4] = dirMode;
	msg[5] = checksum;
	for(int i=0;i<9;i++){
		msg[6+i] = fish_ID[i];		
	}
	return write(fd, msg, 15);	
	//return 0;
}




/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
*
* Main
*
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
int main(int argc, char **argv)
{
	bool mode=false; //purpose?
	float x_move=0;
	float y_move=2;
	int direction=0;
	int speed=0;
	int prevDirection=0;
	int prevSpeed=0;

	fd = connectit(port0,115200);
   	if (fd != -1) {
		isConnected = true;
		ROS_INFO("Serial port ttyACM0 connected...");
		sleep(1); 
  		tcflush(fd,TCIOFLUSH);
   	}
	else {
		ROS_ERROR("Error!: Serial port ttyACM0 not found!");
	}

	ros::init(argc, argv, "red_rofish_telemetry");
	ros::NodeHandle nh;
	ros::Subscriber sub = nh.subscribe("red/cmd_vel", QUEUE_SIZE, velCallback);
	ros::Rate loop_rate(LOOP_RATE);

	while(ros::ok()) {
		ros::spinOnce();

		//do processing here
		if (ang_vel >= 0) {
			direction = (char) 8*ang_vel;
		}
		if (ang_vel < 0) {
			direction = (char) 7*ang_vel;
		}
		if (lin_vel >= 0){
			speed = (char) 15*lin_vel;
		}

		writecommand(speed, direction, mode);

		if( (direction!=prevDirection) || (speed!=prevSpeed) ){
			ROS_INFO("direction=%d, speed=%d\n", direction, speed);
		}
		prevDirection = direction;
		prevSpeed = speed;
		
		loop_rate.sleep();
	}
return 0; //never reaches here
}