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

#include <native/task.h>
#include <native/timer.h>
#include <rtdm/rtdm.h>
#include "inc.h"

unsigned int my_state = 0;
int timer_started = 0;
int my_fd = -1;
int shutdownnow = 0;
RT_TASK my_task;
//                        --s-ms-us-ns
RTIME my_task_period_ns =   1000000000llu;

#ifdef USEMMAP
void *mmappointer;
#endif

/**********************************************************/
/*            CLOSE RT DRIVER                             */
/**********************************************************/
static int close_file( int fd, unsigned char *name) {
  int ret,i=0;
  do {
    i++;
    ret = rt_dev_close(fd);
    switch(-ret){
      case EBADF:   printf("%s -> invalid fd or context\n",name);
                    break;
      case EAGAIN:  printf("%s -> EAGAIN (%d times)\n",name,i);
                    rt_task_sleep(50000); // wait 50us
                    break;
      case 0:       printf("%s -> closed\n",name);
                    break;
      default:      printf("%s -> ???\n",name);
                    break;
    }
  } while (ret == -EAGAIN && i < 10);
  return ret;
}

/**********************************************************/
/*            CLEANING UP                                 */
/**********************************************************/
void cleanup_all(void) {
  if (my_state & STATE_FILE_OPENED) {
    close_file( my_fd, DEV_FILE " (user)");
    my_state &= ~STATE_FILE_OPENED;
  }
  if (my_state & STATE_TASK_CREATED) {
    printf("delete my_task\n");
    rt_task_delete(&my_task);
    my_state &= ~STATE_TASK_CREATED;
  }
  if (timer_started) {
    printf("stop timer\n");
    rt_timer_stop();
  }
}

void catch_signal(int sig) {
  shutdownnow = 1;
  cleanup_all();
  printf("exit\n");
  return;
}

/**********************************************************/
/*            REAL TIME TASK                              */
/**********************************************************/
void my_task_proc(void *arg) {
  int ret;
  ssize_t sz = sizeof(RTIME);
  ssize_t written = 0;
  ssize_t read = 0;
  int counter = 0;
  int readbackcounter;
  unsigned char buf[17] = "CAPTAIN WAS HERE\0";
  unsigned char buf2[17] = "XXXXXXXXXXXXXXXX\0";
/* no periodic task, due blocking read from the RT driver (see below 
too)
  ret = rt_task_set_periodic(NULL, TM_NOW, 
rt_timer_ns2ticks(my_task_period_ns));
  if (ret) {
    printf("error while set periodic, code %d\n",ret);
    goto exit_my_task;
  }
*/

#ifdef USEMMAP
  printf("ioctl = %d\n", rt_dev_ioctl(my_fd, MMAP, &mmappointer));
  printf("*p = %d, p = %p\n", *((int *)mmappointer), mmappointer);
#endif

  while (1) {
    sprintf(buf, "CAPTAIN %d", counter);
    counter++;
    if (counter > 100) counter = 0;
    /* switch to primary mode */
    ret = rt_task_set_mode(0, T_PRIMARY, NULL);
    if (ret) {
      printf("error while rt_task_set_mode, code %d\n",ret);
      goto exit_my_task;
    }
    
/*  // rt_dev_read blocks, so we can't use rt_task_wait_period here,
    //   otherwise we get a ETIMEDOUT 110 = Connection timed out
    //   after the next rt_task_wait_period
    ret = rt_task_wait_period(NULL);
    if (ret) {
      printf("error while rt_task_wait_period, code %d\n",ret);
      goto exit_my_task;
    }
*/
    sz = sizeof(buf);
    written = rt_dev_write(my_fd, &buf, sizeof(buf));
    printf("WRITE: written=%d sz=%d\n", written, sz);
    if (written != sz ) {
      if (written < 0 ) {
        printf("error while rt_dev_write, code %d\n",written);
      } else {
        printf("only %d / %d byte transmitted\n",written, sz);
      }
      goto exit_my_task;
    }

    sz = sizeof(buf2);
    read = rt_dev_read(my_fd, &buf2, sizeof(buf2));
    if (read == sz ) {
		printf("READ: read=%s\n",buf2);
    } else {
      if (read < 0 ) {
        printf("error while rt_dev_read, code %d\n",read);
      } else {
        printf("only %d / %d byte received \n",read,sz);
      }
    }

    // read blocks, so check if user hit CTRL-C meanwhile
    //   otherwise we segfault when mmap'ing
    if (shutdownnow == 1) break;
    
#ifdef USEMMAP
    *((int *)mmappointer + 10) = counter + 1000;
    printf("MMAP: *((int *)mmappointer + 10) = %d\n", 
                         *((int *)mmappointer + 10));
#endif

    rt_dev_ioctl(my_fd, SETVALUE, &counter);
    rt_dev_ioctl(my_fd, GETVALUE, &readbackcounter);
    printf("IOCTL: readbackcounter=%d\n", readbackcounter);
    
    
  }
exit_my_task:
  if (my_state & STATE_FILE_OPENED) {
    if (!close_file( my_fd, DEV_FILE " (write)")) {
      my_state &= ~STATE_FILE_OPENED;
    }
  }
  printf("exit\n");
}

/**********************************************************/
/*            MAIN: mainly RT task initialization         */
/**********************************************************/
int main(int argc, char* argv[]) {
  int ret = 0;
  signal(SIGTERM, catch_signal);
  signal(SIGINT, catch_signal);

  printf("PRESS CTRL-C to EXIT\n");
  /* no memory-swapping for this programm */
  mlockall(MCL_CURRENT | MCL_FUTURE);


  /* open DEV_FILE */
  my_fd = rt_dev_open( DEV_FILE, 0);
  if (my_fd < 0) {
    printf("can't open %s\n", DEV_FILE);
    goto error;
  }
  my_state |= STATE_FILE_OPENED;
  printf("%s opened\n", DEV_FILE);

  /* create my_task */
  ret = rt_task_create(&my_task,"my_task",0,50,0);
  if (ret) {
    printf("failed to create my_task, code %d\n",ret);
    goto error;
  }
  my_state |= STATE_TASK_CREATED;
  printf("my_task created\n");

  /* start my_task */
  printf("starting my_task\n");
  ret = rt_task_start(&my_task,&my_task_proc,NULL);
  if (ret) {
    printf("failed to start my_task, code %d\n",ret);
    goto error;
  }

  pause();
  return 0;

error:
  cleanup_all();
  return ret;
}