#include "mainex.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "lis3dh_driver.h"
#include "dw_driver.h"
#include "deca_device_api.h"
#include "global_param.h"
#include "TrackingDiff.h"
#include "dw_app.h"
#include "APL.h"
#include "comm.h"
#include "Reboot.h"
#include "SPIFlash.h"
#include "APPConfig.h"
#define NSH1 0x0001
#define GP   0x0002
//#define DEBUG_MODE
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
//#define DEBUG_MODE

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/

/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
uint32_t dev_id;
uint16_t heartbeat_timer,poll_timer,sync_timer;
uint8_t aRxBuffer[1],group_id;
uint8_t bat_percent=0,g_start_send_flag;
 u8 motor_state;
uint16_t tyncpoll_time,lpsettime;
uint16_t slottime,max_slotpos;
uint16_t lastpoll_count,interval_count,slot_startcount,tag_frequency,lastpoll_time,current_time;
extern uint8_t module_power;
extern float nomove_count;
float motor_keeptime;
uint8_t imu_enable,motor_enable;
//u16 GetLPTime(void)
//{
//    u16 count=0; //TODO HAL_LPTIM_ReadCounter(&hlptim1);
// return count*LPTIMER_LSB/1000;
//}

#if 0
void UsartInit(void)
{
    waitusart_timer = tag_frequency*USART_KEEPWAKE_TIME;
     MX_DMA_Init();
    MX_USART1_UART_Init();
 if(HAL_UART_Receive_DMA(&huart1,m_EUART_DMA_RXBuf,EUART_RX_BUF_SIZE))
    {
     Error_Handler();
    }
}
#endif

uint16_t hardware_version,hardware_pici,hardware_type;
u16 current_time;
uint8_t state5v;



void IdleTask(void)
{
  // APLPollTask();
        if(g_com_map[CNT_RESTART]==1)
        {
            g_com_map[CNT_RESTART]=0;
            printf("%s URTRestart",__debug_info__);
            URTRestart(); 
        }
            if(g_com_map[MAP_SIGN_INDEX]!=0x55AA||g_com_map[COM_INTERVAL]==0)
        {
            printf("%s URTRestart",__debug_info__);
            URTRestart();
              //  SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //Èí¸´Î»»Øµ½bootloader
        }

    }



extern int32_t offsettimeus;
#define FIXSLOT
#define FIXSLOTPOS  4
u16 slotpos_intoatl;
uint16_t bigslot_num;
uint8_t fix_slotpos =FIXSLOTPOS;
int32_t lpcount,lptime;

    

