// \~japanese シリアル接続でのセンサとの接続と距離データの取得
// \~english Connects to the sensor via serial interface and gets range data

#include "urg_sensor.h"
#include "urg_utils.h"
#include <stdlib.h>


int main(void)
{
    urg_t urg;
    int ret;
    long *length_data;
    int length_data_size;

    // \~japanese "COM1" は、センサが認識されているデバイス名にする必要がある
    // \~english "COM1" is, in this case, the device name detected for the sensor
    const char connect_device[] = "COM1";
    const long connect_baudrate = 115200;

    // \~japanese センサに対して接続を行う。
    // \~english Connects to the sensor
    ret = urg_open(&urg, URG_SERIAL, connect_device, connect_baudrate);
    // \todo check error code

    // \~japanese データ受信のための領域を確保する
    // \~english Allocates memory to hold received measurement data
    length_data = (long *)malloc(sizeof(long) * urg_max_data_size(&urg));
    // \todo check length_data is not NULL

    // \~japanese 距離データの計測開始。
    // \~english Starts range data measurement
    ret = urg_start_measurement(&urg, URG_DISTANCE, 1, 0);
    // \todo check error code

    // \~japanese センサから距離データを取得する。
    // \~english Receives the measurement data
    length_data_size = urg_get_distance(&urg, length_data, NULL);
    // \todo process length_data array

    // \~japanese センサとの接続を閉じる。
    // \~english Disconnects from the sensor
    urg_close(&urg);

    return 0;
}

for (i = 0; i < length_data_size; ++i) {
    printf("%ld, ", length_data[i]);
}
printf("\n"); 

// \~japanese イーサーネット接続でのセンサとの接続と距離データの取得
// \~english Connects to the sensor via Ethernet and receives range data

const char connect_address[] = "192.168.0.10";
const long connect_port = 10940;

// \~japanese センサに対して接続を行う。
// \~english Connects to the sensor
ret = urg_open(&urg, URG_ETHERNET, connect_address, connect_port);
// \todo check error code

// \~japanese scan_times 回のスキャンデータを取得
// \~english Obtains measurement data for scan_times scans

// \~japanese urg_start_measurement() 関数でスキャン回数を指定し
// \~english Uses urg_start_measurement() function to define the number of scans
// \~japanese urg_get_distance() 関数で指定した回数だけデータを受信する。
// \~english Uses urg_get_distance() function to receive the measurement data

const int scan_times = 123;
int length_data_size;
int i;

// \~japanese センサから距離データを取得する。
// \~english Starts range data measurement
ret = urg_start_measurement(&urg, URG_DISTANCE, scan_times, 0);
// \todo check error code

for (i = 0; i < scan_times; ++i) {
    length_data_size = urg_get_distance(&urg, length_data, NULL);
    // \todo process length_data array
}

// \~japanese 計測パラメータの設定
// \~english Configures measurement parameters

// \~japanese センサに対して接続を行う。
// \~japanese 接続を行うと、計測パラメータの設定は初期化される
// \~english Connects to the sensor
// \~english Upon connection, measurement parameters are initialized (default values)
ret = urg_open(&urg, URG_SERIAL, connect_device, connect_baudrate);
// \todo check error code

// \~japanese 計測範囲を指定する
// \~japanese センサ正面方向の 90 [deg] 範囲のデータ取得を行い、ステップ間引きを行わない例
// \~english Defines the measurement scope (start, end steps)
// \~english Defines a measurement scope of 90 [deg] at the front of the sensor, and no step grouping in this example
first_step = urg_deg2step(&urg, -45);
last_step = urg_deg2step(&urg, +45);
skip_step = 0;
ret = urg_set_scanning_parameter(&urg, first_step, last_step, skip_step);
// \todo check error code

// \~japanese 計測回数と計測の間引きを指定して、計測を開始する
// \~japanese 123 回の計測を指示し、スキャンの間引きを行わない例
// \~english Defines the number of scans
// \~english 123 scans are requested, and no scan skipping in this example
scan_times = 123;
skip_scan = 0;
ret = urg_start_measurement(&urg, URG_DISTANCE, scan_times, skip_scan);
// \todo check error code

// \~japanese タイムスタンプの取得
// \~english Gets timestamp values

// \~japanese urg_get_distance() 関数に変数を与え、タイムスタンプを取得する。
// \~english Uses the urg_get_distance() function and returns the timestamp values for each scan

const int scan_times = 123;
int length_data_size;
long timestamp;
int i;

// \~japanese センサから距離データを取得する。
// \~english Starts range data measurement
ret = urg_start_measurement(&urg, URG_DISTANCE, scan_times, 0);
// \todo check error code

for (i = 0; i < scan_times; ++i) {
    length_data_size = urg_get_distance(&urg, length_data, &timestamp);
    // \todo process length_data array

    // \~japanese 取得したタイムスタンプを出力する
    // \~english Outputs the received timestamp value
    printf("%ld\n", timestamp);
}

// \~japanese 距離データを X-Y 座標系に変換して表示する
// \~english Converts data to X-Y coordinates and displays it

length_data_size = urg_get_distance(&urg, length_data, NULL);
for (i = 0; i < length_data_size; ++i) {
    // \~japanese その距離データのラジアン角度を求め、X, Y の座標値を計算する
    // \~english Gets the angle in radians for range data, and convert to X-Y coordinates
    double radian;
    long length;
    long x;
    long y;

    radian = urg_index2rad(&urg, i);
    length = length_data[i];
    // \todo check length is valid

    x = (long)(length * cos(radian));
    y = (long)(length * sin(radian));
    printf("(%ld, %ld), ", x, y);
}
printf("\n");