/*!
  \file
  \brief Win32 を用いた URG データの取得サンプル

  \author Satofumi KAMIMURA

  $Id: capture_sample.cpp 982 2009-06-10 08:18:45Z satofumi $

  コンパイル・実行方法
  - Visual Studio の場合
  - capture_sample.zip を展開してできるフォルダ内の capture_sample.sln を選択
  - Visual Studio が起動したら、F5 を押してビルドと実行を行う
  - COM ポートが見つからない場合は、main 内の com_port を変更すること

  - MinGW, Cygwin などの場合
  - % g++ capture_sample.cpp -o capture_sample
  - % ./capture_sample
  - COM ポートが見つからない場合は、main 内の com_port を変更すること

  \attention main() 中の com_port, com_baudrate は適切に変更すること
  \attention プログラム使用に伴う損害等の責任は負わないものとする
  \attention バグ報告は歓迎いたします
*/

#define _CRT_SECURE_NO_WARNINGS

#include <windows.h>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <string>

using namespace std;


// SCIP の出力を記録するときは、RAW_OUTPUT を有効にする
//#define RAW_OUTPUT

#if defined(RAW_OUTPUT)
static FILE* Raw_fd_ = NULL;
#endif


enum {
  Timeout = 1000,               // [msec]
  EachTimeout = 2,              // [msec]
  LineLength = 64 + 3 + 1 + 1 + 1 + 16,
};

static HANDLE HCom = INVALID_HANDLE_VALUE;
static int ReadableSize = 0;
static char* ErrorMessage = "no error.";


/*!
  \brief センサ情報の管理
*/
typedef struct
{
  enum {
    MODL = 0,                   //!< センサ型式情報
    DMIN,                       //!< 最小計測可能距離 [mm]
    DMAX,                       //!< 最大計測可能距離 [mm]
    ARES,                       //!< 角度分解能(360度の分割数)
    AMIN,                       //!< 最小計測可能方向値
    AMAX,                       //!< 最大計測可能方向値
    AFRT,                       //!< 正面方向値
    SCAN,                       //!< 標準操作角速度
  };
  string model;            //!< 取得した MODL 情報
  long distance_min;            //!< 取得した DMIN 情報
  long distance_max;            //!< 取得した DMAX 情報
  int area_total;               //!< 取得した ARES 情報
  int area_min;                 //!< 取得した AMIN 情報
  int area_max;                 //!< 取得した AMAX 情報
  int area_front;               //!< 取得した AFRT 情報
  int scan_rpm;                 //!< 取得した SCAN 情報

  int first;                    //!< 測定開始位置
  int last;                     //!< 測定終了位置
  int max_size;                 //!< データの最大サイズ
  long last_timestamp; //!< 最後にデータを取得したときのタイムスタンプ
} urg_state_t;


// 待機
static void delay(int msec)
{
  Sleep(msec);
}


static int com_changeBaudrate(long baudrate)
{
  DCB dcb;

  GetCommState(HCom, &dcb);
  dcb.BaudRate = baudrate;
  dcb.ByteSize = 8;
  dcb.Parity = NOPARITY;
  dcb.fParity = FALSE;
  dcb.StopBits = ONESTOPBIT;
  SetCommState(HCom, &dcb);

  return 0;
}


// シリアル送受信
static int com_connect(const char* device, long baudrate)
{
#if defined(RAW_OUTPUT)
  Raw_fd_ = fopen("raw_output.txt", "w");
#endif

  char adjust_device[16];
  _snprintf(adjust_device, 16, "\\\\.\\%s", device);
  HCom = CreateFile(adjust_device, GENERIC_READ | GENERIC_WRITE, 0,
                     NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);

  if (HCom == INVALID_HANDLE_VALUE) {
    return -1;
  }

  // ボーレート設定
  return com_changeBaudrate(baudrate);
}


static void com_disconnect(void)
{
  if (HCom != INVALID_HANDLE_VALUE) {
    CloseHandle(HCom);
    HCom = INVALID_HANDLE_VALUE;
  }
}