u8 active_flag=0;
u16 current_slotnum;
extern char swname[];
extern uint8_t udp_uwbenable,uwb_state;
extern uint16_t ip0,ip1,ip2,ip3,port;
void Program_Init(void)
{
    float temp;
    uint16_t temp2;
    uint16_t i;
  
    parameter_init();
//    hardware_version = STMFLASH_ReadHalfWord(FLASH_HARDWARE_VERSION_MAP);
//    hardware_pici = STMFLASH_ReadHalfWord(FLASH_HARDWARE_VERSION_MAP + 2);
//    hardware_type = STMFLASH_ReadHalfWord(FLASH_HARDWARE_VERSION_MAP + 4);
    //deca_sleep(1000);


	 g_com_map[VERSION] = (1<<8)|9;

        udp_uwbenable = 1;

  //  g_com_map[GROUP_ID]=2;
//  g_com_map[DEV_ID]=0x11;
#ifdef DEBUG_MODE
    g_com_map[GROUP_ID]=21;
//  g_com_map[DEV_ROLE]=1;
//      g_com_map[DEV_ID]=0x10;
  g_com_map[COM_INTERVAL]=1000;
//  g_com_map[MAX_REPORT_ANC_NUM]=1;
//  g_com_map[NOMOVESLEEP_TIME]=10;
//    g_com_map[IMU_ENABLE]=0;
//    g_com_map[MOTOR_ENABLE]=1;
#endif
//g_com_map[NOMOVESLEEP_TIME]=10;
//  g_com_map[IMU_ENABLE]=0;
 //  g_com_map[COM_INTERVAL]=100;
    active_flag = g_com_map[ACTIVE_INDEX];
    active_flag = 1;
    if (active_flag == 0)
    {
        g_com_map[COM_INTERVAL] = 1000;
        g_com_map[IMU_ENABLE] = 1;
        g_com_map[NOMOVESLEEP_TIME] = 10;
        g_com_map[MOTOR_ENABLE] = 0;
    }
    if (g_com_map[COM_INTERVAL] <50)
    {
        g_com_map[COM_INTERVAL] = 100;
    }
        if (g_com_map[COM_INTERVAL] >1000)
    {
        g_com_map[COM_INTERVAL] = 1000;
    }
    if (g_com_map[IMU_THRES] > 10)
    {
        g_com_map[IMU_THRES] = 2;
    }
    if (g_com_map[POWERx] > MAX_RFPOWER)
    {
        g_com_map[POWERx] = MAX_RFPOWER;
    }
    if (g_com_map[POWERx] < 0)
    {
        g_com_map[POWERx] = 0;
    }
    if(g_com_map[STATIONARY_TIME] == 0xffff)
    {
        g_com_map[STATIONARY_TIME] = 10;
    }
    if(g_com_map[NEARBASE_ID2]>10000)
    {
        g_com_map[NEARBASE_ID2] = 0;
    }
    module_power = g_com_map[POWERx];
    imu_enable = g_com_map[IMU_ENABLE];
    motor_enable = g_com_map[MOTOR_ENABLE];
    //g_com_map[COM_INTERVAL] = 100; 
    group_id = g_com_map[GROUP_ID];
    dev_id = g_com_map[DEV_ID];
    //slottime=ceil((double)g_com_map[MAX_REPORT_ANC_NUM]*SLOT_SCALE)+3;
    slottime = 5;
    max_slotpos = g_com_map[COM_INTERVAL] / slottime;
    tyncpoll_time = (g_com_map[DEV_ID] % max_slotpos) * slottime;
//    slot_startcount = tyncpoll_time * 1000 / LPTIMER_LSB;
    lastpoll_time = tyncpoll_time;
//    tag_frequency = REGISTER_FREQUENCY;
    tag_frequency = 1000/g_com_map[COM_INTERVAL];
    bigslot_num = TOTAL_SLOTNUM / tag_frequency;
    current_slotnum = 0;
  //  current_time = GetLPTime();
    usart_send[0] = 0x55;
    usart_send[1] = 0xAA;
		ip0 = (g_com_map[TCP_IP_0]>>12&0xf)*1000+(g_com_map[TCP_IP_0]>>8&0xf)*100+(g_com_map[TCP_IP_0]>>4&0xf)*10+(g_com_map[TCP_IP_0]&0xf);
    ip1 = (g_com_map[TCP_IP_1]>>12&0xf)*1000+(g_com_map[TCP_IP_1]>>8&0xf)*100+(g_com_map[TCP_IP_1]>>4&0xf)*10+(g_com_map[TCP_IP_1]&0xf);
    ip2 = (g_com_map[TCP_IP_2]>>12&0xf)*1000+(g_com_map[TCP_IP_2]>>8&0xf)*100+(g_com_map[TCP_IP_2]>>4&0xf)*10+(g_com_map[TCP_IP_2]&0xf);
    ip3 = (g_com_map[TCP_IP_3]>>12&0xf)*1000+(g_com_map[TCP_IP_3]>>8&0xf)*100+(g_com_map[TCP_IP_3]>>4&0xf)*10+(g_com_map[TCP_IP_3]&0xf);
    port = g_com_map[TCP_PORT];
		
    printf("É豸ID: %x .\r\n",dev_id);
    printf("¹Ì¼þ°æ±¾: URT-MK°²°î³µÔØ V%d.%d. \r\n",g_com_map[VERSION]>>8,g_com_map[VERSION]&0xff);
    printf("·þÎñÆ÷µØÖ·: %d.%d.%d.%d:%d.\r\n",ip0,ip1,ip2,ip3,port);
       if(g_com_map[RTCMMODE_INDEX] == RTCMMODE_TCP)
    {        printf("TCP_RTCMģʽ,·þÎñÆ÷µØÖ·: %d.%d.%d.%d:%d.\r\n",g_com_map[TCP_IP_0],g_com_map[TCP_IP_1],g_com_map[TCP_IP_2],g_com_map[TCP_IP_3],g_com_map[TCP_PORT]);
    }else if(g_com_map[RTCMMODE_INDEX] == RTCMMODE_NTRIP)
    {
//        sprintf((char *)&g_com_map[NTRIP_HOST_INDEX],"140.143.212.42");
//        g_com_map[NTRIP_PORT_INDEX] = 8005;
//        sprintf((char *)&g_com_map[NTRIP_USERNANME_INDEX],"test006");
//        sprintf((char *)&g_com_map[NTRIP_PASSWORD_INDEX],"hxzk20151102");
//        sprintf((char *)&g_com_map[NTRIP_SOURCENAME_INDEX],"RTCM32_GNSS2");
        printf("NtripHost:%s.\r\n",(char *)&g_com_map[NTRIP_HOST_INDEX]);
        printf("NtripPort:%d.\r\n",g_com_map[NTRIP_PORT_INDEX]);
        printf("NtripUsername:%s.\r\n",(char *)&g_com_map[NTRIP_USERNANME_INDEX]);
        printf("NtripPassword:%s.\r\n",(char *)&g_com_map[NTRIP_PASSWORD_INDEX]);
        printf("NtripSourcename:%s.\r\n",(char *)&g_com_map[NTRIP_SOURCENAME_INDEX]);
    }else if(g_com_map[RTCMMODE_INDEX] == RTCMMODE_NONE)
    { 
        printf("µ¥µã¶¨Î»Ä£Ê½Ä£Ê½. \r\n");
    }
}

