/*!
  \file
  \brief URG 制御

  \author Satofumi KAMIMURA

  $Id: urg_ctrl.c 1333 2009-09-23 01:43:49Z satofumi $
*/

#include "math_utils.h"
#include "urg_ctrl.h"
#include "scip_handler.h"
#include "urg_errno.h"
#include "serial_ctrl.h"
#include "serial_utils.h"
#include "serial_errno.h"
#include "ticks.h"
#include "delay.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#if defined(WINDOWS_OS)
#define snprintf _snprintf
#endif

extern int snprintf(char *, size_t, const char *, ...);


enum {
  ScipTimeout = 1000,           /*!< [msec] */
  EachTimeout = 100,		/*!< [msec] */
};


/* 接続処理 */
static int urg_firstConnection(urg_t *urg, long baudrate)
{
  long try_baudrates[] = { 115200, 19200, 38400 };
  int try_size = sizeof(try_baudrates) / sizeof(try_baudrates[0]);
  int pre_ticks;
  int reply = 0;
  int ret;
  int i;

  /* 接続したいボーレートを配列の先頭と入れ換える */
  for (i = 1; i < try_size; ++i) {
    if (baudrate == try_baudrates[i]) {
      long swap_tmp = try_baudrates[i];
      try_baudrates[i] = try_baudrates[0];
      try_baudrates[0] = swap_tmp;
      break;
    }
  }

  /* 指定のボーレートで接続し、応答が返されるかどうか試す */
  for (i = 0; i < try_size; ++i) {

    /* ホスト側のボーレートを変更 */
    ret = serial_setBaudrate(&urg->serial_, try_baudrates[i]);
    if (ret < 0) {
      return ret;
    }

    serial_clear(&urg->serial_);

    /* QT の発行 */
    ret = scip_qt(&urg->serial_, &reply, ScipWaitReply);
    if (ret == UrgSerialRecvFail) {
      /* 応答が返されない場合、ボーレートが違うとみなす */
      continue;
    }

    if ((ret == UrgMismatchResponse) && (reply != -0xE)) {
      /* MD/MS コマンドの応答を受け取ったときの処理 */
      /* 受信内容を全て読み飛ばしてから、次の処理を行う */
      /* (reply == -0xE) のときは、SCIP1.1 応答で 'E' の場合 */
      serial_clear(&urg->serial_);
      serial_skip(&urg->serial_, ScipTimeout, EachTimeout);
      reply = 0x00;
    }

    /* 応答が返されれば、既に SCIP2.0 モードであり "SCIP2.0" の発行は不要 */
    if (reply != 0x00) {
      if ((ret = scip_scip20(&urg->serial_)) < 0) {
        /* 応答がなければ、違うボーレートに接続したものとみなす */
        continue;
      }
      if (ret == 12) {
        /* SCIP1.1 プロトコル */
        return UrgScip10;
      }
    }

    /* ボーレートを変更する必要がなければ、戻る */
    if (baudrate == try_baudrates[i]) {
      return 0;
    }

    /* URG 側を指定されたボーレートに変更する */
    pre_ticks = ticks();
    if (scip_ss(&urg->serial_, baudrate) < 0) {
      return UrgSsFail;

    } else {
      /* シリアル通信の場合、ボーレート変更後、１周分だけ待つ必要がある */
      int reply_msec = ticks() - pre_ticks;
      delay((reply_msec * 4 / 3) + 10);

      return serial_setBaudrate(&urg->serial_, baudrate);
    }
  }

  return UrgAdjustBaudrateFail;
}


static void urg_t_initialize(urg_t *urg)
{
  urg->parameters_.area_max_ = 0;
  urg->parameters_.scan_rpm_ = 0;
  urg->parameters_.sensor_type[0] = '\0';
  urg->remain_byte_ = 0;
}


/* シリアルデバイスを開き、URG との初期化を行う */
int urg_connect(urg_t *urg, const char *device, long baudrate)
{
  int ret;
  urg_t_initialize(urg);

  /* シリアル接続を開く */
  ret = serial_connect(&urg->serial_, device, baudrate);
  if (ret != 0) {
    urg->errno_ = UrgSerialConnectionFail;
    return ret;
  }
  // Windows でタイマ分解能を変更するための呼び出し
  delay(0);

  /* URG への接続処理 */
  ret = urg_firstConnection(urg, baudrate);
  if (ret < 0) {
    urg->errno_ = ret;
    serial_disconnect(&urg->serial_);
    return ret;
  }

  /* パラメータ情報の更新、および初期化 */
  ret = scip_pp(&urg->serial_, &urg->parameters_);
  if (ret < 0) {
    urg->errno_ = ret;
    serial_disconnect(&urg->serial_);
    return ret;
  }
  urg->skip_lines_ = 1;
  urg->skip_frames_ = 0;
  urg->capture_times_ = 0;
  urg->is_laser_on_ = UrgLaserUnknown;
  urg->remain_times_ = 0;

  urg->errno_ = UrgNoError;
  return 0;
}


