#include "ptz_header_file.h"
#include "angle_zerooffset.h"

///计算角度偏移量的效验码
unsigned int ptz_zero_offset_angle_crc(PtzZeroOffsetAngle offset)
{
  float crc = 0;
  unsigned int crc1 = 0;
  crc = crc + offset.hori_zero_offset_angle;
  crc = crc + offset.vert_zero_offset_angle;
  crc = crc + offset.hori_offset_switch;
  crc = crc + offset.vert_offset_switch;
  crc1 = (unsigned int)crc;
  return crc1;
}


///存储角度偏移量
char ptz_zero_offset_angle_save()
{
  PtzZeroOffsetAngle zero_offset_angle;
  PtzZeroOffsetAngle zero_offset_angle_a;
  char i,j;
  
  //复制数据
  memcpy(&zero_offset_angle, &g_ptz.offset_angle, sizeof(zero_offset_angle));
  if(g_ptz.offset_angle.hori_offset_switch == 2)
  {
    zero_offset_angle.hori_offset_switch = 1;
  }
  if(g_ptz.offset_angle.vert_offset_switch == 2)
  {
    zero_offset_angle.vert_offset_switch = 1;
  }
  //计算效验码
  zero_offset_angle.crc = ptz_zero_offset_angle_crc(zero_offset_angle);
  
  //写入主存储区
  for(i = 0; i < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM; i++)
  {
    memset(&zero_offset_angle_a, 0, sizeof(zero_offset_angle_a));
    write_many_data(sizeof(zero_offset_angle),(unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_FLASH_ADD);
    asm("nop");
    asm("nop");
    Flash_Read((unsigned char *)(&zero_offset_angle_a),ZERO_OFFSET_ANGLE_FLASH_ADD,sizeof(zero_offset_angle_a));
    if(memcmp(&zero_offset_angle, &zero_offset_angle_a, sizeof(zero_offset_angle)) == 0)//判断写入数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
  }
  
  
  //写入备份存储区
  for(j = 0; j < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM; j++)
  {
    memset(&zero_offset_angle_a, 0, sizeof(zero_offset_angle_a));
    write_many_data(sizeof(zero_offset_angle),(unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_BACKUP_FLASH_ADD);
    asm("nop");
    asm("nop");
    Flash_Read((unsigned char *)(&zero_offset_angle_a),ZERO_OFFSET_ANGLE_BACKUP_FLASH_ADD,sizeof(zero_offset_angle_a));
    if(memcmp(&zero_offset_angle, &zero_offset_angle_a, sizeof(zero_offset_angle)) == 0)//判断写入数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
  }
  
  if(i < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM || j < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM)//判断写入数据是否成功
  {
    //写入成功
    return 1;
  }
  //写入失败
  return 0;
}



///读取角度偏移量***初始化在get_angle.c中
char ptz_zero_offset_angle_read()
{
   PtzZeroOffsetAngle zero_offset_angle;
  char i,j;
  //首先从主存储区读取数据
  for(i = 0; i < ZERO_OFFSET_ANGLE_READ_FLASH_NUM; i++)
  {
    memset(&zero_offset_angle, 0, sizeof(zero_offset_angle));
    Flash_Read((unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_FLASH_ADD,sizeof(zero_offset_angle));
    if(zero_offset_angle.crc == ptz_zero_offset_angle_crc(zero_offset_angle))
    {
      g_ptz.offset_angle.hori_zero_offset_angle = zero_offset_angle.hori_zero_offset_angle;
      g_ptz.offset_angle.vert_zero_offset_angle = zero_offset_angle.vert_zero_offset_angle;
      g_ptz.offset_angle.hori_offset_switch = zero_offset_angle.hori_offset_switch;
      g_ptz.offset_angle.vert_offset_switch = zero_offset_angle.vert_offset_switch;
      if(g_ptz.offset_angle.hori_offset_switch == 1)
      {
        g_ptz.offset_angle.hori_offset_switch = 2;
      }
      else
      {
        g_ptz.offset_angle.hori_offset_switch = 0;
      }
      
      if(g_ptz.offset_angle.vert_offset_switch == 1)
      {
        g_ptz.offset_angle.vert_offset_switch = 2;
      }
      else
      {
        g_ptz.offset_angle.vert_offset_switch = 0;
      }
      return 1;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
  }
  //如果主存储区读取的数据是错误的，则从备份存储区读取数据
  for(j = 0; j < ZERO_OFFSET_ANGLE_READ_FLASH_NUM; j++)
  {
    memset(&zero_offset_angle, 0, sizeof(zero_offset_angle));
    Flash_Read((unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_BACKUP_FLASH_ADD,sizeof(zero_offset_angle));
    if(zero_offset_angle.crc == ptz_zero_offset_angle_crc(zero_offset_angle))
    {
      g_ptz.offset_angle.hori_zero_offset_angle = zero_offset_angle.hori_zero_offset_angle;
      g_ptz.offset_angle.vert_zero_offset_angle = zero_offset_angle.vert_zero_offset_angle;
      g_ptz.offset_angle.hori_offset_switch = zero_offset_angle.hori_offset_switch;
      g_ptz.offset_angle.vert_offset_switch = zero_offset_angle.vert_offset_switch;
      if(g_ptz.offset_angle.hori_offset_switch == 1)
      {
        g_ptz.offset_angle.hori_offset_switch = 2;
      }
      else
      {
        g_ptz.offset_angle.hori_offset_switch = 0;
      }
      
      if(g_ptz.offset_angle.vert_offset_switch == 1)
      {
        g_ptz.offset_angle.vert_offset_switch = 2;
      }
      else
      {
        g_ptz.offset_angle.vert_offset_switch = 0;
      }
      return 1;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
  }
  return 0;
}


//擦除角度偏移量
char ptz_zero_offset_angle_erase()
{
  char i, j;
  PtzZeroOffsetAngle zero_offset_angle;
  PtzZeroOffsetAngle zero_offset_angle_a;
  memset(&zero_offset_angle_a, 0, sizeof(zero_offset_angle_a));
  memset(&zero_offset_angle, 0, sizeof(zero_offset_angle));
  
  //擦除主存储区
  for(i = 0; i < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM; i++)
  {
    memset(&zero_offset_angle_a, 0, sizeof(zero_offset_angle_a));
    write_many_data(sizeof(zero_offset_angle),(unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_FLASH_ADD);
    asm("nop");
    asm("nop");
    Flash_Read((unsigned char *)(&zero_offset_angle_a),ZERO_OFFSET_ANGLE_FLASH_ADD,sizeof(zero_offset_angle_a));
    if(memcmp(&zero_offset_angle, &zero_offset_angle_a, sizeof(zero_offset_angle)) == 0)//判断擦除数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
  }
  //擦除备份存储区
  for(j = 0; j < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM; j++)
  {
    memset(&zero_offset_angle_a, 0, sizeof(zero_offset_angle_a));
    write_many_data(sizeof(zero_offset_angle),(unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_BACKUP_FLASH_ADD);
    asm("nop");
    asm("nop");
    Flash_Read((unsigned char *)(&zero_offset_angle_a),ZERO_OFFSET_ANGLE_BACKUP_FLASH_ADD,sizeof(zero_offset_angle_a));
    if(memcmp(&zero_offset_angle, &zero_offset_angle_a, sizeof(zero_offset_angle)) == 0)//判断擦除数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
  }
  
  if(i < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM || j < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM)//判断擦除数据是否成功
  {
    //擦除成功
    return 1;
  }
  //擦除失败
  return 0;
}


//偏移角度数据回传
void ptz_zero_offset_angle_return(char dev)
{
  unsigned short int uint16_angle;
  unsigned char data[7];
  
  //读取保存的偏移量
  ptz_zero_offset_angle_read();
 
  uint16_angle = (unsigned short int)(g_ptz.offset_angle.hori_zero_offset_angle * 100 + 0.5);
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x3e;
  data[3] = PTZ_HORI;//0x07;//PTZ_HORI;//水平偏移角度
  data[4] = (u_int16_t)(uint16_angle >> 8);
  data[5] = (u_int16_t)(uint16_angle & 0x00ff);
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  uint16_angle = (unsigned short int)(g_ptz.offset_angle.vert_zero_offset_angle * 100 + 0.5);
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x3e;
  data[3] = PTZ_VERT;//0x08;//PTZ_VERT;//垂直偏移角度
  data[4] = (u_int16_t)(uint16_angle >> 8);
  data[5] = (u_int16_t)(uint16_angle & 0x00ff);
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  
  ptz_send_data(dev, data, sizeof(data));
}


//设置零位偏移
void ptz_zero_offset_angle_set(char dev, PTZ_DATA_PACK *pack)
{
  int16_t int16_angle;
        switch(pack->command[1])
        {
          case 1://视频设置水平0位
            g_ptz.offset_angle.hori_zero_offset_angle = g_ptz.hori_angle_actual_a;
            g_ptz.offset_angle.hori_offset_switch = 1;
            g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
            if(ptz_zero_offset_angle_save() == 1)
            {
              ptz_reply(dev, CMD_RIGHT);
              g_ptz.offset_angle.hori_offset_switch = 2;
            }
            else
            {
              ptz_reply(dev, CMD_FAIL);
            }
            break;
            
          case 2://视频设置垂直0位
            g_ptz.offset_angle.vert_zero_offset_angle = g_ptz.vert_angle_actual_a;
            g_ptz.offset_angle.vert_offset_switch = 1;
            g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
            if(ptz_zero_offset_angle_save() == 1)
            {
              ptz_reply(dev, CMD_RIGHT);
              g_ptz.offset_angle.vert_offset_switch = 2;
            }
            else
            {
              ptz_reply(dev, CMD_FAIL);
            }
            
            if(g_ptz.offset_angle.vert_offset_switch == 2)
            {
              g_ptz.vert_angleP.angle_allow_min =  g_ptz.vert_angleP.angle_range / 2.0 * (-1);
              g_ptz.vert_angleP.angle_allow_max =  g_ptz.vert_angleP.angle_range / 2.0;
              g_ptz.vert_angleP.angle_allow_min = 
                g_ptz.vert_angleP.angle_allow_min - g_ptz.offset_angle.vert_zero_offset_angle;
              g_ptz.vert_angleP.angle_allow_max = 
                g_ptz.vert_angleP.angle_allow_max - g_ptz.offset_angle.vert_zero_offset_angle;
            }
            
            break;
            
          case 3://视频同时设置水平0位和垂直0位
            g_ptz.offset_angle.hori_zero_offset_angle = g_ptz.hori_angle_actual_a;
            g_ptz.offset_angle.hori_offset_switch = 1;

            g_ptz.offset_angle.vert_zero_offset_angle = g_ptz.vert_angle_actual_a;
            g_ptz.offset_angle.vert_offset_switch = 1;
            
            g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
            
            if(ptz_zero_offset_angle_save() == 1)
            {
              ptz_reply(dev, CMD_RIGHT);
              g_ptz.offset_angle.hori_offset_switch = 2;
              g_ptz.offset_angle.vert_offset_switch = 2;
            }
            else
            {
              ptz_reply(dev, CMD_FAIL);
            }
            
            if(g_ptz.offset_angle.vert_offset_switch == 2)
            {
              g_ptz.vert_angleP.angle_allow_min =  g_ptz.vert_angleP.angle_range / 2.0 * (-1);
              g_ptz.vert_angleP.angle_allow_max =  g_ptz.vert_angleP.angle_range / 2.0;
              g_ptz.vert_angleP.angle_allow_min = 
                g_ptz.vert_angleP.angle_allow_min - g_ptz.offset_angle.vert_zero_offset_angle;
              g_ptz.vert_angleP.angle_allow_max = 
                g_ptz.vert_angleP.angle_allow_max - g_ptz.offset_angle.vert_zero_offset_angle;
            }
            
            break;
          
          case 4://通过发送水平角度，设置水平0位
            g_ptz.offset_angle.hori_zero_offset_angle = 
              ((pack->data[0] << 8) | pack->data[1]) / 100.0;
            g_ptz.offset_angle.hori_offset_switch = 1;
            g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
            if(ptz_zero_offset_angle_save() == 1)
            {
              ptz_reply(dev, CMD_RIGHT);
              g_ptz.offset_angle.hori_offset_switch = 2;
            }
            else
            {
              ptz_reply(dev, CMD_FAIL);
            }
            break;
            
          case 5://通过发送垂直角度，设置垂直0位
            int16_angle = (pack->data[0] << 8) | pack->data[1];
            g_ptz.offset_angle.vert_zero_offset_angle = int16_angle / 100.0;
            g_ptz.offset_angle.vert_offset_switch = 1;
            g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
            if(ptz_zero_offset_angle_save() == 1)
            {
              ptz_reply(dev, CMD_RIGHT);
              g_ptz.offset_angle.vert_offset_switch = 2;
            }
            else
            {
              ptz_reply(dev, CMD_FAIL);
            }
            
            if(g_ptz.offset_angle.vert_offset_switch == 2)
            {
              g_ptz.vert_angleP.angle_allow_min =  g_ptz.vert_angleP.angle_range / 2.0 * (-1);
              g_ptz.vert_angleP.angle_allow_max =  g_ptz.vert_angleP.angle_range / 2.0;
              g_ptz.vert_angleP.angle_allow_min = 
                g_ptz.vert_angleP.angle_allow_min - g_ptz.offset_angle.vert_zero_offset_angle;
              g_ptz.vert_angleP.angle_allow_max = 
                g_ptz.vert_angleP.angle_allow_max - g_ptz.offset_angle.vert_zero_offset_angle;
            }
            break;
            
          case 6://删除基准0位偏移量，恢复初始0位
            if(ptz_zero_offset_angle_erase() == 1)
            {
              g_ptz.offset_angle.hori_offset_switch = 0;
              g_ptz.offset_angle.vert_offset_switch = 0;
              g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
              ptz_reply(dev, CMD_RIGHT);
            }
            else
            {
              ptz_reply(dev, CMD_FAIL);
            }
            
            g_ptz.vert_angleP.angle_allow_min =  
              g_ptz.vert_angleP.angle_range / 2.0 * (-1);
            g_ptz.vert_angleP.angle_allow_max =  
              g_ptz.vert_angleP.angle_range / 2.0;
            break;
        }
}

//查询偏移