#if 0
static void SystemPower_Config(void)
{
  /* Enable Ultra low power mode */
  HAL_PWREx_EnableUltraLowPower();

  /* Enable the fast wake up from Ultra low power mode */
  HAL_PWREx_EnableFastWakeUp();
}
#endif

void mcu_sleep(void);
uint32_t temp;
extern uint8_t sleep_flag,m_bEUARTTxEn;
uint16_t waitusart_timer;

void HardWareTypeDiffConfig(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
//hardware_type=2;
    switch(hardware_type)
        {case 1:
         HAL_GPIO_WritePin(GPIOB, MOTOR_Pin, GPIO_PIN_SET);
  GPIO_InitStruct.Pin = MOTOR_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
            break;
        case 2:
            //MX_TIM2_Init();
        //BarInit();
            break;
        }
}
#if 0
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{
        //SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //Èí¸´Î»»Øµ½bootloader
}
#endif
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int uwb_app_init(void)
{
#if 0
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */
//ÿ´ÎÉú³ÉCUBEºóÐèҪעÒ⣬´®¿Ú³õʼ»¯ÒªÔÚDMA³õʼ»¯ºóÃ棬ÊÖ¶¯µ÷Õû˳Ðò**************
  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_LPTIM1_Init();
  MX_USART1_UART_Init();
  MX_SPI1_Init();
  MX_ADC_Init();
  MX_DMA_Init();
  //MX_TIM2_Init();
  /* USER CODE BEGIN 2 */
#endif
//    LED_LG_ON;
    Program_Init();
    LIS3DH_Data_Init();
    Dw1000_Init();
    Dw1000_App_Init();
  //  HardWareTypeDiffConfig();
    dwt_configuresleep(DWT_PRESRV_SLEEP | DWT_CONFIG, DWT_WAKE_CS | DWT_WAKE_WK| DWT_SLP_EN);
   // dwt_entersleep();
    waitusart_timer=tag_frequency*USART_KEEPWAKE_TIME;;
  //  DW_DISABLE;
     //if(HAL_UART_Receive_DMA(&huart1,m_EUART_DMA_RXBuf,EUART_RX_BUF_SIZE))
    //{
    // Error_Handler();
    //}
//  HAL_Delay(2000);
//  mcu_sleep();
    //LED0_BLINK;
//  SystemPower_Config();
//HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI);
  /* USER CODE END 2 */

}
void CloseUWB(void)
{
    dwt_setinterrupt(  DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 0);
    dwt_forcetrxoff();
    dwt_entersleep();
}
void OpenUWB(void)
{
    UWBSPIWakeup();
    dwt_setinterrupt(  DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 1);
    dwt_setrxtimeout(0);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
    dwt_rxenable(0);
}
u8 rtk_state = 1;
void CloseRTK4G(void)
{
      rtk_state = 0;
        
      rtkled = LEDOFF;
      Power_4GModulePowerOff();
      HAL_GPIO_WritePin(GPIOA, GPIO_PIN_12, GPIO_PIN_RESET);
    CloseUWB();
}
void OpenRTK4G(void)
{
      rtk_state = 1;     
      Power_4GModulePowerOn();
      HAL_GPIO_WritePin(GPIOA, GPIO_PIN_12, GPIO_PIN_SET);
    OpenUWB();
}
void UWBOneSecondTask(void)
{TagListUpdate();}

