/*
 * fx.c
 * Simon Newton <newtons _at_ iinet.net> 2003
 * Acts as a small interpreter for fx light commands.
 * Can control the x axis motor (bits 0 & 1), y axis motor (bits 2 & 3)
 * and colour wheel motor (bits 4 & 5)
 * 
 * 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
 *
 */

#include <stdio.h> 
#include <stdlib.h>
#include <asm/io.h>
#include <signal.h>
                                                          
#define DATA 0x378                                        

#define CW 1
#define CCW 0  

int data = 255;
int x = 0 ;
int y = 0;
int z = 0;

struct timespec tiempo;


void interpret (FILE *stream) ;
void move(int op1, int op2, int op3, int speed, int reps);
void calc (int op, int offset, int * data1, int * data2) ;
void reset() ;
void handler(int signum) ;


int main (int argc, char **argv) {
   	tiempo.tv_sec = 0;
	tiempo.tv_nsec = 0;

	if (ioperm(DATA,1,1))
		fprintf(stderr, "Couldn't get the port at %x\n", DATA), exit(1);

	signal(SIGINT,handler) ;
	printf("Type h for help\n");
	interpret(stdin) ;
}

void interpret(FILE *stream) {

	int op1, op2, op3, op4, op5,i  ;
    char line[80], cmd ;
	char *file;
	FILE *fh;

	int e ;

	while(fgets(line, 80, stream)) {
		switch(line[0]) {
			case 'm':
				/* move command */
				if(sscanf(line, "%c %i %i %i %i %i\n",&cmd, &op1, &op2, &op3, &op4, &op5) == 6) {
					move(op1,op2,op3,op4,op5);
				} else {
					printf("Invalid command: %s", line) ;
				}
				break ;
			case 'r':
				/* run file command */
				op1 = 1;
				i = -1 ;
				file = malloc(80 * sizeof(char)) ;
				sscanf (line, "%c %s %i", &cmd, file, &op1);  
				fh = fopen( file,"rt");
				
				if(!fh) {
					printf("Could not open command file %s\n", file) ;
					break;
				}
				free(file) ;

				if(op1) i = 0 ;

				while(i<op1) {
					printf("%i\n",i) ;
					interpret(fh) ;
					rewind(fh) ;
					if (op1) i++ ;
				}
				fclose(fh) ;
				break;
			case 's':
				reset() ;
				break;
			case 'h':
				printf("m x y z speed number_of_turns \n");
				printf("  x,y,z are -1 (backwards), 0 (no movement)");
				printf(" or 1 (forwards)\n");
				printf("r filename [n]\n") ;
				printf("  run the sequence in the specified file n times\n");
				printf("  n=0 loops forver\n");				
				printf("s\n");
				printf("  reset the motors to their starting position\n");
				printf("q\n");
				printf("  quit (and reset)\n") ;
				break ;
			case 'q':
				/* quit command */
				reset() ;
				exit(0) ;
			case '\n':
				break;
			default:
				printf("Invalid command: %s", line);
		}
	}
}


void move( int op1, int op2, int op3, int speed, int reps) {
	int i, data1, data2;
	struct timespec pause;
	
	pause.tv_sec = 0;
	pause.tv_nsec = speed;

	data1 = data2 = data;

	calc(op1, 1, &data1, &data2) ;
	calc(op2, 4, &data1, &data2) ;
	calc(op3, 16, &data1, &data2) ;

	x += op1 * reps;
	y += op2 * reps;
	z += op3 * reps;
	z %= 200;
	
	for(i=0 ; i<reps; i++) {
		outb(data1,DATA);
		nanosleep(&tiempo,NULL);
		outb(data2,DATA);
		if (speed!=-1) nanosleep(&pause,NULL);
	}

	if (op1 || op2 || op3) 
		printf("x=%i, y=%i, z=%i\n",x,y,z) ;
}

void calc (int op, int offset, int * data1, int * data2) {
	switch(op) {
		case -1:
			*data1 = *data1 & (~(3*offset)); 	/* dir 0 , step 0 */
			*data2 = *data2 & (~offset); 		/* dir 0 */
			*data2 = *data2 | (2*offset);		/* step 1 */
			break;
		case 0:
			*data1 = *data1 | (2*offset); 		/* step 1 */
			*data2 = *data2 | (2*offset); 		/* step 1 */
			break;
		case 1:
			*data1 = *data1 & (~(2*offset)); 	/* dir 1 */
			*data1 = *data1 | offset; 			/* step 0 */
			*data2 = *data2 | (3*offset); 		/* dir 1, step 1 */
			break;
	}
}

void reset() {
	printf("Resetting...\n") ;
	move(x<0?1:-1,0,0,-1,abs(x)) ;
	move(0,y<0?1:-1,0,-1,abs(y)) ;
	move(0,0,z<0?1:-1,-1,abs(z)) ;
	printf("Done\n") ;
}

void handler(int signum) {
	reset() ;
	exit(0);
}