static int com_send(const char* data, int size)
{
  DWORD n;
  WriteFile(HCom, data, size, &n, NULL);
  return n;
}


static int com_recv(char* data, int max_size, int timeout)
{
  if (max_size <= 0) {
    return 0;
  }

  /* 要求サイズ分のデータ受信可能ならば、読み出して返す */
  if (ReadableSize < max_size) {
    DWORD dwErrors;
    COMSTAT ComStat;
    ClearCommError(HCom, &dwErrors, &ComStat);
    ReadableSize = ComStat.cbInQue;
  }

  if (max_size > ReadableSize) {
    COMMTIMEOUTS pcto;
    int each_timeout = 2;

    if (timeout == 0) {
      /* 存在するだけを読み出させて返す */
      max_size = ReadableSize;

    } else {
      if (timeout < 0) {
	/* タイムアウトの値をゼロにすると、タイマーは動作せず受信を待ち続ける */
	timeout = 0;
	each_timeout = 0;
      }

      /* タイムアウトを設定 */
      GetCommTimeouts(HCom, &pcto);
      pcto.ReadIntervalTimeout = timeout;
      pcto.ReadTotalTimeoutMultiplier = each_timeout;
      pcto.ReadTotalTimeoutConstant = timeout;
      SetCommTimeouts(HCom, &pcto);
    }
  }

  DWORD n;
  ReadFile(HCom, data, (DWORD)max_size, &n, NULL);
#if defined(RAW_OUTPUT)
  if (Raw_fd_) {
    for (int i = 0; i < n; ++i) {
      fprintf(Raw_fd_, "%c", data[i]);
    }
    // 終了させるタイミングがないため、毎回フラッシュしてしまう
    fflush(Raw_fd_);
  }
#endif
  if (n > 0) {
    ReadableSize -= n;
  }

  return n;
}


// URG に対してコマンド送信を行う
static int urg_sendTag(const char* tag)
{
  char send_message[LineLength];
  _snprintf(send_message, LineLength, "%s\n", tag);
  int send_size = (int)strlen(send_message);
  com_send(send_message, send_size);

  return send_size;
}


// URG から改行までのデータを読み出す
static int urg_readLine(char *buffer)
{
  int i;
  for (i = 0; i < LineLength -1; ++i) {
    char recv_ch;
    int n = com_recv(&recv_ch, 1, Timeout);
    if (n <= 0) {
      if (i == 0) {
        return -1;              // タイムアウト
      }
      break;
    }
    if ((recv_ch == '\r') || (recv_ch == '\n')) {
      break;
    }
    buffer[i] = recv_ch;
  }
  buffer[i] = '\0';

  return i;
}


// URG に対してコマンドを送り、応答を待つ
static int urg_sendMessage(const char* command, int timeout, int* recv_n)
{
  int send_size = urg_sendTag(command);
  int recv_size = send_size + 2 + 1 + 2;
  char buffer[LineLength];

  int n = com_recv(buffer, recv_size, timeout);
  *recv_n = n;

  if (n < recv_size) {
    // 受信サイズが合わない場合
    return -1;
  }

  if (strncmp(buffer, command, send_size -1)) {
    // command が一致しない場合
    return -1;
  }

  // !!! 可能ならば、ここでチェックサムの正当性を確認すべき

  // 応答の文字列を、16進数化して返す
  char reply_str[3] = "00";
  reply_str[0] = buffer[send_size];
  reply_str[1] = buffer[send_size + 1];
  return strtol(reply_str, NULL, 16);
}


// ボーレート変更
static int urg_changeBaudrate(long baudrate)
{
  char buffer[] = "SSxxxxxx\r";
  _snprintf(buffer, 10, "SS%06d\r", baudrate);
  int dummy = 0;
  int ret = urg_sendMessage(buffer, Timeout, &dummy);

  if ((ret == 0) || (ret == 3) || (ret == 4)) {
    return 0;
  } else {
    return -1;
  }
}


