/////////////////////////////////////////////////

/////////////////////////////////////////////////
#include <ros/ros.h>

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>

#define _USE_MATH_DEFINES
#include <math.h>
#include <GL/glut.h>
#include <GL/glui.h>
#include <GL/gl.h>
#include <GL/glu.h>

//#include "GLMetaseq.h"
//MQO_MODEL gl_mqoModel;	// MQO繝｢繝�Ν

#include <iostream>
using namespace std;

#include "gl_main.h"
#include "geometory.h"
#include "opencv_inc.h"
#include <float.h>
//Mainwindow
int xBegin, yBegin;                           //mouse縺ｮXY蠎ｧ讓�
int mButton;                                  //mouse縺ｮClick
float distance_gl, twist, elevation, azimuth; //繧ｫ繝｡繝ｩ縺ｮ蝗櫁ｻ｢陦悟�
float xOrig, yOrig, zOrig;                    //繧ｫ繝｡繝ｩ縺ｮ菴咲ｽｮ

//Subwindow
float distance_gl2, twist2, elevation2, azimuth2;
float xOrig2 = 0.0, yOrig2 = 0.0, zOrig2 = 0.0;
int mButton2;
int xBegin2, yBegin2;

int WinID[2]; //繧ｦ繧｣繝ｳ繝峨えID
int WindowNum = 0;
int WinFlag[2];
const char *WindowName[] = {"Sim_Main", "Camera"};

//Robot蛻ｶ蠕｡縺ｮ繝代Λ繝｡繝ｼ繧ｿ
struct ROBOT myrobot;
struct URG urg_area;
struct URG urg_data;
struct URG urg_data2;
struct URG urg_data3;
struct OBSTACLE obst;
struct MOVE_OBSTACLE move_obst;

extern double pantilt_pan_rot_speed;
extern double pantilt_pan_init_deg;
extern double pantilt_tilt_rot_speed;
extern double pantilt_tilt_rot_sin_amp;
extern double pantilt_tilt_rot_sin_speed;
extern double pantilt_tilt_init_deg;

extern ros::Time lrf_scantime;
extern ros::Time pantilt_time;

IplImage *capture_video_buf;
IplImage *capture_video;

int save_flg = 0;
double robot_z = 0.0;

struct pantilt pantable;
int color_no = 0;

int wait_sleep(double time)
{
    usleep(time * 1000.0);

    return 0;
}

static double gettimeofday_sec()
{
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return tv.tv_sec + (double)tv.tv_usec * 1e-6;
}

void robot_view(struct ROBOT &robot)
{

    double robot_height = 2.0;
    double xx, yy, theta;
    xx = robot.x;
    yy = robot.y;
    theta = robot.theta;
    static double radius = 0.5;
    glColor3f(0, 0, 0); //Yellow
    int grid = 24.0;

    glLineWidth(2.0);
    //繝ｭ繝懊ャ繝医�蜑肴婿繧堤ｷ壼�縺ｧ陦ｨ迴ｾ
    glBegin(GL_LINES);
    glVertex3f_p(xx, yy, 0.0);
    glVertex3f_p(cos(theta + M_PI / 2.0) * radius + xx, sin(theta + M_PI / 2.0) * radius + yy, 0.0);
    glEnd();
    glBegin(GL_LINES);
    glVertex3f_p(xx, yy, 2.0);
    glVertex3f_p(cos(theta + M_PI / 2.0) * radius + xx, sin(theta + M_PI / 2.0) * radius + yy, robot_height);
    glEnd();

    glBegin(GL_LINE_LOOP);
    glVertex3f_p(xx, yy, 0.0);
    glVertex3f_p(xx, yy, 2.0);
    glEnd();

    //glBegin(GL_LINE_LOOP);
    //glVertex3f_p(xx-radius, yy-radius, 2.0);
    //glVertex3f_p(xx+radius, yy-radius, 2.0);
    //glVertex3f_p(xx+radius, yy+radius, 2.0);
    //glVertex3f_p(xx-radius, yy+radius, 2.0);
    //glEnd();

    //繝ｭ繝懊ャ繝医ｒ蜀�ｽ｢縺ｧ陦ｨ迴ｾ
    for (int i = 0; i < grid; i++)
    {
        double rad_1 = 2.0 * M_PI * i / grid + theta;
        double rad_2 = 2.0 * M_PI * (i + 1) / grid + theta;

        //蠎暮擇
        glBegin(GL_LINES);
        glVertex3f_p(cos(rad_1) * radius + xx, sin(rad_1) * radius + yy, 0.0);
        glVertex3f_p(cos(rad_2) * radius + xx, sin(rad_2) * radius + yy, 0.0);
        glEnd();

        //螟ｩ莠�
        glBegin(GL_LINES);
        glVertex3f_p(cos(rad_1) * radius + xx, sin(rad_1) * radius + yy, robot_height);
        glVertex3f_p(cos(rad_2) * radius + xx, sin(rad_2) * radius + yy, robot_height);
        glEnd();
    }

    //Dummy縺ｮ菴咲ｽｮ繧定｡ｨ遉ｺ(Odometory隱､蟾ｮ縺ゅｊ)
    glColor3f(1, 0, 0); //Red
    for (int i = 0; i < grid; i++)
    {
        double rad_1 = 2.0 * M_PI * i / grid + theta;
        double rad_2 = 2.0 * M_PI * (i + 1) / grid + theta;
        //蠎暮擇
        glBegin(GL_LINES);
        glVertex3f_p(cos(rad_1) * radius + myrobot.x_dummy, sin(rad_1) * radius + myrobot.y_dummy, 0.0);
        glVertex3f_p(cos(rad_2) * radius + myrobot.x_dummy, sin(rad_2) * radius + myrobot.y_dummy, 0.0);
        glEnd();
    }

    glBegin(GL_LINES);
    glVertex3f_p(cos(myrobot.theta_dummy + M_PI / 2.0) * radius + myrobot.x_dummy, sin(myrobot.theta_dummy + M_PI / 2.0) * radius + myrobot.y_dummy, 0.0);
    glVertex3f_p(myrobot.x_dummy, myrobot.y_dummy, 0.0);
    glEnd();

    //for(int i = 0; i < grid; i++){
    //	double rad_1= 2.0*M_PI*i/grid*4+theta;
    //	glBegin(GL_LINES);
    //	glVertex3f_p(cos(rad_1) * radius+xx, sin(rad_1) * radius+yy, 0.0);
    //	glVertex3f_p(cos(rad_1) * radius+xx, sin(rad_1) * radius+yy, robot_height);
    //	glEnd();
    //}

    glColor3f(1, 0, 0); //Red

    //xx=robot.x;
    //yy=robot.y;
    //theta=robot.theta;
    glLineWidth(1.0);
}

