/*
 *  Copyright (c) 2013 Errante, Tonetto, Edotti, Simonetti
 */

/*
 *  Serial mouse3d driver for Linux
 */

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 */

#include <linux/delay.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/interrupt.h>
#include <linux/input.h>
#include <linux/serio.h>
#include <linux/init.h>

#define DRIVER_DESC	"Serial mouse3d driver"

/* Input and Output Buffer size to keep track of previous data from the board */

#define BUF_SIZE 8
#define IN_BUF_SIZE 2
#define OUT_BUF_SIZE 2

/* Sensitivity for x, y and z translations: parameters decided by user feedback */

#define Z_TRANSLATION_SENSITIVE (10 << 3)	//fixed point notation for filter implementation
#define Y_TRANSLATION_SENSITIVE (10 << 5)	
#define X_TRANSLATION_SENSITIVE (10 << 3)	

/* Sensitivity for yaw, pitch and roll rotations: parameters decided by user feedback */

#define YAW_ROTATION_SENSITIVE (10 << 7)    	//fixed point notation for filter implementation
#define PITCH_ROTATION_SENSITIVE (10 << 8)    
#define ROLL_ROTATION_SENSITIVE (10 << 6)    

/* Offsets to improve data operating area */

#define Y_OFFSET 4
#define X_OFFSET 4
#define Z_OFFSET 6                      
#define YAW_OFFSET 7			
#define PITCH_OFFSET 6	        
#define ROLL_OFFSET 6			
#define COUNT_LIMIT 10

MODULE_AUTHOR("Errante, Tonetto, Edotti, Simonetti");
MODULE_DESCRIPTION(DRIVER_DESC);
MODULE_LICENSE("GPL");

static const char *sermouse_protocols[] = { "Mouse3d"};

/* Mouse structure modified with two addicted buffer */

struct sermouse {
	struct input_dev *dev;
	unsigned char buf[BUF_SIZE];
	int inbuf[6][IN_BUF_SIZE];	// fixed point: 22 bit (int), 10 bit (decimal)
	int outbuf[6][OUT_BUF_SIZE];	// fixed point: 22 bit (int), 10 bit (decimal)
	int countbuf[6];
	unsigned char count;
	unsigned char type;
	unsigned long last;
	char phys[32];
};

static void sermouse_process_3d(struct sermouse *, unsigned char );
unsigned char conv2byteto1(unsigned char, unsigned char );
void restore_buffer_data(unsigned char *, int);
void save_input_data(unsigned char *buf, int inbuf[][IN_BUF_SIZE], int buffer_size);
void shift_output_data(int outbuf[][OUT_BUF_SIZE]);


/*
 * sermouse_process_3d() analyzes the incoming Sparkfun 6DOF Atomic bytestream and
 * applies some prediction to the data.
 */
