#include "rotate_plan.h"
#include "ptz_header_file.h"
#include "gd32f4xx.h"
#include "angle_poweroffsave.h"
#include "mb85rc64.h"
static BSP_OS_SEM ptz_power_off_mutex;//共享资源锁

static float power_v;//实时电压
static float power_v1;//用于保存用于判断电压升高还是降低的基本电压

PowerOffData power_off_data;

//电压采集
static float ptz_voltage_adc1()
{
  unsigned short int adc1_value;
  float float_adc1;
  
  adc_software_trigger_enable(ADC1,ADC_INSERTED_CHANNEL);
  adc1_value = ADC_IDATA1(ADC1);
  
  float_adc1 = (float)adc1_value / 4096.0 * 36.3;
  return float_adc1;
}

static float ptz_power_off_data_crc(PowerOffData data)
{
  float crc = 0;
  crc = data.hori_as5047d.as5047d_ptz_init_angle       +
        data.hori_as5047d.as5047d_ptz_angle_max        +
        data.hori_as5047d.as5047d_ptz_angle_K          +
        data.hori_as5047d.as5047d_ptz_angle_wf_actual  +
        data.hori_as5047d.as5047d_ptz_angle_wf_last    +
        data.hori_as5047d.as5047d_ptz_angle_actual     +
        data.hori_as5047d.as5047d_ptz_angle_last       +
        data.hori_as5047d.as5047d_remain_angle         +
        data.hori_as5047d.as5047d_cycle_num            +
        data.hori_as5047d.as5047d_cycle_count          +
        data.hori_as5047d.as5047d_cycle_count_last     +
        data.hori_as5047d.as5047d_angle_actual         +
        data.hori_as5047d.as5047d_dir                  +
        data.hori_as5047d.as5047d_angle_last           +
        data.hori_as5047d.as5047d_angle_difference     +
        data.hori_as5047d.as5047d_state                +
        data.hori_as5047d.as5047d_data_reset           + 
  
        data.vert_as5047d.as5047d_ptz_init_angle       +
        data.vert_as5047d.as5047d_ptz_angle_max        +
        data.vert_as5047d.as5047d_ptz_angle_K          +
        data.vert_as5047d.as5047d_ptz_angle_wf_actual  +
        data.vert_as5047d.as5047d_ptz_angle_wf_last    +
        data.vert_as5047d.as5047d_ptz_angle_actual     +
        data.vert_as5047d.as5047d_ptz_angle_last       +
        data.vert_as5047d.as5047d_remain_angle         +
        data.vert_as5047d.as5047d_cycle_num            +
        data.vert_as5047d.as5047d_cycle_count          +
        data.vert_as5047d.as5047d_cycle_count_last     +
        data.vert_as5047d.as5047d_angle_actual         +
        data.vert_as5047d.as5047d_dir                  +
        data.vert_as5047d.as5047d_angle_last           +
        data.vert_as5047d.as5047d_angle_difference     +
        data.vert_as5047d.as5047d_state                +
        data.vert_as5047d.as5047d_data_reset           +
        data.vert_angle                                +
        data.hori_angle                                +
        data.power_down_offset_anle                    +

          
        data.num;
  
  return crc;
}




char ptz_power_off_data_save(unsigned short int add)
{
  BSP_OS_SemWait(&ptz_power_off_mutex, 0u);
  PowerOffData power_off_data_b;
  int i;
  memcpy(&power_off_data.hori_as5047d, &g_ptz.hori_as5047d, sizeof(g_ptz.hori_as5047d));
  memcpy(&power_off_data.vert_as5047d, &g_ptz.vert_as5047d, sizeof(g_ptz.vert_as5047d));
  power_off_data.vert_angle = g_ptz.vert_angle_actual;
  power_off_data.hori_angle = g_ptz.hori_angle_actual; 
  power_off_data.power_down_offset_anle = g_ptz.offset_angle.offset_anle;//误差补偿参数保存
  power_off_data.crc = ptz_power_off_data_crc(power_off_data);
  
  for(i = 0; i < PTZ_POWER_OFF_DATA_SAVE_NUM; i++)
  {
    memset(&power_off_data_b, 0, sizeof(power_off_data_b));
    mb85rc64_page_write(add, (unsigned char *)&power_off_data, sizeof(power_off_data));
    asm("nop");
    asm("nop");
    mb85rc64_add_read(add, (unsigned char *)&power_off_data_b, sizeof(power_off_data_b));
    if(memcmp(&power_off_data_b, &power_off_data, sizeof(power_off_data)) == 0)//判断擦除数据是否正确
    {
      BSP_OS_SemPost(&ptz_power_off_mutex);
      return 1;
    }
  }
  BSP_OS_SemPost(&ptz_power_off_mutex);
  return 0;
}