void urg_3d_view(URG &urg_data, double pan, double tilt, double robot_z)
{

    static int log_cnt = 0;
    log_cnt++;
    if (log_cnt > 1)
        log_cnt = 0;
    //static double xxx[500][1081]={};
    //static double yyy[500][1081]={};
    //static double zzz[500][1081]={};

    for (int i = 1; i < urg_data.data_num - 1; i++)
    {
        double dis = urg_data.leng[i];
        if (dis < urg_data.leng_min)
            dis = urg_data.leng_max;
        if (isnan(urg_data.leng[i]))
            dis = urg_data.leng_max;

        double xx = (-1) * dis * sin(i * urg_data.reso + urg_data.start_rad);
        double yy = dis * cos(i * urg_data.reso + urg_data.start_rad);
        double zz = 0.0;

        double xx_r = xx;
        double yy_r = yy * cos(tilt) - zz * sin(tilt);
        double zz_r = yy * sin(tilt) + zz * cos(tilt);

        double xx1 = xx_r * cos(pan + myrobot.theta) - yy_r * sin(pan + myrobot.theta);
        double yy1 = xx_r * sin(pan + myrobot.theta) + yy_r * cos(pan + myrobot.theta);
        double zz1 = zz_r;

        double dis2 = urg_data.leng[i - 1];
        if (dis2 < urg_data.leng_min)
            dis2 = urg_data.leng_max;
        if (isnan(urg_data.leng[i - 1]))
            dis2 = urg_data.leng_max;

        xx = (-1) * dis2 * sin((i - 1) * urg_data.reso + urg_data.start_rad);
        yy = dis2 * cos((i - 1) * urg_data.reso + urg_data.start_rad);
        zz = 0.0;

        xx_r = xx;
        yy_r = yy * cos(tilt) - zz * sin(tilt);
        zz_r = yy * sin(tilt) + zz * cos(tilt);

        double xx2 = xx_r * cos(pan + myrobot.theta) - yy_r * sin(pan + myrobot.theta);
        double yy2 = xx_r * sin(pan + myrobot.theta) + yy_r * cos(pan + myrobot.theta);
        double zz2 = zz_r;

        if (i > 2)
        {
            glColor4f(0, 0, 1, 0.5);

            glBegin(GL_POLYGON);
            glVertex3f_p(myrobot.x, myrobot.y, robot_z);
            glVertex3f_p(myrobot.x + xx1, myrobot.y + yy1, zz1 + robot_z);
            glVertex3f_p(myrobot.x + xx2, myrobot.y + yy2, zz2 + robot_z);
            glEnd();
        }
        glColor3f(0, 1, 1); //Red
        glBegin(GL_POINTS);
        glVertex3f_p(myrobot.x + xx1, myrobot.y + yy1, zz1 + robot_z);
        glEnd();

        //xxx[log_cnt][i]=myrobot.x+xx1;
        //yyy[log_cnt][i]=myrobot.y+yy1;
        //zzz[log_cnt][i]=robot_z+zz1;
    }
}

static double get_dtime()
{
    struct timeval tv;
    gettimeofday(&tv, NULL);
    long double n_time = tv.tv_sec + tv.tv_usec * 1e-6;
    static long double o_time = n_time;
    double dt_msec = (n_time - o_time);
    //cout<<dt_msec<<endl;
    o_time = n_time;
    return dt_msec;
}

static double get_time()
{
    struct timeval tv;
    gettimeofday(&tv, NULL);
    long double n_time = tv.tv_sec + tv.tv_usec * 1e-6;
    return n_time;
}