static void sermouse_process_3d(struct sermouse *sermouse, unsigned char data)
{
	struct input_dev *dev = sermouse->dev;
	unsigned char *buf = sermouse->buf;
	int (*inbuf)[IN_BUF_SIZE] = sermouse->inbuf;	
	int (*outbuf)[OUT_BUF_SIZE] = sermouse->outbuf;	
	int *countbuf = sermouse->countbuf;	

	switch (sermouse->count) {

		/* detect 'A' */
		case 0:
			if (data == 'A')
		           sermouse->count = 1;
			break;

		/* !NOT! save IMU counter */
		case 1:
			buf[7] = data;
			sermouse->count++;
			break;
		case 2:
			conv2byteto1(buf[7], data);
			sermouse->count++;
			break;

		/* save x_acceleration */
		case 3:
			buf[7] = data;
			sermouse->count++;
			break;
		case 4:
			buf[0] = conv2byteto1(buf[7], data);
			sermouse->count++;
			break;

		/* save y_acceleration */
		case 5:
			buf[7] = data;
			sermouse->count++;
			break;
		case 6:
			buf[1] = conv2byteto1(buf[7], data);
			sermouse->count++;
			break;

		/* save z_acceleration */
		case 7:
			buf[7] = data;
			sermouse->count++;
			break;
		case 8:
			buf[2] = conv2byteto1(buf[7], data);
			sermouse->count++;
			break;

		/* save pitch_acceleration */
		case 9:
			buf[7] = data;
			sermouse->count++;
			break;
		case 10:
			buf[3] = conv2byteto1(buf[7], data);
			sermouse->count++;
			break;

		/* save roll_acceleration */
		case 11:
			buf[7] = data;
			sermouse->count++;
			break;
		case 12:
			buf[4] = conv2byteto1(buf[7], data);
			sermouse->count++;
			break;

		/* save yaw_acceleration */
		case 13:
			buf[7] = data;
			sermouse->count++;
			break;
		case 14:
			buf[5] = conv2byteto1(buf[7], data);
			sermouse->count++;
			break;

		/* detect 'Z' */
		case 15:
			if (data == 'Z'){
				save_input_data(buf,inbuf,BUF_SIZE);
				shift_output_data(outbuf);

				/* y_translation */

				/* Digital filter comparing current and previous data with data offset*/
				if (inbuf[1][0] -inbuf[1][1] > Y_OFFSET || inbuf[1][0] -inbuf[1][1] < -Y_OFFSET)
				{
					outbuf[1][0]=outbuf[1][1]+(inbuf[1][0]+inbuf[1][1]);	
					countbuf[1] = 0;
				}
				else
				{ 
					countbuf[1]++;
					/*Set outbuf to zero when the signal from the board is stable*/	
					if (countbuf[1] > COUNT_LIMIT)
						outbuf[1][1] = 0;
					outbuf[1][0]=outbuf[1][1];
				}
				/*Send signal to press key_down button and stop key_up button pressure */
				if (outbuf[1][0] < -Y_TRANSLATION_SENSITIVE){				
					input_report_key(dev, KEY_DOWN, 1);
					input_report_key(dev, KEY_UP, 0);
					input_sync(dev);	
				}					

				else if (outbuf[1][0] > Y_TRANSLATION_SENSITIVE){				
					input_report_key(dev, KEY_UP, 1);
					input_report_key(dev, KEY_DOWN, 0);
					input_sync(dev);	
				}
				else {
					input_report_key(dev, KEY_DOWN, 0);
					input_report_key(dev, KEY_UP, 0);
					input_sync(dev);
				}

				/* z_translation */

				if (inbuf[2][0] - inbuf[2][1] > Z_OFFSET || inbuf[2][0] - inbuf[2][1] < -Z_OFFSET)
				{
					outbuf[2][0]=outbuf[2][1]+(inbuf[2][0]+inbuf[2][1]);	
					countbuf[2] = 0;
				}
				else
				{ 
					countbuf[2]++;
					if (countbuf[2] > COUNT_LIMIT)
						outbuf[2][1] = 0;
					outbuf[2][0]=outbuf[2][1];
				}
				if (outbuf[2][0] < -Z_TRANSLATION_SENSITIVE){				
					input_report_key(dev, KEY_KPMINUS, 1);
					input_report_key(dev, KEY_KPPLUS, 0);
					input_sync(dev);	
				}					

				else if (outbuf[2][0] > Z_TRANSLATION_SENSITIVE){				
					input_report_key(dev, KEY_KPPLUS, 1);
					input_report_key(dev, KEY_KPMINUS, 0);
					input_sync(dev);
				}
				else {
					input_report_key(dev, KEY_KPMINUS, 0);
					input_report_key(dev, KEY_KPPLUS, 0);
					input_sync(dev);
				}

				/* pitch_rotation */

				if (inbuf[3][0] > PITCH_OFFSET || inbuf[3][0]  < -PITCH_OFFSET)
				{
					outbuf[3][0]=outbuf[3][1]+(inbuf[3][0]+inbuf[3][1]);	
					countbuf[3] = 0;
				}
				else
				{ 
					countbuf[3]++;
					if (countbuf[3] > COUNT_LIMIT)
						outbuf[3][1] = 0;
					outbuf[3][0]=outbuf[3][1];
				}
				if (outbuf[3][0] < -PITCH_ROTATION_SENSITIVE){				
					input_report_key(dev, KEY_PAGEDOWN, 1);
					input_report_key(dev, KEY_PAGEUP, 0);
					input_sync(dev);				
				}					

				else if (outbuf[3][0] > PITCH_ROTATION_SENSITIVE){				
					input_report_key(dev, KEY_PAGEUP, 1);
					input_report_key(dev, KEY_PAGEDOWN, 0);
					input_sync(dev);				
				}
				else {
					input_report_key(dev, KEY_PAGEUP, 0);
					input_report_key(dev, KEY_PAGEDOWN, 0);
					input_sync(dev);				
				}

				
				/* yaw_rotation */

				if (inbuf[5][0] > YAW_OFFSET || inbuf[5][0] < -YAW_OFFSET)
				{
					outbuf[5][0]=outbuf[5][1]+(inbuf[5][0]+inbuf[5][1]);	
					countbuf[5] = 0;
				}
				else
				{ 
					countbuf[5]++;
					if (countbuf[5] > COUNT_LIMIT)
						outbuf[5][1] = 0;
					outbuf[5][0]=outbuf[5][1];
				}
				if (outbuf[5][0] < -YAW_ROTATION_SENSITIVE){				
					input_report_key(dev, KEY_RIGHT, 1);
					input_report_key(dev, KEY_LEFT, 0);
					input_sync(dev);				
				}					

				else if (outbuf[5][0] > YAW_ROTATION_SENSITIVE){				
					input_report_key(dev, KEY_LEFT, 1);
					input_report_key(dev, KEY_RIGHT, 0);
					input_sync(dev);				
				}
				else {
					input_report_key(dev, KEY_RIGHT, 0);
					input_report_key(dev, KEY_LEFT, 0);
					input_sync(dev);				
				}


			}	
	
				
			else
				restore_buffer_data(buf, BUF_SIZE);

			sermouse->count++;
			break;

		/* default */

		default:
			sermouse->count++;
			break;
	}

	if (sermouse->count > 15)
		sermouse->count = 0;

}