void urg_disconnect(urg_t *urg)
{
  /* MD/MS コマンドを停止させるため */
  urg_laserOff(urg);
  serial_skip(&urg->serial_, ScipTimeout, EachTimeout);

  /* シリアル接続の切断 */
  serial_disconnect(&urg->serial_);
}


int urg_isConnected(const urg_t *urg)
{
  /* シリアル接続が有効ならば、0 を返す */
  return serial_isConnected(&urg->serial_);
}


const char *urg_error(const urg_t *urg)
{
  return urg_strerror(urg->errno_);
}


int urg_versionLines(urg_t *urg, char* lines[], int lines_max)
{
  if (! urg_isConnected(urg)) {
    return -1;
  }
  return scip_vv(&urg->serial_, lines, lines_max);
}


/* PP コマンドを送信し、応答を解析して格納してから返す */
int urg_parameters(urg_t *urg, urg_parameter_t* parameters)
{
  int ret = 0;

  if (urg_isConnected(urg)) {
    *parameters = urg->parameters_;
  } else {
    ret = scip_pp(&urg->serial_, &urg->parameters_);
    if (parameters) {
      *parameters = urg->parameters_;
    }
  }

  urg->errno_ = UrgNoError;
  return 0;
}


const char* urg_model(const urg_t *urg)
{
  return urg->parameters_.sensor_type;
}


int urg_dataMax(const urg_t *urg)
{
  return urg->parameters_.area_max_ + 1;
}


int urg_scanMsec(const urg_t *urg)
{
  int scan_rpm = urg->parameters_.scan_rpm_;
  return (scan_rpm <= 0) ? 1 : (1000 * 60 / scan_rpm);
}


long urg_maxDistance(const urg_t *urg)
{
  return urg->parameters_.distance_max_;
}


long urg_minDistance(const urg_t *urg)
{
  return urg->parameters_.distance_min_;
}


int urg_setSkipLines(urg_t *urg, int lines)
{
  /* 間引きライン数を登録 */
  if (lines == 0) {
    lines = 1;
  }
  if ((lines < 0) || (lines > 99)) {
    return -1;
  }

  urg->skip_lines_ = lines;
  return 0;
}


int urg_setSkipFrames(urg_t *urg, int frames)
{
  /* 間引きフレーム数を登録 */
  if ((frames < 0) || (frames > 9)) {
    return -1;
  }

  urg->skip_frames_ = frames;
  return 0;
}


int urg_setCaptureTimes(urg_t *urg, int times)
{
  /* MD/MS のデータ取得回数を登録 */
  if ((times < 0) || (times >= 100)) {
    urg->capture_times_ = 0;
  } else {
    urg->capture_times_ = times;
  }

  return 0;
}


int urg_remainCaptureTimes(const urg_t *urg)
{
  if (urg->capture_times_ == 0) {
    /* 無限回のデータ取得 */
    return 100;

  } else {
    return urg->remain_times_;
  }
}


int urg_requestData(urg_t *urg,
                    urg_request_type request_type,
                    int first_index,
                    int last_index)
{
  char buffer[] = "MDsssseeeellstt\r";

  if (first_index == URG_FIRST) {
    first_index = 0;
  }
  if (last_index == URG_LAST) {
    last_index = urg->parameters_.area_max_;
  }

  if ((request_type == URG_GD) || (request_type == URG_GS) ||
      (request_type == URG_GD_INTENSITY)) {

    /* GD/GS の場合 */
    snprintf(buffer, 14, "G%c%04d%04d%02d\r",
             (((request_type == URG_GD) ||
               (request_type == URG_GD_INTENSITY)) ? 'D' : 'S'),
             first_index, last_index,
             urg->skip_lines_);

    /* レーザが点灯してない場合、点灯を指示する */
    if (urg->is_laser_on_ != UrgLaserOn) {
      urg_laserOn(urg);
    }

  } else if ((request_type == URG_MD) || (request_type == URG_MS) ||
             (request_type == URG_MD_INTENSITY)) {
    char type = (request_type == URG_MS) ? 'S' : 'D';

    /* MD/MS の場合 */
    snprintf(buffer, 17, "M%c%04d%04d%02d%d%02d\r",
             type,
             first_index, last_index,
             urg->skip_lines_,
             urg->skip_frames_,
             urg->capture_times_);
    urg->remain_times_ = urg->capture_times_;

  } else {
    urg->errno_ = UrgInvalidArgs;;
    return urg->errno_;
  }

  if ((request_type == URG_GD_INTENSITY) ||
      (request_type == URG_MD_INTENSITY)) {
    if (! strcmp("UTM-30LX", urg->parameters_.sensor_type)) {
      if (request_type == URG_GD_INTENSITY) {
        urg->errno_ = UtmNoGDIntensity;
        return urg->errno_;
      }
      /* Top-URG は ME コマンドを使う */
      buffer[0] = 'M';
      buffer[1] = 'E';

      /* グループ数を 2 固定にする */
      buffer[10] = '0';
      buffer[11] = '2';

    } else {
      /* Top-URG 以外の URG はまとめる数に特殊文字を使って強度データを受け取る*/
      buffer[10] = 'F';
      buffer[11] = 'F';
    }
  }

  return scip_send(&urg->serial_, buffer);
}