void main_draw()
{

    double now = get_dtime(); //msec
    double now_stock = get_time();

    //URG繧ｻ繝ｳ繧ｵ縺ｮ讀懃衍
    //URG縺ｮ荳翫�陦ｨ遉ｺ

    for (int i = 0; i < urg_area.data_num; i++)
        urg_area.leng[i] = urg_leng_max;
    urg_calcurate(myrobot, urg_area, obst, move_obst, urg_data2);
    urg_noise(urg_data2);


    struct ROBOT myrobot_back;
    myrobot_back.x=myrobot.x;
    myrobot_back.y=myrobot.y;
    myrobot_back.theta=myrobot.theta+M_PI;


    urg_calcurate(myrobot_back, urg_area, obst, move_obst, urg_data3);
    urg_noise(urg_data3);

    obstacle_view();
    //Robot縺ｮ陦ｨ遉ｺ
    move_obstacle_view();

    //glMatrixMode(GL_MODELVIEW);
    //glEnable( GL_DEPTH_TEST );		// 髫�髱｢蜃ｦ逅��驕ｩ逕ｨ

    robot_view(myrobot);

    for (int i = 0; i < move_obst.n; i++)
    {
        double xx = (move_obst.obst_x[i][0] + move_obst.obst_x[i][2]) / 2.0;
        double yy = (move_obst.obst_y[i][0] + move_obst.obst_y[i][2]) / 2.0;
        double theta = -move_obst.obst_theta[i] + M_PI;
        color_no = i % 4;
        //	scene1(xx,yy,theta);
    }

    glPushMatrix();

    glColor3f(0, 1, 0); //Red
    glBegin(GL_LINES);
    glVertex3f_p(0, 0, 2);
    glVertex3f_p(0, 0, 0);
    glEnd();

    glColor3f(1, 0, 0); //Red

    glPointSize(3.0);
    double length = 15.0;

    //glPushMatrix();
    //scene0(now);
    //glPopMatrix();

    glEnable(GL_BLEND);
    //URG縺ｮ荳九�陦ｨ遉ｺ
//////////////////////////////////////////////////
//////////////////////////////////////////////////

    glColor4f(0, 0, 1, 0.5); //Red
    for (int i = 0; i < urg_data2.data_num - 1; i++)
    {
        double length = urg_data2.leng[i];

        if (length == 0)
            length = urg_data2.leng_max;
        if (isnan(urg_data2.leng[i]))
            length = urg_data2.leng_max;

        double xx1 = (-1) * length * sin(i * urg_data2.reso + urg_data2.start_rad + myrobot.theta);
        double yy1 = length * cos(i * urg_data2.reso + urg_data2.start_rad + myrobot.theta);
        double xx2 = (-1) * length * sin((i + 1) * urg_data2.reso + urg_data2.start_rad + myrobot.theta);
        double yy2 = length * cos((i + 1) * urg_data2.reso + urg_data2.start_rad + myrobot.theta);

        glBegin(GL_POLYGON);
        glVertex3f_p(myrobot.x, myrobot.y, 0.2);
        glVertex3f_p(myrobot.x + xx1, myrobot.y + yy1, 0.2);
        glVertex3f_p(myrobot.x + xx2, myrobot.y + yy2, 0.2);
        glEnd();
    }

    glColor3f(0, 1, 1); //Red

    glPointSize(3.0);
    for (int i = 0; i < urg_data2.data_num - 1; i++)
    {
        double length = urg_data2.leng[i] - 0.1;
        if (length >= 0.2)
        {
            double xx1 = (-1) * length * sin(i * urg_data2.reso + urg_data2.start_rad + myrobot.theta);
            double yy1 = length * cos(i * urg_data2.reso + urg_data2.start_rad + myrobot.theta);
            glBegin(GL_POINTS);
            glVertex3f_p(myrobot.x + xx1, myrobot.y + yy1, 0.2);
            glEnd();
        }
    }
//////////////////////////////////////////////////
//////////////////////////////////////////////////

   glColor4f(0, 0, 1, 0.5); //Red
    for (int i = 0; i < urg_data3.data_num - 1; i++)
    {
        double length = urg_data3.leng[i];

        if (length == 0)
            length = urg_data3.leng_max;
        if (isnan(urg_data3.leng[i]))
            length = urg_data3.leng_max;

        double xx1 = (-1) * length * sin(i * urg_data3.reso + urg_data3.start_rad + myrobot.theta+M_PI);
        double yy1 = length * cos(i * urg_data3.reso + urg_data3.start_rad + myrobot.theta+M_PI);
        double xx2 = (-1) * length * sin((i + 1) * urg_data3.reso + urg_data3.start_rad + myrobot.theta+M_PI);
        double yy2 = length * cos((i + 1) * urg_data3.reso + urg_data3.start_rad + myrobot.theta+M_PI);

        glBegin(GL_POLYGON);
        glVertex3f_p(myrobot.x, myrobot.y, 1.2);
        glVertex3f_p(myrobot.x + xx1, myrobot.y + yy1, 0.15);
        glVertex3f_p(myrobot.x + xx2, myrobot.y + yy2, 0.15);
        glEnd();
    }


    glColor3f(0, 1, 0); //Red

    glPointSize(3.0);
    for (int i = 0; i < urg_data3.data_num - 1; i++)
    {
        double length = urg_data3.leng[i] - 0.1;
        if (length >= 0.2)
        {
            double xx1 = (-1) * length * sin(i * urg_data3.reso + urg_data3.start_rad + myrobot.theta+M_PI);
            double yy1 = length * cos(i * urg_data3.reso + urg_data3.start_rad + myrobot.theta+M_PI);
            glBegin(GL_POINTS);
            glVertex3f_p(myrobot.x + xx1, myrobot.y + yy1, 0.15);
            glEnd();
        }
    }

//////////////////////////////////////////////////
//////////////////////////////////////////////////




    if (pantilt_tilt_init_deg == 0)
    {
        double tilt_ros_time = now_stock / pantilt_tilt_rot_sin_speed * 2 * 3.1415;
        double tilt_sin = pantilt_tilt_rot_sin_amp * sin(tilt_ros_time);
        pantable.tilt = tilt_sin;
    }

    pantable.pan += pantilt_pan_rot_speed * now;
    pantable.pan = fmod(pantable.pan, 2 * M_PI);
    pantilt_time = ros::Time::now();

    /*URG urg_data_buff;*/
    urg_calcurate_3d(myrobot, obst, move_obst, urg_data, pantable.pan, pantable.tilt, robot_z);
    urg_noise(urg_data);
    urg_3d_view(urg_data, pantable.pan, pantable.tilt, robot_z);
    lrf_scantime = ros::Time::now();

    glDisable(GL_BLEND);

    glPointSize(1.0);
    glColor4f(1, 0, 0, 1); //Red

    robot_move(myrobot, now);
    glDisable(GL_DEPTH_TEST);

    glPopMatrix();
}