// URG パラメータの読み出し
static int urg_getParameters(urg_state_t* state)
{
  // パラメータ読み出しと反映
  urg_sendTag("PP");
  char buffer[LineLength];
  int line_index = 0;
  enum {
    TagReply = 0,
    DataReply,
    Other,
  };
  int line_length;
  for (; (line_length = urg_readLine(buffer)) > 0; ++line_index) {

    if (line_index == Other + urg_state_t::MODL) {
      buffer[line_length - 2] = '\0';
      state->model = &buffer[5];

    } else if (line_index == Other + urg_state_t::DMIN) {
      state->distance_min = atoi(&buffer[5]);

    } else if (line_index == Other + urg_state_t::DMAX) {
      state->distance_max = atoi(&buffer[5]);

    } else if (line_index == Other + urg_state_t::ARES) {
      state->area_total = atoi(&buffer[5]);

    } else if (line_index == Other + urg_state_t::AMIN) {
      state->area_min = atoi(&buffer[5]);
      state->first = state->area_min;

    } else if (line_index == Other + urg_state_t::AMAX) {
      state->area_max = atoi(&buffer[5]);
      state->last = state->area_max;

    } else if (line_index == Other + urg_state_t::AFRT) {
      state->area_front = atoi(&buffer[5]);

    } else if (line_index == Other + urg_state_t::SCAN) {
      state->scan_rpm = atoi(&buffer[5]);
    }
  }

  if (line_index <= Other + urg_state_t::SCAN) {
    return -1;
  }
  // 必要なデータサイズの計算
  state->max_size = state->area_max +1;

  return 0;
}


/*!
  \brief URG への接続

  \param state [o] センサ情報
  \param port [i] デバイス
  \param baudrate [i] ボーレート [bps]

  \retval 0 正常終了
  \retval < 0 エラー
*/
static int urg_connect(urg_state_t* state,
                       const char* port, const long baudrate)
{
  static char message_buffer[LineLength];

  if (com_connect(port, baudrate) < 0) {
    _snprintf(message_buffer, LineLength,
              "Cannot connect COM device: %s", port);
    ErrorMessage = message_buffer;
    return -1;
  }

  const long try_baudrate[] = { 19200, 115200, 38400 };
  size_t n = sizeof(try_baudrate) / sizeof(try_baudrate[0]);
  for (size_t i = 0; i < n; ++i) {

    // ボーレートを変えながら、通信できるボーレートを探す
    if (com_changeBaudrate(try_baudrate[i])) {
      ErrorMessage = "change baudrate fail.";
      return -1;
    }

    // SCIP2.0 モードに変更
    int recv_n = 0;
    urg_sendMessage("SCIP2.0", Timeout, &recv_n);
    if (recv_n <= 0) {
      // ボーレートの違いにより応答がないとみなし、次のボーレートを試す
      continue;
    }

    // 指定されたボーレートと異なる場合、変更する
    if (try_baudrate[i] != baudrate) {
      urg_changeBaudrate(baudrate);

      // 変更後、測定周期の１周分後に変更が反映させる
      // よって、ここで PC 側のボーレートを変更する前に待機すべき
      delay(100);

      com_changeBaudrate(baudrate);
    }

    // パラメータの読み出し
    if (urg_getParameters(state) < 0) {
      ErrorMessage =
        "PP command fail.\n"
	"This COM device may be not URG, or URG firmware is too old.\n"
        "SCIP 1.1 protocol is not supported. Please update URG firmware.";
      return -1;
    }
    state->last_timestamp = 0;

    // URG の検出に成功
    return 0;
  }

  // URG の検出に失敗
  ErrorMessage = "no urg ports.";
  return -1;
}


/*!
  \brief 切断
*/
static void urg_disconnect(void)
{
  com_disconnect();
}


