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


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;
	
//点位坐标x轴朝前，y轴朝右
void block_select(Action_POS_t*pos,CMD_t*cmd){ 

	//1
	path_1.x = 0;
  path_1.y = 0;	
	path_1.angle=0;
	sick_1.sick_qian = 0;
	sick_1.sick_you = 0;
	sick_1.sick_hou = 0;
	sick_1.sick_zuo = 0;
	sick_1.sick_flag = 0;
	//2
	path_2.x = 2000;
	path_2.y = -2000;	
	path_2.angle=0;
	sick_2.sick_qian = 1873;
	sick_2.sick_you = 2180;
	sick_2.sick_hou = 0;
	sick_2.sick_zuo = 3350;
	sick_2.sick_flag = 1;
	//3
	path_3.x = pos->pos_x;
	path_3.y = pos->pos_y;	
	//path_3.angle= 10;
	sick_3.sick_qian = 0;
	sick_3.sick_you = 0;
	sick_3.sick_hou = 0;
	sick_3.sick_zuo = 0;
	sick_3.sick_flag = 0;

// 状态控制变量
    static float fixed_yaw = 0;
    static int is_calibrating = 0;
    static float prev_camera_yaw = 0;
		
  // 非NAVI模式直接重置状态
    if (cmd->C_cmd.mode != NAVI) {
        is_calibrating = 0;
    }
    
    if (cmd->camera_yaw != 0) {        
        // 只有未校准时才更新固定值
        if (is_calibrating == 0) {
        
                fixed_yaw = pos->pos_yaw + cmd->camera_yaw;
                is_calibrating = 1; 
        }

        // 校准期间始终使用fixed_yaw
        path_3.angle = fixed_yaw;
        prev_camera_yaw = cmd->camera_yaw;
    }
}
		 

/**
 *     路径选择
 *     
 *      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;//选择路径
		    path_state.sick = &sick_2;
		  break;
	   case POS_3:
		   path_state.target = &path_3;//选择路径
		    path_state.sick = &sick_3;
		  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;	
}