Graphics::Graphics()
{
    xOrig = 0.0, yOrig = 0.0, zOrig = 0.0; //蟋狗せ荳ｭ蠢�
    gui_start();
}

Graphics::~Graphics()
{
    gui_end();
}
extern int read_buf_flg;

void Graphics::display()
{

    glClearColor(1, 1, 1, 1);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glPushMatrix();
    polarview();
    glEnable(GL_DEPTH_TEST);

    //繝｡繧､繝ｳ謠冗判縺ｮ髢｢謨ｰ
    main_draw();

    //main_draw2();

    //gl_disp_save();
    read_buf_flg = 1;
    glReadPixels(0, 0, WIDTH, HEIGHT, GL_RGB, GL_UNSIGNED_BYTE, capture_video_buf->imageData);
    cvConvertImage(capture_video_buf, capture_video, CV_CVTIMG_FLIP + CV_CVTIMG_SWAP_RB);
    read_buf_flg = 0;

    glPopMatrix();
    glutSwapBuffers();
    glutPostRedisplay();
    //cout<<now<<endl;;

    wait_sleep(10.0);
    cvWaitKey(10);
}

void Graphics::idle(void)
{

    //  glutPostRedisplay();

    for (int loop = 0; loop < WindowNum; ++loop)
    {
        if (WinFlag[loop] > 0)
        {
            //printf("idle, loop=%d, WinFlag=%d\n", loop, WinFlag[loop]);
            glutSetWindow(WinID[loop]);
            glutPostRedisplay(); //蜀肴緒逕ｻ (窶ｻdisplay()髢｢謨ｰ繧貞他縺ｳ蜃ｺ縺咎未謨ｰ )
        }
    }

    wait_sleep(10);
}

void Graphics::myInit(char *progname)
{

    float aspect = (float)WIDTH / (float)HEIGHT;

    glutInitWindowPosition(0, 0);
    glutInitWindowSize(WIDTH, HEIGHT);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);

    capture_video_buf = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 3);
    capture_video = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 3);

    //WindowMain
    WinID[WindowNum] = glutCreateWindow(WindowName[WindowNum]);
    printf("%d,%d\n", WinID[WindowNum], glutGetWindow());
    WinFlag[WindowNum] = 1;
    WindowNum = WindowNum + 1;
    glutDisplayFunc(display);
    glutReshapeFunc(reshape);
    glutIdleFunc(idle);

    glClearColor(1, 1, 1, 1);

    glutKeyboardFunc(myKbd);
    glutKeyboardUpFunc(myKbdup);
    glutSpecialFunc(mySkey);

    glutMouseFunc(myMouse);
    glutMotionFunc(myMotion);
    resetview();

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45.0, aspect, 1.0, 200.0); //逕ｻ隗堤ｭ芽ｦ夜㍽縺ｮ險ｭ螳�
    glMatrixMode(GL_MODELVIEW);

    //glEnable(GL_LIGHTING);
    //glEnable(GL_LIGHT0);
    mySetLight();
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    // Window2 //
    glutInitWindowPosition(WIDTH, 0);
    glutInitWindowSize(320, 240);
    WinID[WindowNum] = glutCreateWindow(WindowName[WindowNum]);
    printf("%d,%d\n", WinID[WindowNum], glutGetWindow());
    WinFlag[WindowNum] = 1;
    WindowNum = WindowNum + 1;
    glutDisplayFunc(display2);
    glutReshapeFunc(reshape2);
    glutIdleFunc(idle);

    //glutMouseFunc(myMouse2);
    //glutMotionFunc(myMotion2);
    //	glutSpecialFunc( mySkey2 );
    mySetLight();

    pantable.pan = pantilt_pan_init_deg * M_PI * 2.0 / 360.0;
    pantable.tilt = pantilt_tilt_init_deg * M_PI * 2.0 / 360.0;
}

