/*
 *  SPL - The SPL Programming Language
 *  Copyright (C) 2006 Clifford Wolf <clifford@clifford.at>
 *  Copyright (C) 2007 Raphael Langerhorst <raphael@raphael.g-system.at>
 *
 *  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.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 *  mod_gyro.c: Gyroscope 3D ADC board.
 */

/**
 * SPL Gyroscope Sensor Module
 *
 * This Module provides an interface to the Gyroscope Sensor Board.
 * The board has 16 channels and detects 3D translation and rotation.
 * Compass sensors are also on board.
 *
 * Some board specific documentation:
 *
 * The sensor board can be connected through RS232 or USB.
 * The board has 16 channels.
 * To read a certain number of channels once, send:
 *   1, 2, 3, 4, 5, 6, 7, 8, 9, A, B, C, D, E, F or G
 * as a character (note: from SPL use integers 1 to 16).
 * The result comes back as 2 bytes per channel, unsigned, MSB first.
 * (note: in SPL the result is an array with numbers)
 *
 * For example reading 3 channels can be done by sending '3'.
 * Then a string with 6 characters can be read with 2 bytes for each channel.
 * (note: in SPL it is done by sending 3 and receiving an array with 3 values).
 *
 *
 * This module translates the integers 10 to 16 to the characters 'A' to 'G'
 * before sending to the board.
 *
 * Received data is translated into an array of values. The number of elements
 * corresponds to the number of channels requested.
 *
 * You can use gyro_prefetch_state to request a certain number of channels
 * without waiting for the result. Then some other processing can be done
 * before calling gyro_state which will read the prefetched state and thus
 * return faster. However, using this method the time at which the values
 * are valid is when calling gyro_prefetch_state, not gyro_state.
 * In most cases the difference can be ignored unless there is heavy
 * processing done between prefetch_state and state calls.
 *
 * Note: due to an unknown reason the first read result is bad and discarded
 */

#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>

#include <poll.h>
#include <errno.h>
#include <signal.h>
#include <sys/param.h>
#include <sys/types.h>
#include <sys/stat.h>

#include "spl.h"
#include "compat.h"


extern void SPL_ABI(spl_mod_gyro_init)(struct spl_vm *vm, struct spl_module *mod, int restore);
extern void SPL_ABI(spl_mod_gyro_done)(struct spl_vm *vm, struct spl_module *mod);

/**
 * Opens the serial (tty) device for communication.
 *
 * The single argument must be the full path to the device file.
 *
 * The return value is the file descriptor (fd) for the serial device.
 * On failure, undef is returned.
 *
 * This file descriptor must be provided to all other gyro functions as first argument.
 */
// builtin gyro_open(devicename)
static struct spl_node *handler_gyro_open(struct spl_task *task, void *data UNUSED)
{
	char* tty_name = spl_clib_get_string(task);
	if (tty_name == NULL || strlen(tty_name) <= 0)
	{
		spl_report(SPL_REPORT_RUNTIME|SPL_REPORT_WARNING, task,"Gyro devicename not given\n");
		return 0;
	}
	int fd = open(tty_name, O_RDWR|O_NDELAY|O_NOCTTY);
	if (fd <= 0)
	{
		spl_report(SPL_REPORT_RUNTIME|SPL_REPORT_WARNING, task,"Could not open gyro device\n");
		return 0;
	}

	struct termios tty;
	tcgetattr(fd, &tty);
	cfsetospeed(&tty, (speed_t)B9600);
	cfsetispeed(&tty, (speed_t)B9600);
	tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;
	tty.c_iflag =  IGNBRK;
	tty.c_lflag = 0;
	tty.c_oflag = 0;
	tty.c_cflag |= CLOCAL | CREAD;
	tty.c_cflag &= ~CRTSCTS;
	tty.c_cc[VMIN] = 1;
	tty.c_cc[VTIME] = 5;
	tty.c_iflag &= ~(IXON|IXOFF|IXANY);
	tty.c_cflag &= ~(PARENB | PARODD);
	tty.c_cflag &= ~CSTOPB;

	tcsetattr(fd, TCSANOW, &tty);

	return SPL_NEW_INT(fd);
}

char gyro_translate_channels_native(int channels)
{
	switch(channels)
	{
		case 1:
			return '1';
		case 2:
			return '2';
		case 3:
			return '3';
		case 4:
			return '4';
		case 5:
			return '5';
		case 6:
			return '6';
		case 7:
			return '7';
		case 8:
			return '8';
		case 9:
			return '9';
		case 10:
			return 'A';
		case 11:
			return 'B';
		case 12:
			return 'C';
		case 13:
			return 'D';
		case 14:
			return 'E';
		case 15:
			return 'F';
		case 16:
			return 'G';
		default:
			return '1';
	}
}

/**
 * Sends a request to the device to read the state of given number of channels.
 * This function does not wait for the result to be ready and returns immediately.
 *
 * Thus it can be used to avoid having to wait for the result when using gyro_state only.
 */