/* Function for updating data in output buffer */

void shift_output_data(int outbuf[][OUT_BUF_SIZE]){
	int i,j;
	//shift
	for (i=0 ; i<6; i++){
		for (j = OUT_BUF_SIZE-2 ; j>=0; j--)
			outbuf[i][j+1] = outbuf[i][j];
	}
	
}
/* Function for updating data in input buffer */

void save_input_data(unsigned char *buf, int inbuf[][IN_BUF_SIZE], int buffer_size)
{	
	int i,j;
	//shift
	for (i=0 ; i<6; i++){
		for (j = IN_BUF_SIZE-2 ; j>=0; j--)
			inbuf[i][j+1] = inbuf[i][j];
	}
	/*Save data substracting mean value*/
	inbuf[0][0]= (int)buf[0]-122; 	//x
	inbuf[1][0]= (int)buf[1]-122; 	//y
	inbuf[2][0]= (int)buf[2]-169;	//z
	inbuf[3][0]= (int)buf[3]-122;	//pitch
	inbuf[4][0]= (int)buf[4]-122;	//roll
	inbuf[5][0]= (int)buf[5]-122;	//yaw

}

/*Function to convert from two bytes to one byte data*/

unsigned char conv2byteto1(unsigned char msb, unsigned char lsb)
{
	return lsb>>2 | msb<<6; //'low pass' filter
}

/* Function for restore input buffer when data is corrupted*/

void restore_buffer_data(unsigned char *buf, int buffer_size)
{
	//swap values (restore old values)
	int i;
	for (i = 0; i<buffer_size/2; i++)
		buf[i*2+1] = buf[i*2];
}


/*
 * sermouse_interrupt() handles incoming characters, either gathering them into
 * packets or passing them to the command routine as command output.
 */

static irqreturn_t sermouse_interrupt(struct serio *serio,
		unsigned char data, unsigned int flags)
{
	struct sermouse *sermouse = serio_get_drvdata(serio);

	sermouse_process_3d(sermouse, data);

	return IRQ_HANDLED;
}

/*
 * sermouse_disconnect() cleans up after we don't want talk
 * to the mouse anymore.
 */