/* URG の 6bit データをデコード */
static long decode(const char* data, int data_byte)
{
  const char* p = data;
  const char* last_p = p + data_byte;

  int value = 0;
  while (p < last_p) {
    value <<= 6;
    value &= ~0x3f;
    value |= *p++ - 0x30;
  }
  return value;
}


static int convertRawData(long data[], int data_max,
                          const char* buffer, int buffer_size, int filled,
                          int is_me_type, int data_bytes, int skip_lines,
                          int store_last, urg_t* urg)
{
  int n;
  int i;
  int j;
  int remain_byte = urg->remain_byte_;
  long length;
  long *data_p = data + filled;

  (void)is_me_type;

  /* store_first, store_last でデータサイズを制限する */
  if (data_max > store_last) {
    data_max = store_last;
  }

  if (filled == 0) {
    /* 最初の呼び出しのときに、残りデータ数の初期化を行う */
    remain_byte = 0;
  }

  if (buffer_size <= 0) {
    return filled;
  }

  /* 直前の処理データが残っていれば、それを処理する */
  /* !!! 下の実装との重複を関数にする */
  if (remain_byte > 0) {
    memcpy(&urg->remain_data_[remain_byte], buffer, data_bytes - remain_byte);
    n = skip_lines;
    if ((filled + n) > data_max) {
      n = data_max - filled;
    }
    length = decode(urg->remain_data_, data_bytes);
    for (j = 0; j < n; ++j) {
      *data_p++ = length;
    }
    filled += n;
#if 0
    if (is_me_type) {
      for (j = 0; j < skip_lines; ++j) {
        /* !!! */
      }
    }
#endif
  }

  /* １行のデータを処理する */
  n = buffer_size - data_bytes;
  for (i = (data_bytes - remain_byte) % data_bytes; i <= n; i += data_bytes) {
    length = decode(&buffer[i], data_bytes);
    for (j = 0; j < skip_lines; ++j) {
      if (filled >= data_max) {
        return data_max;
      }
      *data_p++ = length;
      ++filled;
    }

#if 0
    if (is_me_type) {
      for (j = 0; j < skip_lines; ++j) {
        /* !!! */
      }
    }
#endif
  }

  /* 残ったデータを退避する */
  urg->remain_byte_ = buffer_size - i;
  memcpy(urg->remain_data_, &buffer[i], urg->remain_byte_);

  return filled;
}


static int checkSum(const char buffer[], int size, char actual_sum)
{
  const char *p = buffer;
  const char *last_p = p + size;
  char expected_sum = 0x00;

  while (p < last_p) {
    expected_sum += *p++;
  }
  expected_sum = (expected_sum & 0x3f) + 0x30;

  return (expected_sum == actual_sum) ? 0 : -1;
}


static int atoi_substr(const char *str, size_t len)
{
  char buffer[13];

  strncpy(buffer, str, len);
  buffer[len] = '\0';

  return atoi(buffer);
}


