/*!
    \file
    \brief Beego用 走行プログラム

    $Id: xxxx test_run.c 2011-04-15 17:00:xxx fukuda $

    \グローバルをGL、ローカルをLC、フロントサイドをFS、と以下略す
*/

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <math.h>
#include <ypspur.h>
#include "test_run.h"

int main( int argc, char *argv[] )
{
    double angle, x, y, theta;
    FILE *gp;
    gp = popen( "gnuplot", "w" );

    /* 初期化 */
    setSigInt( );
    Spur_init( );
    if ( !Spur_init( ) )
    {
        fprintf( stderr, "ERROR : cannot open spur\n" );
        return -1;
    }

    /* 走行パラメータ設定 */
    Spur_set_vel( 0.2 );
    Spur_set_accel( 0.3 );
    Spur_set_angvel( 1.0 );
    Spur_set_angaccel( 1.0 );

    /* 初期位置設定 */
    Spur_spin_GL( - 1.0 );
    usleep( 300000 );
    Spur_set_pos_GL( 0, 0, 0 );

    /* 直線走行 */
    printf( "line\n" );
    Spur_line_GL( 0, 0, 0 ); // 直線走行
    while (!Spur_over_line_GL( 2.55, 0, 0 ) ) plot_pos( gp ); // GL座標（2.55, 0）を越えると0を返す

    /* LC座標を零点調整 */
    Spur_set_pos_LC( 0, 0, 0 );

    /* 円弧走行 */
    printf( "rotate\n" );
    Spur_circle_GL( 2.55, 0.45, 0.45 ); // GL座標（2.55, 0.45）を中心とする半径45cmの円弧を左周りに旋回走行
    while (Spur_over_line_LC( 0, 0.45, - M_PI / 4.0 ) ) plot_pos( gp ); // LC座標（0, 0.45）を通り、角度45°に垂直な直線を通ると1を返す
    while (!Spur_near_ang_GL( 1.1, 0.1 ) ) plot_pos( gp ); // GL座標上、角度90°を越えると0を返す

    /* 直線走行 */
    printf( "line\n" );
    Spur_orient_FS( 0 );
    while ( !Spur_over_line_GL( 2.25, 2.25, M_PI/2.0 ) ) plot_pos( gp ); // GL座標（2.55, 0）まで走行　　

    /* 走行停止 */
    printf( "stop\n" );
    Spur_stop( );
    usleep( 4000000 );
    Spur_free( );

    /* 停止後の位置取得 */
    while ( 1 ) {
        Spur_get_pos_GL( &x, &y, &theta );
        printf( "%f %f %f\n", x, y, theta * 180.0 / M_PI );
        usleep( 1000000 );
    }

    pclose( gp );
    return 0;
}