static void sermouse_disconnect(struct serio *serio)
{
	struct sermouse *sermouse = serio_get_drvdata(serio);

	serio_close(serio);
	serio_set_drvdata(serio, NULL);
	input_unregister_device(sermouse->dev);
	kfree(sermouse);
}

/*
 * sermouse_connect() is a callback form the serio module when
 * an unhandled serio port is found.
 */

static int sermouse_connect(struct serio *serio, struct serio_driver *drv)
{
	struct sermouse *sermouse;
	struct input_dev *input_dev;
	unsigned char c = serio->id.extra;
	int err = -ENOMEM;
	int i,k;
	printk(KERN_ERR "Dispositivo connesso\n");
	sermouse = kzalloc(sizeof(struct sermouse), GFP_KERNEL);
	input_dev = input_allocate_device();
	if (!sermouse || !input_dev)
		goto fail1;

	sermouse->dev = input_dev;
	snprintf(sermouse->phys, sizeof(sermouse->phys), "%s/input3d", serio->phys);
	sermouse->type = serio->id.proto;
	sermouse->count = 0;
	for (i=0; i<BUF_SIZE; i++)
		sermouse->buf[i] = 128; //middle value!!
	for (k=0; k<6; k++){
		for (i=0; i<IN_BUF_SIZE; i++)
			sermouse->inbuf[k][i] = 0;	//128?
		for (i=0; i<OUT_BUF_SIZE; i++)
			sermouse->outbuf[k][i] = 0;
	}


	input_dev->name = sermouse_protocols[sermouse->type];
	input_dev->phys = sermouse->phys;
	input_dev->id.bustype = BUS_RS232;
	input_dev->id.vendor  = sermouse->type;
	input_dev->id.product = c;
	input_dev->id.version = 0x0100;
	input_dev->dev.parent = &serio->dev;

	input_dev->evbit[0] = BIT_MASK(EV_KEY);// | BIT_MASK(EV_REL);
	//input_dev->keybit[BIT_WORD(BTN_MOUSE)] = BIT_MASK(BTN_LEFT) | BIT_MASK(BTN_RIGHT);
	//input_dev->relbit[0] = BIT_MASK(REL_X) | BIT_MASK(REL_Y);

	set_bit(KEY_KPMINUS, input_dev->keybit);
	set_bit(KEY_KPPLUS, input_dev->keybit);
	set_bit(KEY_UP, input_dev->keybit);
	set_bit(KEY_DOWN, input_dev->keybit);
	set_bit(KEY_LEFT, input_dev->keybit);
	set_bit(KEY_RIGHT, input_dev->keybit);
	set_bit(KEY_PAGEUP, input_dev->keybit);
	set_bit(KEY_PAGEDOWN, input_dev->keybit);

	serio_set_drvdata(serio, sermouse);

	err = serio_open(serio, drv);
	if (err)
		goto fail2;

	err = input_register_device(sermouse->dev);
	if (err)
		goto fail3;

	return 0;

 fail3:	serio_close(serio);
 fail2:	serio_set_drvdata(serio, NULL);
 fail1:	input_free_device(input_dev);
	kfree(sermouse);
	return err;
}

/* Struct for recognize the mouse */

static struct serio_device_id sermouse_serio_ids[] = {
	{
		.type	= SERIO_ANY,	
		.proto	= 0x40,		//Valore non riscontrato nel protocollo serio
		.id	= 0xF0,
		.extra	= 0xF0,
	},
	{ 0 }
};

MODULE_DEVICE_TABLE(serio, sermouse_serio_ids);

/* Struct for kernel's call*/

static struct serio_driver sermouse_drv = {
	.driver		= {
		.name	= "sermouse3d",
	},
	.description	= DRIVER_DESC,
	.id_table	= sermouse_serio_ids,
	.interrupt	= sermouse_interrupt,
	.connect	= sermouse_connect,
	.disconnect	= sermouse_disconnect,
};

/* Function for device registration*/

static int __init sermouse_init(void)
{
	return serio_register_driver(&sermouse_drv);
}

/*Function for unregister the device*/

static void __exit sermouse_exit(void)
{
	serio_unregister_driver(&sermouse_drv);
}

module_init(sermouse_init);
module_exit(sermouse_exit);