static int internal_receiveData(urg_t *urg, long data[], int data_max,
                                int store_first, int store_last,
                                int skip_lines)
{
  enum {
    EchoBack = 0,
    ReplyCode,
    Timestamp,

    False = 0,
    True = 1,

    MD_MS_Length = 15,          /* MD, MS コマンドの長さ */
    GD_GS_Length = 12,          /* GD, GS コマンドの長さ */
  };

  int lines = 0;
  char buffer[UrgLineWidth];
  int filled = 0;
  int is_echoback = False;
  int n;

  int is_me_type = 0;
  char current_type[] = "xx";
  int current_first = -1;
  int current_last = -1;
  int current_skip_lines = -1;
  int current_skip_frames = -1;
  int current_capture_times = -1;
  int current_data_bytes = 3;
  int dummy_last;
  int timeout = ScipTimeout;

  /* タイムスタンプを初期化 */
  urg->last_timestamp_ = UrgInvalidTimestamp;

  urg->errno_ = UrgNoResponse;

  while (1) {
    n = serial_getLine(&urg->serial_, buffer, ScipLineWidth, timeout);
    //fprintf(stderr, "%d: %s\n", ticks(), buffer);
    if (n <= 0) {
      if (is_echoback) {
        /* !!! 制御構造を見直す */
        is_echoback = False;
        lines = 0;
        continue;
      }
      break;
    }

    if (lines > 0) {
      /* エコーバックにはチェックサム文字列がないので、無視 */
      if (checkSum(buffer, n - 1, buffer[n - 1]) < 0) {
        urg->errno_ = UrgInvalidResponse;
        lines = 0;
        filled = 0;
        is_echoback = False;
        continue;
      }
    }

    if (lines > Timestamp) {
      /* データの変換処理 */
      filled = convertRawData(data, data_max, buffer, n - 1, filled,
                              is_me_type, current_data_bytes, skip_lines,
                              store_last, urg);

    } else if (lines == EchoBack) {
      /* エコーバック */

      if ((n != GD_GS_Length) && (n != MD_MS_Length)) {
        /* GD/GS, MD/MS 以外の応答のとき、戻る */
        urg->errno_ = UrgInvalidResponse;
        lines = 0;
        filled = 0;
        is_echoback = False;
        continue;
      }
      /* 応答コマンド */
      current_type[0] = buffer[0];
      current_type[1] = buffer[1];
      if (! strncmp("ME", current_type, 2)) {
        is_me_type = 1;
      }

      /* 取得設定の初期化 */
      current_first = atoi_substr(&buffer[2], 4);
      current_last = atoi_substr(&buffer[6], 4);
      current_skip_lines = atoi_substr(&buffer[10], 2);

      if ((current_first - store_first) >= data_max) {
        /* 取得範囲が、データサイズに含まれていない */
        return 0;
      }

      /* ダミーデータの配置 */
      dummy_last = current_first - store_first;
      for (filled = 0; filled < dummy_last; ++filled) {
        data[filled] = 0;
      }

      if (n == GD_GS_Length) {
        /* GD/GS コマンドのときは、取得フレーム設定と取得回数設定は無視される */
        urg->remain_times_ = 0;

      } else {
        current_skip_frames = atoi_substr(&buffer[12], 1);
        current_capture_times = atoi_substr(&buffer[13], 2);

        /* MD/MS のときに、残りスキャン回数を格納する */
        urg->remain_times_ = atoi(&buffer[13]);
      }
      current_data_bytes = (current_type[1] == 'S') ? 2 : 3;

    } else if (lines == ReplyCode) {
      /* 応答 */
      if (! strncmp(buffer, "10", 2)) {
        urg->is_laser_on_ = UrgLaserOff;
      }

      /* MD/MS で "00" のときは、送信要求に対する応答なので、*/
      /* もう１行読み出してから、処理をリセットする */
      if (current_type[0] == 'M' && (! strncmp(buffer, "00", 2))) {
        is_echoback = True;
      }

    } else if (lines == Timestamp) {
      /* タイムスタンプ */
      urg->last_timestamp_ = decode(buffer, 4);
    }

    ++lines;
    timeout = EachTimeout;
  }

  /* !!! QT のときは、QT だったとわかる応答を返すようにする */

  if (filled <= 0) {
    return urg->errno_;
  } else {
    return filled;
  }
}


/* データ受信 */
int urg_receiveData(urg_t *urg, long data[], int data_max)
{
  if (! urg_isConnected(urg)) {
    return -1;
  }
  return internal_receiveData(urg, data, data_max,
                              0, data_max, urg->skip_lines_);
}


#if defined(USE_INTENSITY)
int urg_receiveDataWithIntensity(urg_t *urg, long data[], int data_max,
                                 long intensity[])
{
  int i;
  int n;

  n = internal_receiveData(urg, data, data_max,
                           0, data_max, urg->skip_lines_);

  for (i = 0; i < n; i += 2) {
    long length = data[i];

    if ((i + 1) < data_max) {
      long intensity_value = data[i + 1];
      intensity[i] = intensity_value;
      intensity[i + 1] = intensity_value;
      data[i + 1] = length;
    }
  }
  return n;
}
#endif


