/*!
 \file main.c
 \brief ランダムに配置される車庫にビーゴを駐車するプログラム
 \  Created on: 2011/04/18    Author: ksato
 \  Modified on:2011/04/24    Author: fukuda
 $Id: main.c xxxx 2011/04/24 19:57:30Z fukuda$
*/

/* ライブラリ */
#include <stdio.h>       // printfなどの入出力関数
#include <stdlib.h>      // mallocとfreeやexitなど
#include <signal.h>      // signal
#include <math.h>        // 数学関数
#include "scip2awd.h"    // AWDのURGライブラリ
#include <ypspur.h>      // spur用ライブラリ
#include <GL/glut.h>     // opengl用ライブラリ

//マクロ定義
#if !defined(GLUT_WHEEL_UP)
#  define GLUT_WHEEL_UP   3
#  define GLUT_WHEEL_DOWN 4
#endif

#define RAD(deg) ((deg)*M_PI/180)
#define MAXPOINTS 1000      /* 記憶する点の数　　 */

/* 外部変数 */
S2Port *urgPort;         // デバイスファイル名
S2Sdd_t urgBuf;          // データを確保するバッファ
S2Scan_t *urgData;       // バッファへのポインタ
S2Param_t urgParam;      // URGのパラメータを確保

GLint point[MAXPOINTS][2];  /* 座標を記憶する配列 */

int pointNum = 0;           /* 記憶した座標の数　 */
int near = 1, far = 1;
static int x, y;
int i, ret;
double rad;
double scanArea = RAD(239.77);
double selfArea = RAD(300.33);

int gShutoff;

/*!
 \ func : display
 \ brief: ウィンドウ描画
*/
void display(void)
{
    double x_pos0, y_pos0;
    double x_pos, y_pos;
    glClear(GL_COLOR_BUFFER_BIT);
    glColor3d(1.0, 1.0, 1.0);

        /* 測位データの取り出し*/
    ret=S2Sdd_Begin(&urgBuf, &urgData);

    if(ret > 0)
    {
        for(i=0; i < urgData->size; i++)
        {
        x_pos0 = ( x + (double)urgData->data[i] ) * sin(rad);
        y_pos0 = ( y + (double)urgData->data[i] ) * cos(rad);
        x_pos = ( -x_pos0*cos(-selfArea) - y_pos0*sin(-selfArea));
        y_pos = ( x_pos0*sin(-selfArea) - y_pos0*cos(-selfArea));

            if ( urgData->data[i] > 60 && urgData->data[i] < 4000 )
            {
                glColor3d(1.0, 0.0, 0.0);
                glPointSize(8.0);
                glBegin(GL_POINTS);
                rad = (double)(urgParam.step_min + i - urgParam.step_front)/urgData->size*scanArea;
                /*printf("%d\t%d\t%f\t%f\n", i+44, (int)urgData->data[i], x_pos, y_pos);
                */
                glVertex2d( x_pos, y_pos );
                glEnd();
                glColor3d(0.0, 0.0, 1.0);
                glLineWidth(8.0);
                glBegin(GL_LINES);
                glVertex2d( -60.0*sin(rad), 60.0*cos(rad) );
                glVertex2d( x_pos, y_pos );
                glEnd();
            }
        }
        glutSwapBuffers();

        S2Sdd_End(&urgBuf); // アンロック(読み込み終了)

    } else if (ret == -1) {
        fprintf( stderr, "ERROR: Fatal error occurred.\n" );
    } else usleep(100);        // 測位データがロック中(書込中)

    glutPostRedisplay();
}

/*|
 \func : resize
 \brief: ウィンドウスクリーン
*/
void resize(int w, int h)
{
    glViewport(0, 0, w, h);
    glLoadIdentity();
    glOrtho( -5000.0, 5000.0, -5000.0, 5000.0, -(double)near, (double)far );
    /* 点の大きさをスクリーンの幅に比例させる */
    glPointSize((float)w * 0.1f);
}