char ptz_power_off_data_read()
{
  BSP_OS_SemWait(&ptz_power_off_mutex, 0u);
  char i;
  PowerOffData power_off_data_a;
  PowerOffData power_off_data_b;
  char data_a = 0;
  char data_b = 0;
  
  memset(&power_off_data, 0, sizeof(power_off_data));
  //读取存储区域a的数据
  for(i = 0; i < PTZ_POWER_OFF_DATA_READ_NUM; i++)
  {
    memset(&power_off_data_a, 0, sizeof(power_off_data_a));
    mb85rc64_add_read(PTZ_MB85RC64_ADD_A, (unsigned char *)&power_off_data_a, sizeof(power_off_data_a));
    if(power_off_data_a.crc == ptz_power_off_data_crc(power_off_data_a) && power_off_data_a.crc != 0)
    {
       data_a = 1;
       break;
    }
  }
  
  //读取存储区域b的数据
  for(i = 0; i < PTZ_POWER_OFF_DATA_READ_NUM; i++)
  {
    memset(&power_off_data_b, 0, sizeof(power_off_data_b));
    mb85rc64_add_read(PTZ_MB85RC64_ADD_B, (unsigned char *)&power_off_data_b, sizeof(power_off_data_b));
    if(power_off_data_b.crc == ptz_power_off_data_crc(power_off_data_b) && power_off_data_b.crc != 0)
    {
       data_b = 1;
       break;
    }
  }
  
  if(data_a == 1 && data_b == 0)//只有存储区A的数据是对的,则只用A的数据
  {
    memcpy(&power_off_data, &power_off_data_a, sizeof(power_off_data));
    memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
    memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
    
    g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
    
    if(power_off_data.num == 65535)
    {
      power_off_data.num = 0;
    }
    else
    {
      power_off_data.num ++;
    }
    
    BSP_OS_SemPost(&ptz_power_off_mutex);
    return 1;
  }
  
  
  if(data_a == 0 && data_b == 1)//只有存储区B的数据是对的,则只用B的数据
  {
    memcpy(&power_off_data, &power_off_data_b, sizeof(power_off_data));
    memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
    memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
    
    g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
    
    if(power_off_data.num == 65535)
    {
      power_off_data.num = 0;
    }
    else
    {
      power_off_data.num ++;
    }
    
    BSP_OS_SemPost(&ptz_power_off_mutex);
    return 1;
  }
  
  if(data_a == 1 && data_b == 1)//如果存储区A的数据是对的，存储区B的数据也是对的
  {
    if(power_off_data_a.num > power_off_data_b.num)
    {
      if((power_off_data_a.num - power_off_data_b.num) == 1)
      {
        memcpy(&power_off_data, &power_off_data_a, sizeof(power_off_data));
        memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
        memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
        
        g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
        
        if(power_off_data.num == 65535)
        {
          power_off_data.num = 0;
        }
        else
        {
          power_off_data.num ++;
        }
        
        BSP_OS_SemPost(&ptz_power_off_mutex);
        return 1;
      }
      
      if((power_off_data_a.num - power_off_data_b.num) == 65535)
      {
        memcpy(&power_off_data, &power_off_data_b, sizeof(power_off_data));
        memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
        memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
        
        g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
        
        if(power_off_data.num == 65535)
        {
          power_off_data.num = 0;
        }
        else
        {
          power_off_data.num ++;
        }

        BSP_OS_SemPost(&ptz_power_off_mutex);
        return 1;
      }
    }
    
    if(power_off_data_b.num > power_off_data_a.num)
    {
      if((power_off_data_b.num - power_off_data_a.num) == 1)
      {
        memcpy(&power_off_data, &power_off_data_b, sizeof(power_off_data));
        memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
        memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
        
        g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
        
        if(power_off_data.num == 65535)
        {
          power_off_data.num = 0;
        }
        else
        {
          power_off_data.num ++;
        }

        BSP_OS_SemPost(&ptz_power_off_mutex);
        return 1;
      }
      
      if((power_off_data_b.num - power_off_data_a.num) == 65535)
      {
        memcpy(&power_off_data, &power_off_data_a, sizeof(power_off_data));
        memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
        memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
        
        g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
        
        if(power_off_data.num == 65535)
        {
          power_off_data.num = 0;
        }
        else
        {
          power_off_data.num ++;
        }
        
        BSP_OS_SemPost(&ptz_power_off_mutex);
        return 1;
      }
    }
    
  }
  
  
  BSP_OS_SemPost(&ptz_power_off_mutex);
  return 0;
}

