/*
 * 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 <assert.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>

#include <stdio.h>

#include "raw.h"

/**********
 * raw.c
 *
 * This file manages everything related to sending and reading
 * characters on the file descriptor.
 * It ensures communication goes well and try to detect errors.
 *
 **********/


/**
 * Read a line on the wire
 * @return a buffer containing the line. It is NULL terminated
 * @return The user must free it.
 */
char* khep_read(khep_hand_t hand)
{
  char *buffer = malloc(sizeof(char)*KHEP_CMD_MAX);
  ssize_t count;
  
  //tcflush(hand, TCOFLUSH);

  count = read(hand, buffer, sizeof(char)*(KHEP_CMD_MAX-1));

  printf("DEBUG: Read %d bytes back\n", count);

  /* It's ok */
  if ( count > 2  )
    {
      buffer[count-2] = '\0';

      return buffer;
    }

  /* I/O Error */
  free(buffer);
  return NULL;
}

/**
 * Write data on the wire
 */
ssize_t khep_write(khep_hand_t hand,
		   char *data,
		   ssize_t length)
{
  printf("DEBUG: Writing\n");

  /* Flush output buffer */
  //tcflush(hand, TCOFLUSH);

  ssize_t count = write(hand, data, sizeof(char)*length);
  
  if ( count < 0 )
    {
      printf("DEBUG: count is %d whereas length is %d on %u\n", count, length, hand);
      perror("Reason");
    }
  else if ( count != (sizeof(char)*length) )
    {
      printf("DEBUG: write count mismatch!\n");
      perror("Reason");
      exit(1);
    }
  
  printf("done.. flushing\n");

  /* Wait until everything's written */
  tcdrain(hand);

  /* We have to wait for the Khepera to answer.
   * The manual reads that a cycle longs for 30ms,
   * so wait here about 50ms to ensure it's ok.
   */
  usleep(50000);

  return count;
}


khep_rawcmd_ret_t khep_cmd_raw(khep_hand_t hand, 
			    char *cmd)
{
  assert(strlen(cmd) < KHEP_CMD_MAX && "Command too long.");

  char cooked[KHEP_CMD_MAX];
  unsigned int length = strlen(cmd);

  /* Add '\r' at end of command to match Khepera's protocol */
  printf("Copying %d bytes (%s) to cooked, which is %d\n", length, cmd, sizeof(cooked));
  strncpy(cooked, cmd, length);
  cooked[length] = '\r';
  length++;

  /* Write to khepera */
  ssize_t count = khep_write(hand, cooked, length);


  /* Build return structure */
  khep_rawcmd_ret_t res;
  memset(&res, 0, sizeof(khep_rawcmd_ret_t));
  
  /* I/O error */
  if ( count != length )
    {
      res.result = KHEP_CMD_E_IO;
    }
  /* OK */
  else
    { 
      char *line = khep_read(hand);

      if ( line == NULL )
	res.result = KHEP_CMD_E_IO;
      else
	{
	  printf("Res => %s\n", line);

	  /* Split response line here.
	   * Result code is given by treat_answer
	   */
	  khep_treat_answer(cooked, line, &res);

	  free(line);
	}
    }
  
  return res;
}

/**
 * Splits a string each time ',' is encountered.
 * It returns an array of NULL terminated strings.
 * A NULL pointer is set at the end of the array.
 *
 * W: It assumes the answer is well formed.
 */
char **khep_explode_ans(char *str)
{
  char **res = NULL;
  char separator = ',';
  int  nbstr = 1;
  int  len;
  int  from = 0;
  int  i;
  int  j;

  res = (char **) malloc(sizeof (char *));
  len = strlen(str);
  for (i = 0; i <= len; ++i)
    {
      if ((i == len) || (str[i] == separator))
	{
	  res = (char **) realloc(res, ++nbstr * sizeof (char *));
	  res[nbstr - 2] = (char *) malloc((i - from + 1) * sizeof (char));

	  for (j = 0; j < (i - from); ++j)
	    res[nbstr - 2][j] = str[j + from];

	  res[nbstr - 2][i - from] = '\0';
	  from = i + 1;
	  ++i;
	}
    }
  res[nbstr - 1] =  NULL;

  return res;
}


/**
 * Check if the answer is correct
 * Split it if necessary
 * We know we have a well formed line (ensured by khep_read)
 * W: Freeing the result is let to the user.
 */
void khep_treat_answer(char *question,
		       char *answer,
		       khep_rawcmd_ret_t *res)
{
  /* Command not implemented */
  if ( answer[0] == 'z' )
    res->result = KHEP_CMD_UNKNOWN;
  /* Answer doesn't match protocol (A -> a) */
  else if ( answer[0] != tolower(question[0]) )
    res->result = KHEP_CMD_BADREP;
  /* Seems ok, split it */
  else
    {
      char **r = khep_explode_ans(answer);
      res->args = r;
      res->result = KHEP_CMD_OK;
    }
}


/*
 * Free a raw answer returned by
 * khep_cmd_raw()
 */
void khep_free_answer(khep_rawcmd_ret_t *raw)
{
  char **arg = raw->args;
  while ( *arg != NULL )
    {
      free(*arg);
      arg++;
    }

   free(raw->args);
}