/*!
 \func : polarView
 \brief: 視点を変える関数
*/
void polarView(double distance, double twist, double elevation, double azimuth)
{
    glTranslated(0.0, 0.0, -distance);
    glRotated(-twist, 0.0, 0.0, 1.0);
    glRotated(-elevation, 1.0, 0.0, 0.0);
    glRotated(-azimuth, 0.0, 1.0, 0.0);
}

/*!
 \func : keyboard
 \brief: キー設定
*/
static void keyboard(unsigned char key, int x, int y)
{
    switch (key)
    {
    case 'p':
        polarView( 2000.0, 0.0, 30.0, 0.0 );
        break;
    case 'q':
    case 'Q':
    case '\033':
        /* ESC か q か Q をタイプしたら終了 */
        exit(0);
    default:
        break;
    }
}

/*!
 \func : init
 \brief: 初期設定
*/
static void init(void)
{
    /* 初期設定 */
    glClearColor(1.0, 1.0, 1.0, 1.0);

    /* 点にアンチエリアシング処理を行う */
    glEnable(GL_POINT_SMOOTH);

    /* アンチエリアシング処理の品質を最高にする */
    glHint(GL_POINT_SMOOTH_HINT, GL_NICEST);
}

/*!
 \ func : ctrlc
 \ brief: シグナルハンドラの設定．Ctrl+Cでプログラムを終了させる
*/
void ctrlC(int aStatus)
{
    signal(SIGINT, NULL);

    /* URGの測位停止 */
    ret = Scip2CMD_StopMS( urgPort, &urgBuf );
    if( ret == 0 )
    {
        fprintf( stderr, "ERROR: StopMS failed.\n" );
        exit( 0 );
    }
    printf( "Stopped\n" );

    /* バッファの解放 */
    S2Sdd_Dest( &urgBuf );
    printf( "Buffer destructed\n" );

    /* ポートを閉じる */
    Scip2_Close( urgPort );
    printf( "Port closed\n" );

    exit( aStatus );
}
/*!
 \func : setctrlC
 \brief: ctrlCを呼び出す関数
*/
void setctrlC( )
{
    signal( SIGINT, ctrlC );
}

