/*
COPYRIGHT (C) 2002 Lorenzo Dozio (dozio@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.
*/


#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <signal.h>

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

//using hard real-time?
#ifdef USE_HRT
#define HRT_IF_ELSE(arg1, arg2) arg1
#define HRT_IF(arg) arg
#define HRT_IF_NOT(arg)
#else
#define HRT_IF_ELSE(arg1, arg2) arg2
#define HRT_IF(arg)
#define HRT_IF_NOT(arg) arg
#endif

#define N_CHANS		4
#define N_SCANS		20
#define T_CONVERT_ARG	10000000

char *subdevice_types[]={
        "unused",
        "analog input",
        "analog output",
        "digital input",
        "digital output",
        "digital I/O",
        "counter",
        "timer",
        "memory",
        "calibration",
        "processor"
};

char *cmdtest_messages[]={
        "success",
        "invalid source",
        "source conflict",
        "invalid argument",
        "argument conflict",
        "invalid chanlist",
};

char *cmd_src(int src,char *buf)
{
        buf[0]=0;

        if(src&TRIG_NONE)strcat(buf,"none|");
        if(src&TRIG_NOW)strcat(buf,"now|");
        if(src&TRIG_FOLLOW)strcat(buf, "follow|");
        if(src&TRIG_TIME)strcat(buf, "time|");
        if(src&TRIG_TIMER)strcat(buf, "timer|");
        if(src&TRIG_COUNT)strcat(buf, "count|");
        if(src&TRIG_EXT)strcat(buf, "ext|");
        if(src&TRIG_INT)strcat(buf, "int|");
#ifdef TRIG_OTHER
        if(src&TRIG_OTHER)strcat(buf, "other|");
#endif

        if(strlen(buf)==0){
                sprintf(buf,"unknown(0x%08x)",src);
        }else{
                buf[strlen(buf)-1]=0;
        }

        return buf;
}

void dump_cmd(comedi_cmd *cmd)
{
        char buf[100];

        printf("start:      %-8s %d\n",
                cmd_src(cmd->start_src,buf),
                cmd->start_arg);

        printf("scan_begin: %-8s %d\n",
                cmd_src(cmd->scan_begin_src,buf),
                cmd->scan_begin_arg);

        printf("convert:    %-8s %d\n",
                cmd_src(cmd->convert_src,buf),
                cmd->convert_arg);

        printf("scan_end:   %-8s %d\n",
                cmd_src(cmd->scan_end_src,buf),
                cmd->scan_end_arg);

        printf("stop:       %-8s %d\n",
                cmd_src(cmd->stop_src,buf),
                cmd->stop_arg);
}

static void *dev;
static int subdev_ai = -1;
static comedi_cmd *cmd;

//this can save trouble:
void SEGVhandler(int i);

