/*
  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.
*/
/*Before execute you must load rtai,rtai_sched,rtai_lxrt,rtai_shm,rtai_pthreads,comedi using modprobe or insmod*/
/*Compile :
	gcc -I/usr/src/rtai/include -lpthread -lcomedi -g -o -Wall servo_threads servo_threads.c 
*/

#include <comedilib.h>

#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 <pthread.h>
#include <signal.h>

#include "rtai_shm.h"

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

#define NTASKS 16 // our card (PCL812Pg ) has 16 digital outputs.

unsigned int control[NTASKS];
int stop=0;
pthread_t task[NTASKS];

int ntasks = NTASKS;

RT_TASK *m_task;

void *thread(void *arg)
{
    RTIME t1;
    RT_TASK *task;
    long PERIOD=20000000;
    float alfa=0.2;
    unsigned long task_name;
    int task_index;
    comedi_t * device;
    int stype;
    int subdevice =3;
	
	
	//open the device 
    device=comedi_open("/dev/comedi0");
    if(!device){
		comedi_perror("/dev/comedi0");
		exit(0);
    }
    
    //Sure the subdevice is correct
    stype = comedi_get_subdevice_type(device,subdevice);
    if(stype!=COMEDI_SUBD_DO){
		printf("%d is not a digital I/O subdevice\n",subdevice);
		exit(0);
    }
    
    task_index = *((int *)arg);
    task_name = (task_index);
    if (!(task = rt_task_init_schmod(task_name, 1, 0, 0, SCHED_FIFO, 1 ))) {
		printf("CANNOT INIT TASK %lu\n", task_name);
		exit(1);
    }
	//locks pages in mem
	mlockall(MCL_CURRENT | MCL_FUTURE);
	
    {
		t1=rt_get_time();
		while(!stop) {
			//sleep until next period
			rt_sleep_until(t1);
			//Write '1'
			comedi_dio_write(device,3,task_index,1);
			//Espera de tiempo on si ceder control al planificador (Espera activa)
			alfa=control[task_index]*0.001;
			//sleep during a pulse wide 
			rt_sleep(nano2count(alfa*PERIOD));
			t1=t1+nano2count(PERIOD);
			//Write '0' 
			comedi_dio_write(device,3,task_index,0);
			
		}
		
	}
	//delete the thread
    rt_task_delete(task);
    return 0;
}

//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 push=0;
    char pushr;
    int index=0;
    int i, index_t[NTASKS];       
    unsigned long task_name = nam2num("FIRST");
	//this is necesary to use in user-space
	rt_allow_nonroot_hrt();

	
    //init the main task
    if (!(m_task = rt_task_init(task_name, 4, 0, 0))) {
		printf("CANNOT INIT TASK %lu\n", task_name);
		exit(1);
    }
	
	//set timer mode
    rt_set_oneshot_mode();
    start_rt_timer(0);
    
	
    for (i = 0; i < ntasks; i++) {
		control[i]=13;//set the minimun value for the servo Ts, this value depend on the servo
		index_t[i] = i;
		if (pthread_create(&task[i], NULL, thread, &index_t[i])) {
			printf("ERROR IN CREATING THREAD %d\n", index_t[i]);
			exit(1);
		}
    }
    
	//this is a simple interface and it can be use to probe but it doesn´t the best to use .
	//this can perturbe the other threads.
    while(!stop)
    {
		scanf("%c\n",&pushr);
		if(pushr=='s')
		{
			printf("Stop all threads \n");
			stop=1;
			sleep(6);
			
		}
		
		if(pushr=='a'){
			push=0;
			if(control[index]<1000){
				control[index]=control[index]+1;
			}
		}
		if(pushr=='d')
		{	  push=0;
		if(control[index]>0)
			control[index]=control[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;
		}
		
		
    }
	
	
	
	
	
	stop_rt_timer();
    rt_task_delete(task);
    for (i = 0; i < ntasks; i++) {
		pthread_join(task[i], NULL);
    }
    return 0;
}