u16 tagseq;

enumwltagstate wltag_state=RANGE;
extern u8 userkey_state,bat_percent2,tagbattary_list[TAG_NUM_IN_SYS];
uint8_t anchordata_num = 0;
uint16_t anchordata_id[TAG_NUM_IN_SYS];
int16_t anchordata_dist[TAG_NUM_IN_SYS];
uint8_t anchordata_bat[TAG_NUM_IN_SYS];
void PushAnchorDataArray(uint16_t ancid,int16_t dist,uint8_t battary)//°ÑÖмä²ÎÊýuintתΪintÐÞ¸ÄÁËΪÁËÊÊÅäMK8000
{
    uint8_t i;
    wltag_state=RANGE;
    for(i=0;i<anchordata_num;i++)
    {
        if(anchordata_id[i] == ancid)
        {
            
            break;
        }
    }
    if(i==anchordata_num)
    {
        if(anchordata_num<TAG_NUM_IN_SYS-1)
        anchordata_num++;
    }
    anchordata_id[i] = ancid;
    anchordata_dist[i] = dist;
    anchordata_bat[i] = battary;  
}
uint8_t singal_threshold = 100;
void UpdateRecSinalLever(u16 ancnum)
{
	if(ancnum<30)
	{
		if(singal_threshold<120)
		{
			singal_threshold++;
		}
	}else{
		if(singal_threshold>80)
		{
			singal_threshold--;
		}
	}
}
extern u8 bat_percent2,userkey_state,gotosleep_flag,stationary_flag,motor_flag,uwb_state;
void UWBSendUDPTask(void)
{
	u16 checksum,tempdistarray[20];
//    UDP_TEST();
    for(u16 i=0;i<anchordata_num-1;i++)
    {
        for(u16 j=0;j<anchordata_num-1-i;j++)
        {
            if(anchordata_dist[j]>anchordata_dist[j+1])
            {
                u16 id,dist;
                u8 bat;
                id = anchordata_id[j];
                dist = anchordata_dist[j];
                bat = anchordata_bat[j];
                anchordata_id[j] = anchordata_id[j+1];
                anchordata_dist[j] = anchordata_dist[j+1];
                anchordata_bat[j] = anchordata_bat[j+1];
                anchordata_id[j+1] = id;
                anchordata_dist[j+1] = dist;
                anchordata_bat[j+1] = bat;
            }
        }
    
    }
    if(anchordata_num>0)
    {
        if(uwbled==LEDOFF)
        {
            uwbled = BLUE;
        }else{
            uwbled = LEDOFF;
        }
    }
    Set4LEDColor(uwbled,rtkled,led4g,powerled);
    
    if(anchordata_num>20)       //×î¶àÉÏ´«20¸ö»ùÕ¾Êý¾Ý£¬¾àÀë×î½üµÄ20¸ö
        anchordata_num = 20;
					usart_send[2] = 0x12;//Õý³£Ä£Ê½ 
					usart_send[3] = 15+5*(anchordata_num);//Êý¾Ý¶Î³¤¶È
					memcpy(&usart_send[4],&dev_id,2);
					usart_send[6] = tagseq;
					usart_send[7] = (tagseq++)>>8;
					usart_send[8] = bat_percent2;
					usart_send[9] = userkey_state|stationary_flag<<1|gotosleep_flag<<2|motor_flag<<5|uwb_state<<6;
				//	memcpy(&usart_send[10],&rec_tagheight,2);
					usart_send[12] = 0;
					usart_send[13] = 0;
					usart_send[14] = 0;
					usart_send[15] = 0;
					usart_send[16] = anchordata_num;
				 
					memcpy(&usart_send[17],&anchordata_id,2*anchordata_num);
					memcpy(&usart_send[17+anchordata_num*2],&anchordata_dist,2*anchordata_num);
                    memcpy(&usart_send[17+anchordata_num*4],&anchordata_bat,anchordata_num);
					checksum = Checksum_u16(&usart_send[2],15+5*anchordata_num);
					memcpy(&usart_send[17+5*anchordata_num],&checksum,2);
					HexToAsciiSendUDP(usart_send,19+5*anchordata_num);
                    anchordata_num = 0;                    
}
 uint8_t tt3;