/*!
 \func  :main
 \brief :メインプログラム
*/
int main( int aArgc, char **aArgv )
{
    int cnt = 0;

    /* OpenGL設定 */
    glutInitWindowPosition(50, 50);
    glutInitWindowSize(700, 500);
    glutInit(&aArgc, aArgv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
    glutCreateWindow(aArgv[0]);
    glutDisplayFunc(display);
    glutReshapeFunc(resize);
    glutKeyboardFunc(keyboard);
    init();

    gShutoff = 0;
    aArgv[1] = "/dev/ttyACM0";  // デバイス認識

    setctrlC( );     // シグナルハンドラの設定

    /**********************spur関連****************************/
    Spur_init( );                 // 初期化
    Spur_set_vel( 0.15 );          // 最大速度0.15m/sに設定
    Spur_set_accel( 1.0 );         // 加速度1.0m/sに設定
    Spur_set_angvel( 0.25 );       // 最大角速度0.25rad/sに設定
    Spur_set_angaccel( 2.0 );      // 各加速度2.0rad/ssに設定
    Spur_set_pos_GL( 0, 0, 0 );      // スタート地点を原点にGL座標を設定
    /*********************************************************/


    /**********************URG関連****************************/
    /* ポートオープン */
    urgPort = Scip2_Open( aArgv[1], B115200 );    //デバイス名,ボーレート設定

    if ( urgPort == 0 )
    {
        fprintf( stderr, "ERROR: Failed to open device.\n" );
        return 0;
    }
    printf( "Port opened\n" );

    /* バッファの初期化 */
    S2Sdd_Init( &urgBuf );
    printf( "Buffer initialized\n" );

    /* URGパラメータの読み出し */
    Scip2CMD_PP( urgPort, &urgParam );

    /* 垂れ流しモードの開始 */
    Scip2CMD_StartMS( urgPort, urgParam.step_min, urgParam.step_max, 1, 0, 0, &urgBuf, SCIP2_ENC_2BYTE );

    Spur_line_GL( 0, 0, 0 ); // 直進

    while ( !gShutoff )
    {
        /* 測位データの取り出し */
        ret = S2Sdd_Begin( &urgBuf, &urgData );

        cnt++;
        if ( cnt == 1 ) printf( "\nAcquring sencing data Until obstacles approach ...\n\n" );

        if ( ret > 0 )
        {
            /* 最初の壁検出からポールへ、その後、車庫？の壁までの移動 */
            if ( urgData -> data[urgParam.step_front] <= 500 )
            {
                printf("\nNow Checking ...\n\n");
                Spur_free( );
                usleep( 100000 );
                Spur_spin_GL( - M_PI / 2.0 );
                while ( !Spur_near_ang_GL( - M_PI / 2.0, 0.1 ) ) printf("SPIN_GL -> NEAR_ANG_GL\n");
                Spur_orient_FS( 0 );
                Spur_set_pos_LC( 0, 0, 0 );
                while ( (urgData -> data[urgParam.step_front*(int)sin(1.5)]) < 500 )
                {
                    if( Spur_near_pos_LC( 0.45, 0, 0 ) )
                    {
                        Spur_free( );
                        usleep(1000000);
                        Spur_spin_GL( - M_PI / 2.0 );
                        while ( !Spur_near_ang_GL( - M_PI, 1.0 ) ) printf("SPIN_GL -> NEAR_ANG_GL\n");
                        Spur_orient_FS( 0 );
                    }
                }
            }

            /* 左と前に壁がある環境からの車庫？までの移動 */
            else if ( urgData -> data[urgParam.step_front*(int)sin(1.5)] <= 5000 && urgData -> data[urgParam.step_front] <= 500 )
            {
                Spur_free( );
                usleep( 1000000 );
                Spur_spin_GL( - M_PI / 2.0 );
                while ( !Spur_near_ang_GL( - 3.0 * M_PI / 2.0, 0.1 ) ) printf("SPIN_GL -> NEAR_ANG_GL\n");
                Spur_set_pos_LC( 0, 0, 0 );
                usleep( 1000000 );
                Spur_orient_FS( 0 );

                /* 車庫①判定 */
                while ( !Spur_stop_line_LC( 1.5, 0, 0.1 ) ) printf("ORIENT_FS -> STOP_LINE_LC\n");
                Spur_free( );
                usleep( 1000000 );
                Spur_circle_LC( 1.5, 0.45, 0.1 );
                while ( !Spur_near_ang_LC( M_PI, 0.1 ) ) printf("CIRCLE_LC -> NEAR_ANG_LC\n");
                Spur_free( );
                usleep( 1000000 );
                Spur_orient_FS( 0 );
            }

                /* 車庫①or②判定 */
            else if ( urgData -> data[urgParam.step_front] < 300 && urgData -> data[urgParam.step_front*(int)cos(1.5)] < 300 )
            {
                if( urgData -> data[urgParam.step_front*(int)cos(1.5)] < 300 || urgData -> data[urgParam.step_front*(int)sin(1.5)] < 300 )
                {
                    Spur_stop( );
                    gShutoff = 1;
                    usleep( 100 );
                }
            }

        S2Sdd_End( &urgBuf ); // アンロック(読み込み終了)

        } else if ( ret == - 1 )
        {
            fprintf( stderr, "ERROR: Fatal error occurred.\n" );

        } else usleep( 100 );        // 測位データがロック中(書込中)
    }

    glutMainLoop();

    ret = Scip2CMD_StopMS( urgPort, &urgBuf ); // URGの測位停止
    if( ret == 0 )
    {
        fprintf( stderr, "ERROR: StopMS failed.\n" );
        return 0;
    }
    printf( "Stopped\n" );

    S2Sdd_Dest( &urgBuf );      // バッファの解放
    printf( "Buffer destructed\n" );

    Scip2_Close( urgPort );     // ポートを閉じる
    printf( "Port closed\n" );

    //glutMainLoop(); // opengl 無限ループ

    return 0;
}