void Graphics::reshape(int, int)
{
    //glutReshapeWindow( WIDTH, HEIGHT );
}
int Graphics::GUImain()
{
    //	read_obstacle(obst);
    //read_moveobstacle(move_obst);
    int argc = 3;
    char *argv[] = {"program", "-display", ":0.0"};
    glutInit(&argc, argv);

    read_obstacle(obst);
    char *win_name = "simulation";
    myInit(win_name);

    //mqoInit();
    //gl_mqoModel = mqoCreateModel("robot.mqo", 0.01);

    glutMainLoop();

    return 0;
}

void Graphics::gui_start()
{
    th1 = SDL_CreateThread((int (*)(void *))GUImain, this);
}

void Graphics::gui_end()
{
    SDL_KillThread(th1);
}

double Graphics::timer_ms()
{

    double timer_ms = cv::getTickCount();
    return timer_ms;
}

void Graphics::myMouse(int button, int state, int x, int y)
{

    double obj_x = 0, obj_y = 0, obj_z = 0;

    if (state == GLUT_DOWN)
    {
        switch (button)
        {
        case GLUT_LEFT_BUTTON:
            mButton = button;
            //click_pickup(x,y,obj_x,obj_y,obj_z);
            //goal_y=obj_y;
            //goal_x=obj_x;

            break;
        case GLUT_MIDDLE_BUTTON:

            break;
        case GLUT_RIGHT_BUTTON:

            mButton = button;
            break;
        }
        xBegin = x;
        yBegin = y;
    }
    if (state == GLUT_UP)
    {
        switch (button)
        {
        case GLUT_RIGHT_BUTTON:

            break;
        }
    }

    return;
}

void Graphics::myMotion(int x, int y)
{

    int xDisp, yDisp;

    xDisp = x - xBegin;
    yDisp = y - yBegin;

    switch (mButton)
    {
    case GLUT_LEFT_BUTTON:
        azimuth -= (float)xDisp / 2.0;
        elevation -= (float)yDisp / 2.0;

        break;
    case GLUT_RIGHT_BUTTON:
        distance_gl -= (float)xDisp / 10.0;
        if (distance_gl < 0.000001)
            distance_gl = 0.000001;

        break;
    }
    xBegin = x;
    yBegin = y;

    glutPostRedisplay();
}

void Graphics::myKbdup(unsigned char key, int x, int y)
{

    switch (key)
    {
        //case 'w':	//ROBOT縺ｮ遘ｻ蜍墓欠莉､
        //	myrobot.v=0.0;
        //	break;
        //case 's':	//ROBOT縺ｮ遘ｻ蜍墓欠莉､
        //	myrobot.v=-0.0;
        //	break;
        //case 'a':	//ROBOT縺ｮ遘ｻ蜍墓欠莉､
        //	myrobot.w=-0.0;
        //	break;
        //case 'd':	//ROBOT縺ｮ遘ｻ蜍墓欠莉､
        //	myrobot.w=+0.0;
        //	break;
    }
}
void Graphics::myKbd(unsigned char key, int x, int y)
{
    switch (key)
    {
    case 0x1B: //邨ゆｺ�
        //	exit(0);
        break;

    case 'q': //隕也せ繧偵Μ繧ｻ繝�ヨ
        //	exit(0);;
        break;

    case 'z': //隕也せ繧偵Μ繧ｻ繝�ヨ
        //	gl_mode++;
        //myrobot.v=0;
        //myrobot.w=0;

        break;
    case 'x': //隕也せ繧偵Μ繧ｻ繝�ヨ
        //gl_mode--;
        //myrobot.v=0;
        //myrobot.w=0;

        break;

    //case 'w':	//ROBOT縺ｮ遘ｻ蜍墓欠莉､
    //	myrobot.v=4.0;
    //
    //	break;
    //case 's':	//ROBOT縺ｮ遘ｻ蜍墓欠莉､
    //	myrobot.v=-4.0;
    //
    //	break;
    //case 'a':	//ROBOT縺ｮ遘ｻ蜍墓欠莉､
    //	myrobot.w=-1.0;
    //	break;
    //case 'd':	//ROBOT縺ｮ遘ｻ蜍墓欠莉､
    //	myrobot.w=+1.0;
    //	break;

    case 'c': //ROBOT縺ｮ遘ｻ蜍墓欠莉､
        if (save_flg == 0)
            save_flg = 1;
        else if (save_flg == 1)
            save_flg = 0;
        break;
    }
}

void Graphics::mySkey(int key, int x, int y)
{
    switch (key)
    {
    case GLUT_KEY_LEFT:
        xOrig -= 1;
        break;
    case GLUT_KEY_RIGHT:
        xOrig += 1;
        break;
    case GLUT_KEY_UP:
        yOrig += 1;
        break;
    case GLUT_KEY_DOWN:
        yOrig -= 1;
        break;
    case GLUT_KEY_PAGE_UP:
        zOrig += 1;
        break;
    case GLUT_KEY_PAGE_DOWN:
        zOrig -= 1;
        break;
    }
    glutPostRedisplay();
}