u8 bat_percent2;

u32 wltag_statetimer;
extern uint16_t uwb_losttimer;
u32 uwbtasktimer=0,uwbtagsendtimer=0;
void UWBIdleTask(void)
{
	if(HIDO_TimerGetTick()-uwbtasktimer >=1000)
	{
		uwbtasktimer = HIDO_TimerGetTick() ;
		UWBOneSecondTask();
	}
    //printf("¿ÕÏÐÖжÏ");
   // printf("״̬ÏÔʾ%d,%d,%d,%d",HIDO_TimerGetTick(),uwbtagsendtimer,g_com_map[COM_INTERVAL],wltag_state);
	//if(HIDO_TimerGetTick()-uwbtagsendtimer>=g_com_map[COM_INTERVAL]&&wltag_state == RANGE )
    if(HIDO_TimerGetTick()-uwbtagsendtimer>=g_com_map[COM_INTERVAL] )
	{
        static u8 uwbledcount = 0;
//        if(uwbledcount++%2)
//        {
//            uwbled = BLUE;
//        }else{
//           uwbled = LEDOFF; 
//        }
//        Set4LEDColor(uwbled,rtkled,led4g,powerled);
		uwbtagsendtimer = HIDO_TimerGetTick();
		UWBSendUDPTask();
	}
}

#define UWB_DBGMSG