// builtin gyro_prefetch_state(fd,channels)
static struct spl_node *handler_gyro_prefetch_state(struct spl_task *task, void *data UNUSED)
{
	int fd = spl_clib_get_int(task);
	int channels = spl_clib_get_int(task);
	if (fd <= 0 || channels <= 0 || channels > 16)
	{
		spl_report(SPL_REPORT_RUNTIME|SPL_REPORT_WARNING, task,"Invalid fd or channels parameters\n");
		return 0;
	}
	char c = gyro_translate_channels_native(channels);
	if (write(fd,&c,1) == -1)
	{
		spl_report(SPL_REPORT_RUNTIME|SPL_REPORT_WARNING, task,"Error writing to gyro device file descriptor\n");
	}

	return 0;
}

int gyro_poll_tty(int fd, int timeout)
{
  struct pollfd pfd;
  pfd.fd = fd;
  pfd.events = POLLIN;
  pfd.revents = 0;
  
  int bytes_ready = poll(&pfd,1,timeout);
  
  if ((pfd.revents & POLLIN) > 0 && bytes_ready == 1)
    return 1;
  else
    return 0;
}

/**
 * Reads given number of channels from device.
 * Returns an array with the read values.
 */
// builtin gyro_state(fd,channels)
static struct spl_node *handler_gyro_state(struct spl_task *task, void *data UNUSED)
{

	int fd = spl_clib_get_int(task);
	int channels = spl_clib_get_int(task);
	// 1) attempt to read given number of channels
	// 2) if read fails, drop any partial result and reread current state

	if (fd <= 0 || channels <= 0 || channels > 16)
	{
		spl_report(SPL_REPORT_RUNTIME|SPL_REPORT_WARNING, task,"gyro device file descriptor not given or number of channels invalid\n");
		return 0;
	}
	char state[channels * 2]; // channels a 2 byte, no \0 needed 

	int result = 0;
	int writecount = 0;

	while (result < channels * 2)
	{
		int new_result = 0;

	 	if (gyro_poll_tty(fd,30) == 0)
		{
			if (writecount <= 1)
			{
				// write again
				char c = gyro_translate_channels_native(channels);
				result = write(fd,&c,1); 

				if (result == -1)
				{
					spl_report(SPL_REPORT_RUNTIME|SPL_REPORT_WARNING, task,"Error writing to gyro device\n");
					return 0;
				}
				result = 0;
				writecount++;
			}
			else
			{
				spl_report(SPL_REPORT_RUNTIME|SPL_REPORT_WARNING, task,"Could not read state from gyro device\n");
				return 0;
			}
		}

		if ( (new_result = read(fd, state + result, 2*channels - result)) != -1 && new_result != 0 )
		{
			result += new_result;
		}
		else
		{
			spl_report(SPL_REPORT_RUNTIME|SPL_REPORT_WARNING, task,"Error writing to gyro device\n");
			return 0;
		}
	}

	// state array filled now, build state array -- see mod_wscons for nested nodes
	struct spl_node *state_array = spl_get(0);
	int i;
	char channel_num[10];
	int value;
	for (i = 0; i < channels; i++)
	{
		sprintf(channel_num,"ch%d",i);

		// TODO: check if this is correct (done)
		// docs say: 2 bytes per channel, unsigned, MSB first
		// Steuerzeichen: 1,2,3,4,5,6,7,8,9,A,B,C,D,E,F,G
		value = state[2*i] << 8;
		value += state[2*i+1];

		spl_create(task, state_array, channel_num,  SPL_NEW_INT(value),  SPL_CREATE_LOCAL);
	}

	// read pending junk
	while (gyro_poll_tty(fd,1) == 1)
	{
		char t;
		read(fd,&t,1);
	}

	return state_array;
}

/**
 * Closes the serial device.
 */
// builtin gyro_close(fd)
static struct spl_node *handler_gyro_close(struct spl_task *task, void *data UNUSED)
{
	int fd = spl_clib_get_int(task);
	close(fd);
	return 0;
}



// the SIGIO handler doesn't do anything in particular,
// it's enough to wake up the process from task_sleep()
void gyro_sigio_handler()
{
	//spl_report(SPL_REPORT_RUNTIME, task,"sigio\n");
	//printf("sigio\n");
}

static struct spl_node *handler_gyro_sigio(struct spl_task *task, void *data UNUSED)
{
	struct sigaction sigstruct;
	sigstruct.sa_sigaction = 0;
	sigstruct.sa_handler = gyro_sigio_handler;
	sigstruct.sa_flags = 0;
	if (sigaction(SIGIO,&sigstruct,0)==-1)
	{
		spl_report(SPL_REPORT_RUNTIME|SPL_REPORT_WARNING, task,"Could setup SIGIO handler in gyro module\n");
	}
	return 0;
}


void SPL_ABI(spl_mod_gyro_init)(struct spl_vm *vm, struct spl_module *mod UNUSED, int restore UNUSED)
{
  spl_clib_reg(vm, "gyro_open", handler_gyro_open, 0);
  spl_clib_reg(vm, "gyro_prefetch_state", handler_gyro_prefetch_state, 0);
  spl_clib_reg(vm, "gyro_state", handler_gyro_state, 0);
  spl_clib_reg(vm, "gyro_close", handler_gyro_close, 0);
  spl_clib_reg(vm, "gyro_wake_on_sigio", handler_gyro_sigio, 0);
}

void SPL_ABI(spl_mod_gyro_done)(struct spl_vm *vm UNUSED, struct spl_module *mod UNUSED)
{
  return;
}