/*
 * libkhepctrl : A library to control Khepera II robots 
 * Copyright (C) 2007 Guillaume Libersat <glibersat@sigill.org>
 *
 * 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
 */

#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>

#include "access.h"

/**
 * Open the given device at given speed and returns
 * a handle on the opened device.
 */
khep_hand_t khep_open(char *dev, khep_speed_t speed)
{
  int fd;
  struct termios tio;

  //  fd = open(dev, O_RDWR | O_NOCTTY | O_NDELAY);

  fd = open(dev, O_RDWR);
  

  printf("FD = %d\n", fd);

  if ( fd < 0 )
    return -1;


  /*   fd = open(dev, O_RDWR | O_NOCTTY | O_NDELAY); */
  /*   fcntl(fd, F_SETFL, 0); */

  /* Get port options */
  tcgetattr(fd, &tio);
  
  /*
   * Here we setup the serial port to stick to the Kheperia protocol.
   */

  /*
   * PARENB : Valid parity
   * CSIZE : Caracter length mask
   */
  /*   tio.c_cflag &= ~PARENB; */
  /*   tio.c_cflag &= ~CSIZE; */
  tio.c_iflag = 0;
  tio.c_oflag = 0;
  tio.c_cflag = CLOCAL | CREAD | CS8 | CSTOPB;
  tio.c_cc[VMIN] = 1;
  tio.c_cc[VTIME] = 5;

  
  /*
   * CS8 : 8 bit
   * CSTOPB : 2 stop bit
   * CLOCAL : No modem control
   * CREAD : Valid reception
   */
  tio.c_lflag &= ~(ICANON | ISIG | ECHO | ECHONL | ECHOE | ECHOK);


  /* Speed */
  cfsetispeed(&tio, speed);
  cfsetospeed(&tio, speed);


  /*
   * Set options
   */
  tcsetattr(fd, TCSANOW | TCSAFLUSH, &tio);

  /* Wait for the device to seattle */
  sleep(2);

  /*
   * There is a banner which is sent the first time we
   * connect to the Khepera. So we have to flush it
   * to prevent misfonctionning of following commands.
   */
  tcflush(fd, TCIOFLUSH);

  return fd;
}

/**
 * Close the device corresponding to the given handle
 */
void khep_close(khep_hand_t hand)
{
  close(hand);
}