void Graphics::resetview(void)
{
    distance_gl = 30.0;
    twist = 0.0;
    elevation = -60.0;
    azimuth = 25.0;
}

void Graphics::polarview(void)
{
    //繝槭え繧ｹ縺ｧ遘ｻ蜍�
    glTranslatef(0.0, 0.0, -distance_gl);
    glRotatef(-twist, 0.0, 0.0, 1.0);
    glRotatef(-elevation, 1.0, 0.0, 0.0);
    glRotatef(-azimuth, 0.0, 1.0, 0.0);
    //繧ｭ繝ｼ繝懊�繝峨〒遘ｻ蜍�
    glTranslatef(xOrig, zOrig, yOrig);
}

float Graphics::click_Depth(int x, int y)
{ //繝槭え繧ｹ縺ｮX/Y蠎ｧ讓吶°繧吋epth繧堤ｮ怜�
    float z;
    GLint viewport[4]; //繝薙Η繝ｼ繝昴�繝�
    //繝�ヰ繧､繧ｹ蠎ｧ讓咏ｳｻ縺ｨ繧ｦ繧｣繝ｳ繝峨え蠎ｧ讓咏ｳｻ縺ｮ螟画鋤
    glGetIntegerv(GL_VIEWPORT, viewport); //迴ｾ蝨ｨ縺ｮ繝薙Η繝ｼ繝昴�繝医ｒ莉｣蜈･
    glReadPixels(x, viewport[3] - y, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &z);
    // 繝��繧ｹ繝舌ャ繝輔ぃ縺ｮ蛟､繧定ｿ斐☆��
    return z;
}

void Graphics::click_pickup(int x, int y, double &ax, double &ay, double &az)
{ //繝槭え繧ｹ縺ｮX/Y蠎ｧ讓吶°繧厩/Y/Z蠎ｧ讓吶ｒ邂怜�
    GLdouble modelview[16];
    GLdouble projection[16];
    GLint viewport[4];
    glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
    glGetDoublev(GL_PROJECTION_MATRIX, projection);
    glGetIntegerv(GL_VIEWPORT, viewport);
    GLdouble winX, winY, winZ;
    GLdouble objX = 0.0, objY = 0.0, objZ = -distance_gl + yOrig;

    //蜴溽せ縺ｮGLUT蠎ｧ讓咏ｳｻ繧堤ｮ怜�
    gluProject(objX, objY, objZ, modelview, projection, viewport, &winX, &winY, &winZ);
    GLdouble objX1, objY1, objZ1;
    //gluUnProject((double)x,(double)y,0.0,modelview,projection,viewport,&objX1,&objY1,&objZ1);
    //cout<<"near_window:"<<objX1<<"\t"<<objY1<<"\t"<<objZ1<<"\t"<<endl;
    //gluUnProject((double)x,(double)y,1.0,modelview,projection,viewport,&objX1,&objY1,&objZ1);
    //cout<<"far_window:"<<objX1<<"\t"<<objY1<<"\t"<<objZ1<<"\t"<<endl;

    gluUnProject((double)x, (double)y, winZ, modelview, projection, viewport, &objX1, &objY1, &objZ1);
    ax = objX1 - xOrig;
    ay = -(objY1 + zOrig);
    az = objZ1 - yOrig;

    //cout<<"mouse_click:"<<"\t X:"<<x<<"\t Y:"<<y<<"\t x:"<<ax<<"\t y:"<<ay<<"\t z:"<<az<<""<<endl;

    return;
}
void draw_string(string str, int w, int h, int x0, int y0)
{
    glColor3d(0.0, 1.0, 0.0);
    // glDisable(GL_LIGHTING);
    // 蟷ｳ陦梧兜蠖ｱ縺ｫ縺吶ｋ
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    gluOrtho2D(0, w, h, 0);
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();

    // 逕ｻ髱｢荳翫↓繝�く繧ｹ繝域緒逕ｻ
    glRasterPos2f(x0, y0);
    int size = (int)str.size();
    for (int i = 0; i < size; ++i)
    {
        char ic = str[i];
        glutBitmapCharacter(GLUT_BITMAP_9_BY_15, ic);
    }

    glPopMatrix();
    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);
}

// 蜈画ｺ舌�險ｭ螳壹ｒ陦後≧髢｢謨ｰ
void mySetLight(void)
{
    GLfloat light_diffuse[] = {0.9, 0.9, 0.9, 1.0};     // 諡｡謨｣蜿榊ｰ��
    GLfloat light_specular[] = {0.9, 0.9, 0.9, 1.0};    // 髀｡髱｢蜿榊ｰ��
    GLfloat light_ambient[] = {0.3, 0.3, 0.3, 0.1};     // 迺ｰ蠅��
    GLfloat light_position[] = {10.0, 10.0, 10.0, 1.0}; // 菴咲ｽｮ縺ｨ遞ｮ鬘�

    // 蜈画ｺ舌�險ｭ螳�
    glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);   // 諡｡謨｣蜿榊ｰ��縺ｮ險ｭ螳�
    glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular); // 髀｡髱｢蜿榊ｰ��縺ｮ險ｭ螳�
    glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);   // 迺ｰ蠅��縺ｮ險ｭ螳�
    glLightfv(GL_LIGHT0, GL_POSITION, light_position); // 菴咲ｽｮ縺ｨ遞ｮ鬘槭�險ｭ螳�

    glShadeModel(GL_SMOOTH); // 繧ｷ繧ｧ繝ｼ繝�ぅ繝ｳ繧ｰ縺ｮ遞ｮ鬘槭�險ｭ螳�
    glEnable(GL_LIGHT0);     // 蜈画ｺ舌�譛牙柑蛹�
}