char ptz_power_off_data_erase(unsigned short int add)
{
  BSP_OS_SemWait(&ptz_power_off_mutex, 0u);
  PowerOffData power_off_data_a;
  int i;
  memset(&power_off_data, 0, sizeof(power_off_data));
  power_off_data.crc = -100;

  for(i = 0; i < PTZ_POWER_OFF_DATA_SAVE_NUM; i++)
  {
    memset(&power_off_data_a, 0, sizeof(power_off_data_a));
    mb85rc64_page_write(add, (unsigned char *)&power_off_data, sizeof(power_off_data));
    asm("nop");
    asm("nop");
    mb85rc64_add_read(add, (unsigned char *)&power_off_data_a, sizeof(power_off_data_a));
    if(memcmp(&power_off_data_a, &power_off_data, sizeof(power_off_data)) == 0)//判断擦除数据是否正确
    {
      BSP_OS_SemPost(&ptz_power_off_mutex);
      return 1;
    }
  }
  BSP_OS_SemPost(&ptz_power_off_mutex);
  return 0;
}





#define  ADC1_NUM    5 //电压采集次数

static char ptz_power_off_task()
{
  unsigned short int number;
#ifdef PTZ_POWER_OFF_ADC_MANY 
  static float adc1_v[ADC1_NUM];
  float tem;
  char i,j;
#endif
  while(1)
  {
#ifdef PTZ_POWER_OFF_ADC_MANY    
    //连续读取电压
    for(i = 0; i < ADC1_NUM; i++)
    {
      tem = 0;
      tem = ptz_voltage_adc1();
      if(tem < 0 || isnan(tem) == 1)
      {
        tem = 0;
      }
      adc1_v[i] = tem;
      OSTimeDlyHMSM(0u, 0u, 0u, 1u);
    }
    //对读取电压排序
    for(i = 0; i < ADC1_NUM-1; i++)//采样值由小到大排列
    {
      for(j = 0; j < ADC1_NUM-i-1; j++)
      {
        if(adc1_v[j] > adc1_v[j+1])
        {
           tem         = adc1_v[j];
           adc1_v[j]   = adc1_v[j+1];
           adc1_v[j+1] = tem;
        }
       }
     }
    //去掉一个最小值和一个最大值求和
    tem = 0;
    for(i = 1; i < ADC1_NUM - 1; i++)
    {
      tem = tem + adc1_v[i];
    }
    tem = tem /((float)(ADC1_NUM - 2));//去掉一个最大值和一个最小值求平均值
    
    
    g_ptz.Voltage = tem;
    power_v = tem;
#endif    

#ifdef PTZ_POWER_OFF_ADC_SINGLE  
    
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
    //读取实时电压
    g_ptz.Voltage = ptz_voltage_adc1();
    //判断读出的电压是否大于0以及是否是一个数字
    if(g_ptz.Voltage < 0 || isnan(g_ptz.Voltage) == 1)
    {
      g_ptz.Voltage = 0;
    }
    if(g_ptz.Voltage == 0)//如果采集的值为0（采集数据异常为0或者电压数据确实降到了0），则再从新采集一次，
                          //可以防止某个时刻出现电压采集出错，导致云台误判为电源关电
    {
      //OSTimeDlyHMSM(0u, 0u, 0u, 1u);
      asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
      g_ptz.Voltage = ptz_voltage_adc1();//再采集一次
      asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
      g_ptz.Voltage = ptz_voltage_adc1();//再采集一次
      //判断读出的电压是否小于于0以及是否是一个数字
      if(g_ptz.Voltage < 0 || isnan(g_ptz.Voltage) == 1)
      {
        g_ptz.Voltage = 0;
      }
    }
    power_v = g_ptz.Voltage;

#endif   
    
/*****************************************强制保存位置数据********************************************************/       
    if(g_ptz.no_self_check_force_save == 1)
    {
      //存储云台当前运行的数据
      if((power_off_data.num & 1) == 0)//偶数
      {
        if(ptz_power_off_data_save(PTZ_MB85RC64_ADD_A) == 1)//存储云台当前运行的数据
        {
          if(power_off_data.num == 65535)
          {
            
            power_off_data.num = 0;
          }
          else
          {
            power_off_data.num ++;
          }
        }
      }
      else
      {
        if(ptz_power_off_data_save(PTZ_MB85RC64_ADD_B) == 1)//存储云台当前运行的数据
        {
          if(power_off_data.num == 65535)
          {
            power_off_data.num = 0;
          }
          else
          {
            power_off_data.num ++;
          }
        }
      }
      g_ptz.no_self_check_force_save = 2;//表示强制保存过云台数据
    }
    
    if((g_ptz.hori_start_stop_set == PTZ_HORI_START || g_ptz.vert_start_stop_set == PTZ_VERT_START)
       && g_ptz.no_self_check_force_save == 2)
    {//强制保存数据后，如果云台又转动了，则又恢复自动判断掉电保存功能
      g_ptz.no_self_check_force_save = 0;
    }
/**************************************强制擦除保存的位置数据******************************************************/   
    if(g_ptz.no_self_check_force_erase == 1)
    {
      ptz_power_off_data_erase(PTZ_MB85RC64_ADD_A);
      ptz_power_off_data_erase(PTZ_MB85RC64_ADD_B);
      g_ptz.no_self_check_force_erase = 2;
    }
/********************************************自动保存位置数据*****************************************************/      
    if(g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_END &&
       g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_END && 
       g_ptz.no_self_check_force_save  != 2 && 
       g_ptz.no_self_check_force_erase != 2 &&
       g_ptz.no_self_check_state       != 1 &&
       g_ptz.hori_angle_state          == 1 &&
       g_ptz.vert_angle_state          == 1  )
    {
      //电压出现大的下降
      if(power_v < power_v1 && ((power_v1 - power_v) >= PTZ_POWER_DOWN_INC))
      {
        if((power_off_data.num & 1) == 0)//偶数
        {
          if(ptz_power_off_data_save(PTZ_MB85RC64_ADD_A) == 1)//存储云台当前运行的数据
          {
            if(power_off_data.num == 65535)
            {
              power_off_data.num = 0;
            }
            else
            {
              power_off_data.num ++;
            }
            
            power_v1 = power_v;
          }
        }
        else
        {
          if(ptz_power_off_data_save(PTZ_MB85RC64_ADD_B) == 1)//存储云台当前运行的数据
          {
            if(power_off_data.num == 65535)
            {
              power_off_data.num = 0;
            }
            else
            {
              power_off_data.num ++;
            }
            
            power_v1 = power_v;
          }
        }
        
        
      }
      
      //电压升高了
      if(power_v > power_v1)
      {
        power_v1 = power_v;
      }
//      term_printf("\n               power down save angle        \r\n\r\n");//测试
 
    }
   
/**********************************启动后，只要云台一转动就要擦除原先保存的位置数据**************************************/          
    if(g_ptz.no_self_check_state == 1 && 
      (g_ptz.hori_start_stop_set == PTZ_HORI_START || g_ptz.vert_start_stop_set == PTZ_VERT_START))
    {
      number = power_off_data.num;
      ptz_power_off_data_erase(PTZ_MB85RC64_ADD_A);
      ptz_power_off_data_erase(PTZ_MB85RC64_ADD_B);
      power_off_data.num = number;
      g_ptz.no_self_check_state = 2;
    }
    
  }
}


static OS_STK  task_power_off_stk[POWER_OFF_STK_SIZE];
static  void  creat_task_power_off(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_power_off_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_power_off_stk[POWER_OFF_STK_SIZE - 1],
		   (INT8U           ) POWER_OFF_PRIO,                   
		   (INT16U          ) POWER_OFF_PRIO,
		   (OS_STK         *)&task_power_off_stk[0],
		   (INT32U          ) POWER_OFF_STK_SIZE,
		   (void           *) 0,
		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));     
    #if (OS_TASK_NAME_EN > 0)
    OSTaskNameSet(POWER_OFF_PRIO, "ptz_power_off_task", &name_err);
    #endif
       

}

void init_power_off_module(void)
{
#ifdef PTZ_NO_SELF_CHECK 
  mb85rc64_gpio_init();
  
  BSP_OS_SemCreate(&ptz_power_off_mutex,1u,"ptz_power_off_mutex");//创建信号量
  
  creat_task_power_off();
#endif
}