/*

COPYRIGHT (C) 2004 Alvaro CASADO RODRIGUEZ

  Next code has been developed based on examples of rtai by 
  COPYRIGHT (C) 2000  Paolo Mantegazza (mantegazza@aero.polimi.it)

 This library is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 2 of the License, or (at your option) any later version.
 
  This library 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
  Lesser General Public License for more details.
  
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
*/

/*Compile :
	gcc -I/usr/src/rtai/include -lcomedi -g -o -Wall servos main.c 
*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sched.h>

#include <rtai_shm.h>
#define KEEP_STATIC_INLINE
#include <rtai_lxrt_user.h>
#include <rtai_lxrt.h>


//auxiliary function to set the servo to use
int new_index(int index,int val,int push)
{
	if(push==1)
	{
		return index*10+val;
	}	
	else
	{
		return val;
	}
}



int main(int argc,char * argv[])
{
	unsigned int *adress,*stop;
	int pid,i;
	char pushr;
	char *args[3];
	char b[2];
	int index=0;
	int push=0;
	if(argc!=2){
		printf("Use : servos: num_servos (1--8)\n");
		exit(-1);}
	adress =rtai_malloc(nam2num("SERVO1"),8);
	stop=rtai_malloc(nam2num("CONDICION"),1);

	stop[0]=1;
	for(i=0;i<atoi(argv[1]);i++)
    {
		adress[i]=0;
		b[0]=(i%25)+48;
		b[1]='\0';
		args[0]="./servo";
		args[1]=strdup(b);
		args[2]=NULL;
		pid=fork();
		if(!pid)
		  {if(execvp("./servo",args)!=0)
		    {
		      printf("Error exec\n");
		      exit(0);
		    }
		    
		  }
    }
    while(stop[0])
    {
		scanf("%c\n",&pushr);
		if(pushr=='s')
		{
			printf("Stop all threads \n");
			stop[0]=0;
			sleep(6);
		}
		if(pushr=='a'){
			push=0;
			if(adress[index]<1000){
				adress[index]=adress[index]+1;
			}
		}
		if(pushr=='d')
		{	  push=0;
		if(adress[index]>0)
			adress[index]=adress[index]-1;
		}
		switch(pushr)
		{
		case '0':index=new_index(index,0,push);push=1;break;
		case '1':index=new_index(index,1,push);push=1;break;
		case '2':index=new_index(index,2,push);push=1;break;
		case '3':index=new_index(index,3,push);push=1;break;
		case '4':index=new_index(index,4,push);push=1;break;
		case '5':index=new_index(index,5,push);push=1;break;
		case '6':index=new_index(index,6,push);push=1;break;
		case '7':index=new_index(index,7,push);push=1;break;
		case '8':index=new_index(index,8,push);push=1;break;
		case '9':index=new_index(index,9,push);push=1;break;
		default:break;
		}
    }
	
	
	
	exit(0);
}