void Graphics::gl_disp_save()
{
    static int skip = 0;
    skip++;
    if (save_flg == 0)
        return;
    if (skip % 2 != 0)
        return;
    char *data;
    data = new char[glutGet(GLUT_WINDOW_WIDTH) * glutGet(GLUT_WINDOW_HEIGHT) * 3];
    glReadPixels(0, 0, glutGet(GLUT_WINDOW_WIDTH), glutGet(GLUT_WINDOW_HEIGHT),
                 GL_RGB, GL_UNSIGNED_BYTE, data);

    IplImage *imgA;
    imgA = cvCreateImage(cvSize(glutGet(GLUT_WINDOW_WIDTH), glutGet(GLUT_WINDOW_HEIGHT)), IPL_DEPTH_8U, 3);

    memcpy(imgA->imageData, data, imgA->imageSize);

    cvCvtColor(imgA, imgA, CV_BGR2RGB);
    cvFlip(imgA, NULL, 0);
    //

    static int cnt = 0;
    char fname[256];
    sprintf(fname, "img/img_%.5d.png", cnt);

    cvSaveImage(fname, imgA);

    cnt++;
    delete[] data;
    cvReleaseImage(&imgA);

    //	save_flg=0;
}

void move_obstacle_view()
{

    //int t_now= timeGetTime();		//msec
    int t_now = gettimeofday_sec();
    static int t_start = t_now;

    static long double now = 0;
    now += (t_now - t_start) / 1000.0;
    //cout<<now<<endl;

    double phase = 0;
    double size = 0.25;
    double offset_y = 2.0;
    for (int i = 0; i < move_obst.n; i++)
    {
        //		move_obst.obst_x[i][0]=move_obst.amp[i]*sin(2*M_PI*now/move_obst.freq[i]-2*M_PI/360.0*move_obst.phase[i])+ move_obst.x[i]-size;
        //		move_obst.obst_y[i][0]=move_obst.amp[i]*cos(2*M_PI*now/move_obst.freq[i]-2*M_PI/360.0*move_obst.phase[i])+ move_obst.y[i]-size;
        //		move_obst.obst_x[i][1]=move_obst.amp[i]*sin(2*M_PI*now/move_obst.freq[i]-2*M_PI/360.0*move_obst.phase[i])+ move_obst.x[i]-size;
        //		move_obst.obst_y[i][1]=move_obst.amp[i]*cos(2*M_PI*now/move_obst.freq[i]-2*M_PI/360.0*move_obst.phase[i])+ move_obst.y[i]+size;
        //		move_obst.obst_x[i][2]=move_obst.amp[i]*sin(2*M_PI*now/move_obst.freq[i]-2*M_PI/360.0*move_obst.phase[i])+ move_obst.x[i]+size;
        //		move_obst.obst_y[i][2]=move_obst.amp[i]*cos(2*M_PI*now/move_obst.freq[i]-2*M_PI/360.0*move_obst.phase[i])+ move_obst.y[i]+size;
        //		move_obst.obst_x[i][3]=move_obst.amp[i]*sin(2*M_PI*now/move_obst.freq[i]-2*M_PI/360.0*move_obst.phase[i])+ move_obst.x[i]+size;
        //		move_obst.obst_y[i][3]=move_obst.amp[i]*cos(2*M_PI*now/move_obst.freq[i]-2*M_PI/360.0*move_obst.phase[i])+ move_obst.y[i]-size;
        //		move_obst.obst_x[i][4]=move_obst.amp[i]*sin(2*M_PI*now/move_obst.freq[i]-2*M_PI/360.0*move_obst.phase[i])+ move_obst.x[i]-size;
        //		move_obst.obst_y[i][4]=move_obst.amp[i]*cos(2*M_PI*now/move_obst.freq[i]-2*M_PI/360.0*move_obst.phase[i])+ move_obst.y[i]-size;
        move_obst.obst_x[i][0] = move_obst.x[i] - size;
        move_obst.obst_x[i][1] = move_obst.x[i] - size;
        move_obst.obst_x[i][2] = move_obst.x[i] + size;
        move_obst.obst_x[i][3] = move_obst.x[i] + size;
        move_obst.obst_x[i][4] = move_obst.x[i] - size;

        move_obst.obst_y[i][0] = move_obst.y[i] - size + offset_y;
        move_obst.obst_y[i][1] = move_obst.y[i] + size + offset_y;
        move_obst.obst_y[i][2] = move_obst.y[i] + size + offset_y;
        move_obst.obst_y[i][3] = move_obst.y[i] - size + offset_y;
        move_obst.obst_y[i][4] = move_obst.y[i] - size + offset_y;

        move_obst.obst_theta[i] = 2 * M_PI * now / move_obst.freq[i] - 2 * M_PI / 360.0 * move_obst.phase[i] + M_PI;
    }
    //move_obst.move(now);

    //	double size=0.5;
    glColor3f(1, 0.5, 0); //Red

    for (int i = 0; i < move_obst.n; i++)
    {
        glLineWidth(2.0);

        glBegin(GL_LINE_LOOP);
        glVertex3f_p(move_obst.obst_x[i][0], move_obst.obst_y[i][0], 0);
        glVertex3f_p(move_obst.obst_x[i][1], move_obst.obst_y[i][1], 0);
        glVertex3f_p(move_obst.obst_x[i][2], move_obst.obst_y[i][2], 0);
        glVertex3f_p(move_obst.obst_x[i][3], move_obst.obst_y[i][3], 0);
        glVertex3f_p(move_obst.obst_x[i][4], move_obst.obst_y[i][4], 0);
        glEnd();

        glBegin(GL_LINE_LOOP);
        glVertex3f_p(move_obst.obst_x[i][0], move_obst.obst_y[i][0], 2);
        glVertex3f_p(move_obst.obst_x[i][1], move_obst.obst_y[i][1], 2);
        glVertex3f_p(move_obst.obst_x[i][2], move_obst.obst_y[i][2], 2);
        glVertex3f_p(move_obst.obst_x[i][3], move_obst.obst_y[i][3], 2);
        glVertex3f_p(move_obst.obst_x[i][4], move_obst.obst_y[i][4], 2);
        glEnd();

        for (int j = 0; j < move_obst.m; j++)
        {
            glBegin(GL_LINES);
            glVertex3f_p(move_obst.obst_x[i][j], move_obst.obst_y[i][j], 0);
            glVertex3f_p(move_obst.obst_x[i][j], move_obst.obst_y[i][j], 2);
            glEnd();
        }

        glColor3f(1, 0.7, 0); //Red

        glBegin(GL_QUADS);
        glVertex3f_p(move_obst.obst_x[i][0], move_obst.obst_y[i][0], 0);
        glVertex3f_p(move_obst.obst_x[i][1], move_obst.obst_y[i][1], 0);
        glVertex3f_p(move_obst.obst_x[i][2], move_obst.obst_y[i][2], 0);
        glVertex3f_p(move_obst.obst_x[i][3], move_obst.obst_y[i][3], 0);
        glEnd();

        glBegin(GL_QUADS);
        glVertex3f_p(move_obst.obst_x[i][0], move_obst.obst_y[i][0], 2);
        glVertex3f_p(move_obst.obst_x[i][1], move_obst.obst_y[i][1], 2);
        glVertex3f_p(move_obst.obst_x[i][2], move_obst.obst_y[i][2], 2);
        glVertex3f_p(move_obst.obst_x[i][3], move_obst.obst_y[i][3], 2);
        glEnd();

        glBegin(GL_QUADS);
        for (int j = 0; j < move_obst.m; j++)
        {
            glVertex3f_p(move_obst.obst_x[i][j], move_obst.obst_y[i][j], 0);
            glVertex3f_p(move_obst.obst_x[i][j + 1], move_obst.obst_y[i][j + 1], 0);
            glVertex3f_p(move_obst.obst_x[i][j + 1], move_obst.obst_y[i][j + 1], 2);
            glVertex3f_p(move_obst.obst_x[i][j], move_obst.obst_y[i][j], 2);
        }
        glEnd();
    }

    glLineWidth(1.0);
}

