/*
 Copyright (C) 2000 James R. Bruce (jbruce@cs.cmu.edu)

 This library is free software; you can redistribute it and/or
 modify it under the terms of the GNU Library 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
 Library General Public License for more details.

 You should have received a copy of the GNU Library General Public
 License along with this library; if not, visit
   www.gnu.org/copyleft/lgpl.html
 or write to:
   Free Software Foundation, Inc., 59 Temple Place - Suite 330,
   Boston, MA  02111-1307, USA.

 REVISION HISTORY:
   05/19/2000 - initial version (JRB)
*/

#include "mini_ssc.h"


/* here we map termios constants to their actual numbers.  I don't
   know why such constants were used, but they were, necessitating
   this table
*/
#define BAUD_RATES 20
const int baud_rates[BAUD_RATES][2] = {
  {      B0,      0 },
  {     B50,     50 },
  {     B75,     75 },
  {    B110,    110 },
  {    B134,    134 },
  {    B150,    150 },
  {    B200,    200 },
  {    B300,    300 },
  {    B600,    600 },
  {   B1200,   1200 },
  {   B1800,   1800 },
  {   B2400,   2400 },
  {   B4800,   4800 },
  {   B9600,   9600 },
  {  B19200,  19200 },
  {  B38400,  38400 },
  {  B57600,  57600 },
  { B115200, 115200 },
  { B230400, 230400 },
  { B460800, 460800 }
};


//==== Helper Functions =============================================//

speed_t baud_rate_to_flag(int speed)
{
  int i;

  for(i=0; i<BAUD_RATES; i++){
    if(baud_rates[i][1] == speed) return(baud_rates[i][0]);
  }

  return(0);
}

int baud_flag_to_rate(speed_t s)
{
  int i;

  for(i=0; i<BAUD_RATES; i++){
    if(baud_rates[i][0] == s) return(baud_rates[i][1]);
  }

  return(0);
}


//==== Global State =================================================//

int debug_level; // amount of debugging output to produce
int serial_fd;   // The file for the serial port we are using


//==== Main Functions ===============================================//

int ssc_debug_level(int level)
{
  if(level < 0) return(SSC_INVALID_DEBUG_LEVEL);

  debug_level = level;
  return(SSC_SUCCESS);
}

int ssc_open(int port,int baudrate)
{
  char device_name[32];
  struct termios tio;

  int baud_in,baud_out;
  int bf;

  if(port < 0) return(SSC_INVALID_PORT_NUM);
  sprintf(device_name,SERIAL_DEVICE,port);

  bf = baud_rate_to_flag(baudrate);
  if(!bf) return(SSC_INVALID_BAUD_RATE);

  // open device
  if(debug_level >= 2){
    printf("SSC: Opening serial device [%s].\n",device_name);
    fflush(stdout);
  }
  serial_fd = open(device_name,O_RDWR);

  if(serial_fd < 0){
    printf("SSC: Error, cannot open serial device [%s].\n",device_name);
    return(SSC_CANNOT_OPEN_DEVICE);
  }

  // set the parameters we need
  if(debug_level >= 2){
    printf("SSC: Setting serial port parameters.\n");
    fflush(stdout);
  }
  tcgetattr(serial_fd,&tio);
  cfmakeraw(&tio); // set up for binary io
  tio.c_cflag &= ~(CRTSCTS|IXON|IXOFF); // disable flow control

  printf("flow control = %d\n",!!(tio.c_cflag&(CRTSCTS|IXON|IXOFF)));

  bf = baud_rate_to_flag(baudrate);
  cfsetispeed(&tio,bf);
  cfsetospeed(&tio,bf);
  tcsetattr(serial_fd,TCSANOW,&tio);

  // test what actually got set
  memset(&tio,0,sizeof(tio));
  tcgetattr(serial_fd,&tio);
  baud_in = baud_flag_to_rate(cfgetispeed(&tio));
  baud_out = baud_flag_to_rate(cfgetospeed(&tio));
  if((baud_in != baudrate) || (baud_out != baudrate)){
    printf("SSC: Warning, could not set requested baud rate:\n"
           "     %dbps requested, achieved %d in, %d out\n",
	   baudrate,baud_in,baud_out);
  }

  if(debug_level >= 1){
    printf("SSC: Opened serial port %d at %dbps.\n",
	   port,baud_out);
  }

  return(0);
}

int ssc_move(int servo,int position)
{
  char cmdbuf[4];

  if((servo < 0) || (servo > MAX_SERVO)){
    return(SSC_INVALID_SERVO);
  }

  if((position < 0) || (position > MAX_POSITION)){
    return(SSC_INVALID_POSITION);
  }

  if(debug_level >= 2){
    printf("SSC: setting servo %d to postion %d.\n",servo,position);
    fflush(stdout);
  }

  // build command
  cmdbuf[0] = 255;
  cmdbuf[1] = servo;
  cmdbuf[2] = position;
  cmdbuf[3] = 0;

  if(write(serial_fd,(void*)cmdbuf,3) == 3){
    return(SSC_SUCCESS);
  }else{
    return(SSC_WRITE_ERROR);
  }
}

int ssc_close()
{
  if(debug_level >= 1){
    printf("SSC: closing serial device.\n");
    fflush(stdout);
  }

  if(serial_fd) close(serial_fd);
  serial_fd = 0;
  return(SSC_SUCCESS);
}