/*!
  \brief GD コマンドを用いたデータ取得

  \param state [i] センサ情報

  \retval 0 正常終了
  \retval < 0 エラー
*/
static int urg_captureByGD(const urg_state_t* state)
{
  char send_message[LineLength];
  _snprintf(send_message, LineLength,
            "GD%04d%04d%02d", state->first, state->last, 1);

  return urg_sendTag(send_message);
}


/*!
  \brief MD を用いたデータ取得

  \param state [i] センサ情報
  \param capture_times [i] 測定回数

  \retval 0 正常終了
  \retval < 0 エラー
*/
static int urg_captureByMD(const urg_state_t* state, int capture_times)
{
  // 100 回を超えるデータ取得に対しては、回数に 00 (無限回取得)を指定し、
  // QT or RS コマンドでデータ取得を停止すること
  if (capture_times >= 100) {
    capture_times = 0;
  }

  char send_message[LineLength];
  _snprintf(send_message, LineLength, "MD%04d%04d%02d%01d%02d",
            state->first, state->last, 1, 0, capture_times);

  return urg_sendTag(send_message);
}


// URG の 6bit データをデコード
static long urg_decode(const char data[], int data_byte)
{
  long value = 0;
  for (int i = 0; i < data_byte; ++i) {
    value <<= 6;
    value &= ~0x3f;
    value |= data[i] - 0x30;
  }
  return value;
}


// URG 距離データの受信
static int urg_addRecvData(const char buffer[], long data[], int* filled)
{
  static int remain_byte = 0;
  static char remain_data[3];
  const int data_byte = 3;

  const char* pre_p = buffer;
  const char* p = pre_p;

  if (*filled <= 0) {
    remain_byte = 0;
  }

  if (remain_byte > 0) {
    memmove(&remain_data[remain_byte], buffer, data_byte - remain_byte);
    data[*filled] = urg_decode(remain_data, data_byte);
    ++(*filled);
    pre_p = &buffer[data_byte - remain_byte];
    p = pre_p;
    remain_byte = 0;
  }

  do {
    ++p;
    if ((p - pre_p) >= static_cast<int>(data_byte)) {
      data[*filled] = urg_decode(pre_p, data_byte);
      ++(*filled);
      pre_p = p;
    }
  } while (*p != '\0');
  remain_byte = (int)(p - pre_p);
  memmove(remain_data, pre_p, remain_byte);

  return 0;
}


