/*
 *地图，存储全场定位点位
 * 
 */
#include "map.h"
#include "define.h"
/* Private variables -------------------------------------------------------- */

/*    
            1  ________ 7
						/           \
					2               6
					|               |
					3	---- 4  ----  5 

计算值: 1-2: 根2x   2-3：2.5-x   3-4:1.25+x   4-5:1.25+x  5-6:2.5-x  6-7:根2x   7-1:2.5m    
测量值：x≈0.8（量的） 1-2:   2-3:   3-4:   4-5:   5-6:    6-7:    7-1:   

*/

Path_t path_state; //地图数据
/*点位数据*/
point_t path_1;
sick_t sick_1;
point_t path_2;
sick_t sick_2;
point_t path_3;
sick_t sick_3;
point_t path_4;
sick_t sick_4;
point_t path_5;
sick_t sick_5;
point_t path_6;
sick_t sick_6;
point_t path_7;
sick_t sick_7;
point_t path_8;
sick_t sick_8;
point_t path_9;
sick_t sick_9;
point_t path_10;
sick_t sick_10;
point_t path_11;
sick_t sick_11;
point_t path_12;
sick_t sick_12;
point_t path_13;
sick_t sick_13;
point_t path_14;
sick_t sick_14;
point_t path_15;
sick_t sick_15;
//点位坐标x轴朝前，y轴朝右
void block_select(Action_POS_t*pos,CMD_t*cmd){ 

//	path_1.x = 0;
//  path_1.y = 0;	
//	path_1.angle=0;
//	sick_1.sick_flag = 0;

//	path_2.x = pos->pos_x;
//	path_2.y = pos->pos_y;	
//	path_2.angle= 0;
//	sick_2.sick_flag = 0;
	
#ifdef dribble_1	//点上运球		
	//1
	path_1.x = 0;
  path_1.y = 0;	
	path_1.angle=0;
	sick_1.sick_flag = 0;	
	//2
	path_2.x = -800;
  path_2.y = -800;	
	path_2.angle=0;
	sick_2.sick_flag = 0;
	//3
	path_3.x = -2500;
	path_3.y = -800;	
	path_3.angle=0;
	sick_3.sick_flag = 0;
	//4
	path_4.x = -2500;
	path_4.y = 1250;	
	path_4.angle= 0;
	sick_4.sick_flag = 0;
	//5
	path_5.x = -2500;
	path_5.y = 3300;	
	path_5.angle= 0;
	sick_5.sick_flag = 0;
	//6
	path_6.x = -800;
	path_6.y = 3300;	
	path_6.angle= 0;
	sick_6.sick_flag = 0;
	//7
	path_7.x = 0;
	path_7.y = 2500;	
	path_7.angle= 0;
	sick_7.sick_flag = 0;
	//8
	path_8.x = 0;
	path_8.y = 0;	
	path_8.angle= 0;
	sick_8.sick_flag = 0;	
	
#elif defined(dribble_2) //线上运球
	//1
	path_1.x = 0;
  path_1.y = 0;	
	path_1.angle=0;
	sick_1.sick_flag = 0;	
	//2
	path_2.x = -400;
  path_2.y = -400;	
	path_2.angle=0;
	sick_2.sick_flag = 0;
	//3
	path_3.x = -800;
	path_3.y = -800;	
	path_3.angle=0;
	sick_3.sick_flag = 0;
	//4
	path_4.x = -1650;
	path_4.y = -800;	
	path_4.angle= 0;
	sick_4.sick_flag = 0;
	//5
	path_5.x = -2500;
	path_5.y = -800;	
	path_5.angle= 0;
	sick_5.sick_flag = 0;
	//6
	path_6.x = -2500;
	path_6.y = 200;	
	path_6.angle= 0;
	sick_6.sick_flag = 0;
	//7
	path_7.x = -2500;
	path_7.y = 1250;	
	path_7.angle= 0;
	sick_7.sick_flag = 0;
	//8
	path_8.x = -2500;
	path_8.y = 2300;	
	path_8.angle= 0;
	sick_8.sick_flag = 0;
	//9
	path_9.x = -2500;
	path_9.y = 3300;	
	path_9.angle= 0;
	sick_9.sick_flag = 0;
	//10
	path_10.x = -1650;
	path_10.y = 3300;	
	path_10.angle= 0;
	sick_10.sick_flag = 0;
	//11
	path_11.x = -800;
	path_11.y = 3300;	
	path_11.angle= 0;
	sick_11.sick_flag = 0;
	//12
	path_12.x = -400;
	path_12.y = 2900;	
	path_12.angle= 0;
	sick_12.sick_flag = 0;
	//13
	path_13.x = 0;
	path_13.y = 2500;	
	path_13.angle= 0;
	sick_13.sick_flag = 0;	
	//14
	path_14.x = 0;
	path_14.y = 1250;	
	path_14.angle= 0;
	sick_14.sick_flag = 0;	
	//15
	path_15.x = 0;
	path_15.y = 0;	
	path_15.angle= 0;
	sick_15.sick_flag = 0;	

#endif	
}
		 

/**
 *     路径选择
 *     
 *      path_state.target  
 */
void  path_select(CMD_t*cmd){
	
	 static int8_t last_pos = -1;

	 if (cmd->C_cmd.pos != last_pos) {
         	
    switch (cmd->C_cmd.pos){
    	case POS_1:
		   path_state.target = &path_1;
			 path_state.sick = &sick_1;
		  break;
	   case POS_2:
		   path_state.target = &path_2;
		  break;
	   case POS_3:
		   path_state.target = &path_3;
		  break;	
	   case POS_4:
		   path_state.target = &path_4;
		    path_state.sick = &sick_4;
		  break;	
	   case POS_5:
		   path_state.target = &path_5;
		    path_state.sick = &sick_5;
		  break;	
	   case POS_6:
		   path_state.target = &path_6;
		    path_state.sick = &sick_6;
		  break;	
	   case POS_7:
		   path_state.target = &path_7;
		    path_state.sick = &sick_7;
		  break;
	   case POS_8:
		   path_state.target = &path_8;
		    path_state.sick = &sick_8;
		  break;			 
	   }		
	  path_state.points_flag = 0; //标志位清零
	 }
		last_pos = cmd->C_cmd.pos; // 更新上一次的位置
	 
	 if(cmd->C_cmd.mode != NAVI){
	  path_state.points_flag = 0; 
	 }
}

Path_t *get_map_pointer(void){
	return &path_state;	
}