void obstacle_view()
{
    glEnable(GL_LIGHTING); // 蜈画ｺ唇N

    glLineWidth(3.0);
    glColor3f(1, 0.2, 0); //Red

    double p1[3];
    double p2[3];
    double p3[3];
    double n[3];

    //float material_diffuse[4]={0.2, 0.2, 0.2, 1.0};
    //glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, material_diffuse);

    for (int i = 0; i < obst.n; i++)
    {
        p1[0] = obst.x1[i], p1[1] = obst.x1[i], p1[2] = 0;
        p2[0] = obst.x1[i], p2[1] = obst.y1[i], p2[2] = obst.z[i];
        p3[0] = obst.x2[i], p3[1] = obst.y2[i], p3[2] = obst.z[i];

        normal_calc(p2, p1, p3, n);

        glBegin(GL_QUADS);
        glNormal3f((GLfloat)n[0], (GLfloat)n[2], (GLfloat)n[1]);
        glVertex3f_p(obst.x1[i], obst.y1[i], 0);
        glVertex3f_p(obst.x2[i], obst.y2[i], 0);
        glVertex3f_p(obst.x2[i], obst.y2[i], obst.z[i]);
        glVertex3f_p(obst.x1[i], obst.y1[i], obst.z[i]);

        glEnd();
    }
    glLineWidth(1.0);

    glDisable(GL_LIGHTING);

    //Grid縺ｮ陦ｨ遉ｺ
    glColor3f(0.8, 0.8, 0.8);
    for (double x = -20.0; x <= 20.0; x += 1.0)
    {
        for (double y = -20.0; y <= 20.0; y += 1.0)
        {
            glBegin(GL_LINE_LOOP);
            glVertex3f_p(x, y, 0.0);
            glVertex3f_p(x + 1, y, 0.0);
            glVertex3f_p(x + 1, y + 1, 0.0);
            glVertex3f_p(x, y + 1, 0.0);
            glEnd();
        }
    }
}