static int checkSum(char buffer[], int size, char actual_sum)
{
  char expected_sum = 0x00;
  int i;

  for (i = 0; i < size; ++i) {
    expected_sum += buffer[i];
  }
  expected_sum = (expected_sum & 0x3f) + 0x30;

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


/*!
  \brief URG データの受信

  測定データを配列に格納し、格納データ数を戻り値で返す。

  \param state [i] センサ情報
  \param data [o] 測定データ
  \param max_size [i] データバッファサイズ

  \retval >= 0 測定データ数
  \retval < 0 エラー
*/
static int urg_receiveData(urg_state_t* state, long data[], size_t max_size)
{
  int filled = 0;

  // first または min までの領域を 19(計測有効範囲外)で埋める
  for (int i = state->first -1; i >= 0; --i) {
    data[filled++] = 19;
  }

  char message_type = 'M';
  char buffer[LineLength];
  int line_length;
  for (int line_count = 0; (line_length = urg_readLine(buffer)) >= 0; ++line_count) {

    // チェックサムの確認
    if ((line_count > 3) && (line_length >= 3)) {
      if (checkSum(buffer, line_length - 1, buffer[line_length - 1]) < 0) {
	fprintf(stderr, "line_count: %d: %s\n", line_count, buffer);
        return -1;
      }
    }

    if ((line_count >= 6) && (line_length == 0)) {

      // データ受信の完了
      for (size_t i = filled; i < max_size; ++i) {
        // データ末尾までを 19(計測有効範囲外)で埋める
        data[filled++] = 19;
      }
      return filled;

    } else if (line_count == 0) {
      // 送信メッセージの最初の文字でメッセージの判定を行う
      if ((buffer[0] != 'M') && (buffer[0] != 'G')) {
        return -1;
      }
      message_type = buffer[0];

    } else if (! strncmp(buffer, "99b", 3)) {
      // "99b" を検出し、以降を「タイムスタンプ」「データ」とみなす
      line_count = 4;

    } else if ((line_count == 1) && (message_type == 'G')) {
      line_count = 4;

    } else if (line_count == 4) {
      // "99b" 固定
      if (strncmp(buffer, "99b", 3)) {
        return -1;
      }

    } else if (line_count == 5) {
      state->last_timestamp = urg_decode(buffer, 4);

    } else if (line_count >= 6) {
      // 取得データ
      if (line_length > (64 + 1)) {
        line_length = (64 + 1);
      }
      buffer[line_length -1] = '\0';
      int ret = urg_addRecvData(buffer, data, &filled);
      if (ret < 0) {
        return ret;
      }
    }
  }
  return -1;
}


void outputData(long data[], int n, size_t total_index)
{
  char output_file[] = "data_xxxxxxxxxx.csv";
  _snprintf(output_file, sizeof(output_file), "data_%03d.csv", total_index);
  FILE* fd = fopen(output_file, "w");
  if (! fd) {
    perror("fopen");
    return;
  }

  for (int i = 0; i < n; ++i) {
    fprintf(fd, "%ld, ", data[i]);
  }
  fprintf(fd, "\n");

  fclose(fd);
}


int main(int argc, char *argv[])
{
  // COM ポート設定
  // !!! 各自の環境に合わせて COM 設定を変更すること
  const char com_port[] = "COM3";
  const long com_baudrate = 115200;

  // URG に接続
  urg_state_t urg_state;
  int ret = urg_connect(&urg_state, com_port, com_baudrate);
  if (ret < 0) {
    // エラーメッセージを出力して終了
    printf("urg_connect: %s\n", ErrorMessage);

    // 即座に終了しないための処理。不要ならば削除すること
    getchar();
    exit(1);
  }

  int max_size = urg_state.max_size;
  long* data = new long [max_size];

  enum { CaptureTimes = 5 };
  size_t total_index = 0;

  //////////////////////////////////////////////////////////////////////
  // GD コマンドを用いたデータ取得
  printf("using GD command\n");

  // GD コマンドでのデータ取得の場合には、BM コマンドでのレーザ点灯が必要
  int recv_n = 0;
  urg_sendMessage("BM", Timeout, &recv_n);

  for (int i = 0; i < CaptureTimes; ++i) {
    urg_captureByGD(&urg_state);
    int n = urg_receiveData(&urg_state, data, max_size);
    if (n > 0) {
      printf("% 3d: front: %ld, urg_timestamp: %ld\n",
             i, data[urg_state.area_front], urg_state.last_timestamp);

      outputData(data, n, ++total_index);
    }
  }
  printf("\n");

  /////////////////////////////////////////////////////////////////////
  // MD コマンドを用いたデータ取得
  printf("using MD command\n");

  urg_captureByMD(&urg_state, CaptureTimes);
  for (int i = 0; i < CaptureTimes; ++i) {
    int n = urg_receiveData(&urg_state, data, max_size);
    if (n > 0) {
      printf("% 3d: front: %ld, urg_timestamp: %ld\n",
             i, data[urg_state.area_front], urg_state.last_timestamp);

      outputData(data, n, ++total_index);
    }
  }
  // MD コマンドでの取得が完了すると、レーザは自動消灯する

  // ただし、100 回以上のデータ取得を指定した場合には、
  // urg_captureByMD() 内部で無限回のデータ取得に設定されているので、
  // QT コマンドを用いて、明示的にデータ停止を行う
  if (CaptureTimes >= 100) {
    int dummy;
    urg_sendMessage("QT", Timeout, &dummy);
  }

  urg_disconnect();
  delete [] data;

  printf("end.\n");

  // 即座に終了しないための処理。不要ならば削除すること
  getchar();
  return 0;
}