int uwb_app_poll(void)
{
  //   printf("²â¾à״̬:%d",wltag_state);
	switch(wltag_state)
    {
        case RANGE:
            
            if(uwb_losttimer>10)
            {
                wltag_statetimer = HIDO_TimerGetTick();
                wltag_state = SEARCH;
            } 
           // Anchor_App();
        break;
        case SEARCH:
        //    printf("SEARCH");
          //  Anchor_App();
            if(HIDO_TimerGetTick()-wltag_statetimer>3000)
            {
                wltag_statetimer = HIDO_TimerGetTick();
                wltag_state = CLOSE;
                CloseUWB();
            }
            break;
        case CLOSE:
        //    printf("CLOSE");
             if(HIDO_TimerGetTick()-wltag_statetimer>9000)
            {
                wltag_statetimer = HIDO_TimerGetTick();
                wltag_state = SEARCH; 
                OpenUWB();
            }
            break;
    }
      UWBIdleTask();
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
/* USER CODE BEGIN 4 */
static void SYSCLKConfig_STOP(void)
{
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  uint32_t pFLatency = 0;

  /* Enable Power Control clock */
  __HAL_RCC_PWR_CLK_ENABLE();

  /* Get the Oscillators configuration according to the internal RCC registers */
  HAL_RCC_GetOscConfig(&RCC_OscInitStruct);

  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_NONE;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /* Get the Clocks configuration according to the internal RCC registers */
  HAL_RCC_GetClockConfig(&RCC_ClkInitStruct, &pFLatency);

  /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2
     clocks dividers */
  RCC_ClkInitStruct.ClockType     = RCC_CLOCKTYPE_SYSCLK;
  RCC_ClkInitStruct.SYSCLKSource  = RCC_SYSCLKSOURCE_PLLCLK;
  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, pFLatency) != HAL_OK)
  {
    Error_Handler();
  }
}
void mcu_sleep(void)
{
  //  HAL_LPTIM_DeInit(&hlptim1);
    GPIO_InitTypeDef GPIO_InitStruct = { 0 };

  //  HAL_SPI_DeInit(&hspi1);
 //   CloseRTK4G();
   // HAL_GPIO_WritePin(GPIOA, GPIO_PIN_12|GPIO_PIN_8|GPIO_PIN_9|GPIO_PIN_4, GPIO_PIN_RESET);
//    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, GPIO_PIN_RESET);
//    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15|GPIO_PIN_12, GPIO_PIN_RESET);
      __HAL_FLASH_PREFETCH_BUFFER_DISABLE();

  /* Reset all RCC Sleep and Stop modes register to */
//  RCC->AHB1SMENR  = 0x0; 
//  RCC->AHB2SMENR  = 0x0;
//  RCC->AHB3SMENR  = 0x0;
//  
//  RCC->APB1SMENR1 = 0x0;
//  RCC->APB1SMENR2 = 0x0; 
//  RCC->APB2SMENR  = 0x0; 
  GPS_PowerOff();
    HAL_GPIO_WritePin(GPIOA,GPIO_PIN_15, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_2|GPIO_PIN_6, GPIO_PIN_RESET);
            HAL_SuspendTick();
     Set4LEDColor(LEDOFF,LEDOFF,LEDOFF,LEDOFF); 
     
   HAL_TIM_Base_DeInit(&htim3); 
    HAL_TIM_Base_DeInit(&htim1);
    HAL_TIM_Base_Stop_IT(&htim3);
    HAL_SPI_DeInit(&hspi1);
    HAL_UART_DeInit(&huart1);
    HAL_UART_DeInit(&huart2);
    HAL_UART_DeInit(&huart6);
     __HAL_RCC_DMA1_CLK_DISABLE();
     __HAL_RCC_DMA2_CLK_DISABLE();
    
    GPIO_InitStruct.Pin = GPIO_PIN_All;
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    //
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = GPIO_PIN_All&(~GPIO_PIN_8)&(~GPIO_PIN_0);
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
    /* Enter Sleep Mode , wake up is done once User push-button is pressed */
  //  HAL_PWR_EnterSLEEPMode(PWR_MAINREGULATOR_ON, PWR_SLEEPENTRY_WFI);
//TODO HAL_PWREx_EnterSTOP1Mode(PWR_STOPENTRY_WFI);
    /* Resume Tick interrupt if disabled prior to SLEEP mode entry */
     HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON,PWR_STOPENTRY_WFI);
         HAL_ResumeTick();
    nomove_count = 0;
    printf("%s URTRestart",__debug_info__);
    URTRestart();
   // HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI);
    //HAL_PWR_DisableWakeUpPin(PWR_WAKEUP_PIN1);
    /*Clear all related wakeup flags*/
   // __HAL_PWR_CLEAR_FLAG(PWR_FLAG_WU);
    /*Re-enable all used wakeup sources: Pin1(PA.0)*/
    //HAL_PWR_EnableWakeUpPin(PWR_WAKEUP_PIN1);
    /*Enter the Standby mode*/
   // HAL_PWR_EnterSTOPMode(PWR_MAINREGULATOR_ON,PWR_STOPENTRY_WFI);
//    __HAL_RCC_PWR_CLK_ENABLE();
//  
//  /* Ensure that MSI is wake-up system clock */ 
//  __HAL_RCC_WAKEUPSTOP_CLK_CONFIG(RCC_STOP_WAKEUPCLOCK_MSI);
//    HAL_PWREx_EnterSTOP2Mode(PWR_STOPENTRY_WFI);
//  //  SYSCLKConfig_STOP();
//    nomove_count = 0;
//    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5|GPIO_PIN_2, GPIO_PIN_SET);
}
extern u16 tagslotpos;
extern uint8_t tx_near_msg[80],stationary_flag,gotosleep_flag;
float key_keeptime;
extern float motor_ontime;