int urg_receivePartialData(urg_t *urg, long data[], int data_max,
                           int first_index, int last_index)
{
  return internal_receiveData(urg, data, data_max, first_index, last_index, 1);
}


long urg_recentTimestamp(const urg_t *urg)
{
  /* 直前のデータのタイムスタンプを返す */
  return urg->last_timestamp_;
}


double urg_index2rad(const urg_t *urg, int index)
{
  double radian = (2.0 * M_PI) *
    (index - urg->parameters_.area_front_) / urg->parameters_.area_total_;

  return radian;
}


int urg_index2deg(const urg_t *urg, int index)
{
  int degree = (int)floor((urg_index2rad(urg, index) * 180 / M_PI) + 0.5);

  return degree;
}


int urg_rad2index(const urg_t *urg, double radian)
{
  int index =
    (int)floor((((radian * urg->parameters_.area_total_) / (2.0*M_PI))
                + urg->parameters_.area_front_) + 0.5);

  if (index < 0) {
    index = 0;
  } else if (index > urg->parameters_.area_max_) {
    index = urg->parameters_.area_max_;
  }
  return index;
}


int urg_deg2index(const urg_t *urg, int degree)
{
  return urg_rad2index(urg, M_PI * degree / 180.0);
}


int urg_laserOn(urg_t *urg)
{
  /* BM の送信 */
  int expected_ret[] = { 0, 2, -1 };
  int send_n = scip_send(&urg->serial_, "BM\r");
  if (send_n != 3) {
    /* !!! urg->errno = UrgSendFail; */
    return SerialSendFail;
  }
  if (scip_recv(&urg->serial_, "BM", NULL, expected_ret, ScipTimeout) == 0) {
    urg->is_laser_on_ = UrgLaserOn;
  }

  return 0;
}


int urg_laserOff(urg_t *urg)
{
  /* QT の送信 */
  return scip_qt(&urg->serial_, NULL, ScipWaitReply);
}


int urg_reset(urg_t *urg)
{
  int expected_ret[][2] = {
    { 1, -1 },
    { 0, -1 },
  };
  int send_n;
  int recv_n;
  int i;

  urg_laserOff(urg);

  /* RB を２回送信する */
  for (i = 0; i < 2; ++i) {
    send_n = scip_send(&urg->serial_, "RB\r");
    if (send_n != 3) {
      return SerialSendFail;
    }

    recv_n = scip_recv(&urg->serial_, "RB", NULL,
                       expected_ret[i], ScipTimeout);
    if (recv_n < 0) {
      return recv_n;
    }
  }

  /* リセット後、直ちに切断する */
  urg_disconnect(urg);

  return 0;
}


int urg_enableTimestampMode(urg_t *urg)
{
  /* TM0 の送信 */
  int expected_ret[] = { 0, 2, -1 };
  int send_n = scip_send(&urg->serial_, "TM0\r");
  if (send_n != 4) {
    /* !!! urg->errno = UrgSendFail; */
    return SerialSendFail;
  }
  return scip_recv(&urg->serial_, "TM", NULL, expected_ret, ScipTimeout);
}


int urg_disableTimestampMode(urg_t *urg)
{
  /* TM2 の送信 */
  int expected_ret[] = { 0, 3, -1 };
  int send_n = scip_send(&urg->serial_, "TM2\r");
  if (send_n != 4) {
    /* !!! urg->errno = UrgSendFail; */
    return SerialSendFail;
  }
  return scip_recv(&urg->serial_, "TM", NULL, expected_ret, ScipTimeout);
}


long urg_currentTimestamp(urg_t *urg)
{
  char buffer[ScipLineWidth];
  long timestamp = -1;
  int ret = 0;
  int n;

  /* TM1 の送信 */
  int expected_ret[] = { 0, -1 };
  int send_n = scip_send(&urg->serial_, "TM1\r");
  if (send_n != 4) {
    /* !!! urg->errno = UrgSendFail; */
    return SerialSendFail;
  }
  ret = scip_recv(&urg->serial_, "TM", NULL, expected_ret, ScipTimeout);
  if (ret != 0) {
    return ret;
  }

  /* タイムスタンプをデコードして返す */
  n = serial_getLine(&urg->serial_, buffer, ScipLineWidth, ScipTimeout);
  if (n == 5) {
    timestamp = decode(buffer, 4);
  }

  /* 最後の応答を読み捨て */
  n = serial_recv(&urg->serial_, buffer, 1, ScipTimeout);
  if (! serial_isLF(buffer[0])) {
    serial_ungetc(&urg->serial_, buffer[0]);
  }

  return timestamp;
}
