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

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

#pragma once

#include <iostream>
#include <vector>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <stdio.h>
#include <time.h>
#include <sys/time.h>

using namespace std;
//const double PI=3.14159265359;

#define URG_DATA_MAX 2000
#define OBSTACLE_MAX 1024
#define MOVE_OBST_MAX_MAIN 1000
#define PATH_POINTS_MAX 1024
extern unsigned int urg_data_num;
extern double urg_range_deg;
extern double urg_leng_max;
extern double urg_leng_min;
extern double noise_odometory_v_gain;
extern double noise_odometory_w_gain;
extern double noise_urg;

extern double obstacle_z;

//一様分布乱数
double Uniform( void );
//正規分布乱数
double rand_normal( double mu, double sigma );


struct pantilt{
	double pan;
	double tilt;

	pantilt(){
		pan = 0.0;
		tilt = 0.0;
	}
};
/*
struct PointCloud{
	double point_x[320*240];
	double point_y[320*240];
	double point_z[320*240];
	int data_num;
	PointCloud(){
	data_num=320*240;
	}
};*/


struct TWIST{		//Twist定義
	double v;		//目標速度(m/s)
	double w;		//目標角速度(rad/s)
	TWIST(){
		v = 0.0;
		w = 0.0;
	}
};

struct ROBOT{		//ロボットの定義
	long double x;		//X座標(m)
	long double y;		//Y座標(m)
	long double theta;	//角度(rad)

	double v;		//目標速度(m/s)
	double w;		//目標角速度(rad/s)

	double real_v;	//実態速度(m/s)
	double real_w;	//実態角速度(rad/s)

	double ac_trans_limit;	//加速度(m/s2)
	double ac_rot_limit;		//角速度(rad/s2)

	long double x_dummy;
	long double y_dummy;
	long double theta_dummy;

	ROBOT(){
		x = 0.0;
		y = 0.0;
		v = 0.0;
		w = 0.0;
		theta = 0.0;
		real_v=0.0;
		real_w=0.0;

		ac_trans_limit=5.0;
		ac_rot_limit=5.0;
	}
};

struct URG{			//測域センサの定義
	double leng[URG_DATA_MAX];		
	double x[URG_DATA_MAX];	
	double y[URG_DATA_MAX];	
	int data_num;
	double start_rad;
	double reso;

	double leng_max;
	double leng_min;

	URG(){
		data_num=(int)URG_DATA_MAX*urg_range_deg/360.0;
		start_rad=-data_num/2.0/URG_DATA_MAX*2*M_PI;
		reso=2*M_PI/URG_DATA_MAX;
		leng_max=urg_leng_max;
		leng_min=urg_leng_min;

		for(int i=0;i<URG_DATA_MAX;i++){
			x[i] = 0.0;
			y[i] = 0.0;
			leng[i] = 0.0;
		}
	}
};

struct OBSTACLE{		//障害物の定義
	double x1[OBSTACLE_MAX];
	double x2[OBSTACLE_MAX];
	double y1[OBSTACLE_MAX];
	double y2[OBSTACLE_MAX];
	double z[OBSTACLE_MAX];
	unsigned int n;				//データ数
	unsigned int n_max;			//現在の目標(x,y)の番号

	OBSTACLE(){
		n_max=OBSTACLE_MAX;

		for(int i=0;i<n_max;i++){
			x1[i] = 0.0;
			x2[i] = 0.0;
			y1[i] = 0.0;
			y2[i] = 0.0;
			z[i] =5;
		}

	}
};

struct MOVE_OBSTACLE{		//障害物の定義
	double obst_x[100][100];
	double obst_y[100][100];
	double obst_theta[100];
	double phase[100];
	double freq[100];
	double amp[100];

	double x[100];
	double y[100];
	double now;
	double n;//数
	double m;//オブジェクトの辺(4辺で固定)

	double obst_size;

	MOVE_OBSTACLE(){
		obst_size=0.5;
		n=0;
		m=4;
		for(int i=0;i<n;i++){
			x[i]=0.0;
			y[i]=0.0;
			phase[i]=0.0;
			amp[i]=0;
			freq[i]=0;
		}
		for(int i=0;i<n;i++){
			for(int j=0;j<n;j++){
				obst_x[i][j]=0.0;
				obst_y[i][j]=0.0;
			}
		}
		now=0;

		for(int i=0;i<50;i++){
			x[i]=rand()%2,y[i]=rand()%2;
			amp[i]=rand()%10+5,
			freq[i]=100.0;
			phase[i]=rand()%360;
		}
	}
};






//一様分布乱数
double Uniform( void );
//正規分布乱数
double rand_normal( double mu, double sigma );

//線分の交差判定関数
bool cross_xy(double a1_x,double a1_y,double a2_x,double a2_y,double b1_x,double b1_y,double b2_x,double b2_y,double &cro_x,double &cro_y);

//交点の座標を求める
bool cross_check(double a1_x,double a1_y,double a2_x,double a2_y,double b1_x,double b1_y,double b2_x,double b2_y);

//URGで障害物を検知する
void urg_calcurate( ROBOT &robot, URG urg_area, OBSTACLE obst, MOVE_OBSTACLE move_obst, URG &urg_data);

//障害物を読み込む
void read_obstacle(OBSTACLE &obst,string fname="obstacle.csv");
void write_obstacle(OBSTACLE &obst,string fname="obstacle.csv");
//時間を計測する
static double get_dtime(void);

//strをdoubleに変換する
double str_double(string str);

//xy座標の成す角度thetaを求める。
void arc_tan2(double &target_x,double &target_y,double &theta);
//外積
void normal_calc(double *p1, double *p2, double *p3, double *v_nl);
void urg_calcurate_3d( ROBOT &robot, OBSTACLE obst, MOVE_OBSTACLE move_obst, URG &urg_data,double pan,double tile,double robot_z);
void urg_noise( URG &urg_data);

void robot_move(ROBOT &myrobot,double dt);