int main(int argc, char **argv)
{
	RT_TASK *comedi_task;
	int i, subdev_ao, subdev_dio, n_subdevs, nch, type;
	char name[50];
	SEM *sem;
	unsigned int *chanlist;
	int ret;
	void *buf;

	rt_set_oneshot_mode();
	start_rt_timer(0);
	mlockall(MCL_CURRENT | MCL_FUTURE);

 	if (!(comedi_task = rt_task_init(nam2num("COMEDI"), 0, 0, 0))) {
		printf("CANNOT INIT COMEDI TASK\n");
		exit(1);
	}
	
 	if (0 == (sem = rt_sem_init(nam2num("CMDSEM"), 0))) {
		printf("CANNOT INIT SEMAPHORE\n");
		exit(1);
	}

	dev = comedi_open("/dev/comedi0");
	signal(SIGSEGV,SEGVhandler);

	printf("\n OVERALL INFO:\n");
	printf("   Version code : 0x%06x\n", comedi_get_version_code(dev));
	rt_comedi_get_board_name(dev, name);
	printf("   Board name   : %s\n", name);
	rt_comedi_get_driver_name(dev, name);
	printf("   Driver name  : %s\n", name);
	printf("   Number of subdevices : %d\n", n_subdevs = comedi_get_n_subdevices(dev));

	for (i = 0; i < n_subdevs; i++) {
		printf("\n Subdevice : %d\n", i);
		type = comedi_get_subdevice_type(dev, i);
		printf(" Type : %d (%s)\n", type, subdevice_types[type]);
		printf(" Number of channels : %d\n", nch = comedi_get_n_channels(dev, i));
		if (nch != -1) {
		    printf(" Maxdata : %d\n", comedi_get_maxdata(dev, i, 0));
		    printf(" Number of ranges : %d\n", comedi_get_n_ranges(dev, i, 0));
		}
	}

	subdev_ai = comedi_find_subdevice_by_type(dev, COMEDI_SUBD_AI, 0);

	subdev_ao = comedi_find_subdevice_by_type(dev, COMEDI_SUBD_AO, 0);
	subdev_dio = comedi_find_subdevice_by_type(dev, COMEDI_SUBD_DIO, 0);
	
	printf("\n Asynchronous test (with comedi commands).\n");

	comedi_lock(dev, subdev_ai);

	comedi_register_callback(dev, subdev_ai, COMEDI_CB_EOS, 0, sem);
	
	cmd = 0;
	buf2 = 0;
	cmd = rt_comedi_alloc_cmd(dev, subdev_ai, &chanlist, N_CHANS, &buf);
	
	int buf_len = comedi_get_buffer_size(dev, subdev_ai);
	buf_len /=sizeof(sampl_t);
	
	printf("\n Buffer size is %i samples.\n", buf_len);

/* the subdevice that the command is sent to */
	cmd->subdev = subdev_ai;

/* flags */
	cmd->flags = TRIG_WAKE_EOS HRT_IF(| TRIG_RT);

/* start of acquisition: immediately after call to comedi_command() */
	cmd->start_src = TRIG_NOW;
	cmd->start_arg = 0;

/* event signalling the beginning of each scan: expiring TIMER */
	cmd->scan_begin_src = TRIG_TIMER;
	cmd->scan_begin_arg = N_CHANS*T_CONVERT_ARG;

/* event triggering each A/D conversion: expiring TIMER */
	cmd->convert_src = TRIG_TIMER;
	cmd->convert_arg = T_CONVERT_ARG;

/* end of each scan: after N_CHANS conversions*/
	cmd->scan_end_src = TRIG_COUNT;
	cmd->scan_end_arg = N_CHANS;

/* event signalling the end of acquisition: after N_SCANS*N_CHANS conversions*/
	cmd->stop_src = TRIG_COUNT;
	cmd->stop_arg = N_SCANS*N_CHANS;

/* the channel list */
	cmd->chanlist = chanlist;
	cmd->chanlist_len = N_CHANS;
	chanlist[0] = CR_PACK(0,0,0);
	chanlist[1] = CR_PACK(1,0,0);
	chanlist[2] = CR_PACK(0,1,0);
	chanlist[3] = CR_PACK(1,1,0);
/*	chanlist[4] = CR_PACK(0,0,0);
	chanlist[5] = CR_PACK(0,1,0);
	chanlist[6] = CR_PACK(1,0,0);
	chanlist[7] = CR_PACK(1,1,0);
*/
/*	No longer in use by Comedi drivers:
	cmd->data = buf;
	cmd->data_len = BUFSZ*sizeof(sampl_t);
*/
        printf("\n  command before testing : \n");
        dump_cmd(cmd);
        ret = comedi_command_test(dev, cmd);
        printf("\n  First test returned %d (%s)\n", ret, cmdtest_messages[ret]);
        dump_cmd(cmd);
        ret = comedi_command_test(dev, cmd);
        printf("\n  Second test returned %d (%s)\n\n", ret, cmdtest_messages[ret]);
        if (ret != 0) {
                dump_cmd(cmd);
                printf(" ERROR PREPARING COMMAND\n");
                exit(1);
        }
	printf("Press Enter to start acquisition -- or Ctrl+C to (safely) quit.\n");
	getchar();
	comedi_command(dev, cmd);

	{
	    unsigned int mask = 0, front = 0, rear = 0;
	    int new_bytes = 0, semcnt = 0, n = 0;

	    //give daq card a head start for testing semcnt's return values
	    rt_sleep(nano2count(3*N_CHANS*T_CONVERT_ARG));

HRT_IF(	    printf("Going hard real-time. Please inspect dmesg output for messages.\n");
	    rt_make_hard_real_time();
)
    	    while (!(mask&COMEDI_CB_EOA) && !(front < rear)) {

		mask = rt_comedi_mark_wait_munge(dev, subdev_ai, &new_bytes, sem, &semcnt);
		HRT_IF_ELSE(rt_printk,printf)("\nMASK: %x, SEMCNT: %d, numnewbytes: %d, # %d", mask, semcnt, new_bytes, rear );

		//(new_bytes % sizeof(sampl_t)) _will_ be zero (see comedi_buf_read_n_available in comedi/comedi/drivers.c)
		front += new_bytes/sizeof(sampl_t);

		for (n = rear; n < front; n++) {
		    HRT_IF_ELSE(rt_printk,printf)(" %d",((sampl_t *)buf)[n % buf_len]);
		}
		rear = front;
	    }
	    HRT_IF(rt_make_soft_real_time();)
	    printf("\n");
	}

	comedi_cancel(dev, subdev_ai);

	rt_comedi_free_cmd(cmd);

	comedi_unlock(dev, subdev_ai);
	comedi_close(dev);

        //reset to default SIGINT, SIGSEGV handling
        signal(SIGSEGV,SIG_DFL);

	rt_sem_delete(sem);
	stop_rt_timer();
	rt_task_delete(comedi_task);

	return 0;
}
void SEGVhandler(int i)
{
    rt_make_soft_real_time();
    printf("testa SEGVhandler: ");
    if ((dev!=0)&&(subdev_ai!=-1)) {
      if ( comedi_unlock(dev,subdev_ai) == 0 ) {
        if (cmd != 0)
	  rt_comedi_free_cmd(cmd);
        if (0 == comedi_close(dev))
          printf("Unlocked, Closed, Nice.\n");
        else
          printf("Failed to close comedi device.\n");
      }
      else {
        if (comedi_cancel(dev,subdev_ai) != 0)
          printf("Failed to cancel, unlock, close.\n");
        else {
          if (cmd != 0)
	    rt_comedi_free_cmd(cmd);
          if ( comedi_unlock(dev,subdev_ai) == 0 )
            if (0 == comedi_close(dev))
              printf("Cancelled, Unlocked, Closed, Nice.\n");
            else
              printf("Failed to close comedi device.\n");
	  
        }
      }
      printf("testa crashing on ...\n");
    }
    else {
      if (dev!=0) {
        if (0 == comedi_close(dev))
          printf("Closed, Nice.\n");
        else
          printf("Failed to close comedi device.\n");
      }
      else 
        printf("Couldn't close device because I know none.\n");
    }
    signal(SIGSEGV,SIG_DFL);
    raise(i);
}