#include "wj_716_lidar_protocol.h"
#include <iostream>

namespace wj_lidar
{

/**
 * 扫描点云转换为欧式坐标,并根据标定参数进行坐标变换
 * */
Error_manager scan_transform(wj::wjLidarParams params, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out)
{
  // 1. 合法性检验
  if (!params.has_net_config() || !params.has_transform() || !params.has_scan_limit())
  {
    LOG(WARNING) << "网络参数 " << params.has_net_config() << ", 坐标变换参数: " << params.has_transform() << ", 范围限制: " << params.has_scan_limit();
    return Error_manager(Error_code::PARAMETER_ERROR);
  }

  // 2. 将极坐标转换为欧式坐标，其中原极坐标r存于点云x坐标中，原极坐标theta通过角度最小值与角分辨率计算
  float angle = params.angle_min();
  cloud_out->clear();
  // LOG(INFO) << "cloud size before transform: " <<cloud_in->size();
  for (size_t i = 0; i < cloud_in->size(); ++i)
  {
    const float first_echo = cloud_in->points[i].x;
    // within the border
    if (params.range_min() <= first_echo && first_echo <= params.range_max() && first_echo <= params.scan_limit().dist_limit())
    {
      const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
      Eigen::MatrixXf mat = rotation * (first_echo * Eigen::Vector3f::UnitX());
      /*Eigen::Matrix<float,4,1>  e_transform;
            e_transform<<mat,1.0;
            Eigen::MatrixXf  new_pos=tf_btol*e_transform;*/
      float x = mat(0, 0);
      float y = mat(1, 0);

      if (x < params.scan_limit().minx() || x > params.scan_limit().maxx() || y < params.scan_limit().miny() || y > params.scan_limit().maxy())
      {
        angle += params.angle_increment();
        continue;
      }
      float newx = x * params.transform().m00() + y * params.transform().m01() + params.transform().m02();
      float newy = x * params.transform().m10() + y * params.transform().m11() + params.transform().m12();
      pcl::PointXYZ point(newx, newy, 0);
      cloud_out->push_back(point);
      // cloud_in->points[i].x = newx;
      // cloud_in->points[i].y = newy;
    }
    angle += params.angle_increment();
  }
  return Error_manager(Error_code::SUCCESS);
}

/**
 * 设置参数
 * */
void Wj_716_lidar_protocol::set_config(wj::wjLidarParams &params)
{
  // m_lidar_params.CopyFrom(params);
  m_lidar_params.set_angle_min(params.angle_min());
  m_lidar_params.set_angle_max(params.angle_max());
  m_lidar_params.set_angle_increment(params.angle_increment());
  m_lidar_params.set_range_min(params.range_min());
  m_lidar_params.set_range_max(params.range_max());
  m_lidar_params.set_range_min(params.range_min());
  m_lidar_params.mutable_net_config()->set_ip_address(params.net_config().ip_address());
  m_lidar_params.mutable_net_config()->set_port(params.net_config().port());
  m_lidar_params.mutable_scan_limit()->set_maxx(params.scan_limit().maxx());
  m_lidar_params.mutable_scan_limit()->set_minx(params.scan_limit().minx());
  m_lidar_params.mutable_scan_limit()->set_maxy(params.scan_limit().maxy());
  m_lidar_params.mutable_scan_limit()->set_miny(params.scan_limit().miny());
  m_lidar_params.mutable_scan_limit()->set_dist_limit(params.scan_limit().dist_limit());
  m_lidar_params.mutable_transform()->set_m00(params.transform().m00());
  m_lidar_params.mutable_transform()->set_m01(params.transform().m01());
  m_lidar_params.mutable_transform()->set_m02(params.transform().m02());
  m_lidar_params.mutable_transform()->set_m10(params.transform().m10());
  m_lidar_params.mutable_transform()->set_m11(params.transform().m11());
  m_lidar_params.mutable_transform()->set_m12(params.transform().m12());
  LOG(INFO) << "\n" << m_lidar_params.DebugString();
  // LOG(INFO) << "min_ang:" << m_lidar_params.angle_min();
  // LOG(INFO) << "max_ang:" << m_lidar_params.angle_max();
  // LOG(INFO) << "angle_increment:" << m_lidar_params.angle_increment();
  // LOG(INFO) << "time_increment:" << m_lidar_params.time_increment();
  // LOG(INFO) << "range_min:" << m_lidar_params.range_min();
  // LOG(INFO) << "range_max:" << m_lidar_params.range_max();
}

/**
 * 获取更新扫描点云时间
 * */
std::chrono::steady_clock::time_point Wj_716_lidar_protocol::get_scan_time()
{
  return m_scan_cloud_time;
}

/**
 * 获取更新二次回波时间
 * */
std::chrono::steady_clock::time_point Wj_716_lidar_protocol::get_two_echo_time()
{
  return m_two_echo_cloud_time;
}

/**
 * 获取扫描点云
 * */
Error_manager Wj_716_lidar_protocol::get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out)
{
  Error_manager result;
  m_scan_mutex.lock();
  result = scan_transform(m_lidar_params, mp_scan_points, cloud_out);
  m_scan_mutex.unlock();
  if (result != SUCCESS)
  {
    LOG(WARNING) << "获取点云失败，参数错误";
  }
  return result;
}

/**
 * 获取二次回波点云
 * */
Error_manager Wj_716_lidar_protocol::get_two_echo_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out)
{
  Error_manager result;
  m_two_echo_mutex.lock();
  result = scan_transform(m_lidar_params, mp_two_echo_points, cloud_out);
  m_two_echo_mutex.unlock();
  if (result != SUCCESS)
  {
    LOG(WARNING) << "获取点云失败，参数错误";
  }
  return result;
}

/**
 * 万集协议栈初始化
 * */
Error_manager Wj_716_lidar_protocol::initialize(wj::wjLidarParams params)
{
  memset(&m_sdata, 0, sizeof(m_sdata));
  memset(m_scandata, 0, 811);
  memset(m_scandata_te, 0, 811);
  memset(scaninden, 0, 811);

  m_g_u32PreFrameNo = 0;
  mp_scan_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
  mp_two_echo_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
  mp_scan_points->resize(811);
  mp_two_echo_points->resize(811);

  m_lidar_params.set_angle_min(-2.35619449);
  m_lidar_params.set_angle_max(2.35619449);
  m_lidar_params.set_angle_increment(0.00582);
  m_lidar_params.set_time_increment(0.0667 / 1081);
  m_lidar_params.set_range_min(0);
  m_lidar_params.set_range_max(30);

  // 设置初始时间为昨天，以避免第一次点云更新前获取到空点云
  m_scan_cloud_time = std::chrono::steady_clock::now() - std::chrono::duration<int, std::ratio<60 * 60 * 24>>(1);
  m_two_echo_cloud_time = std::chrono::steady_clock::now() - std::chrono::duration<int, std::ratio<60 * 60 * 24>>(1);

  set_config(params);
  LOG(INFO) << "wj_716_lidar_protocol start successfully";
  mb_initialized = true;
  return Error_manager(Error_code::SUCCESS);
}

Wj_716_lidar_protocol::Wj_716_lidar_protocol():
mb_initialized(0),
s_n32ScanIndex(0)
{
}

Wj_716_lidar_protocol::~Wj_716_lidar_protocol()
{
  LOG(INFO) << "protocol destructed";
}

/**
 * 数据处理主函数
 **/
Error_manager Wj_716_lidar_protocol::data_process(const char *data, const int reclen)
{
  // 1. 判断长度合法性
  if (reclen > MAX_LENGTH_DATA_PROCESS)
  {
    return Error_manager(Error_code::WJ_PROTOCOL_EXCEED_MAX_SIZE);
  }

  if (m_sdata.m_u32in + reclen > MAX_LENGTH_DATA_PROCESS)
  {
    memset(&m_sdata, 0, sizeof(m_sdata));
    return Error_manager(Error_code::WJ_PROTOCOL_EXCEED_MAX_SIZE);
  }
  memcpy(&m_sdata.m_acdata[m_sdata.m_u32in], data, reclen * sizeof(char));
  m_sdata.m_u32in += reclen;

  // 2. 数据处理
  while (m_sdata.m_u32out < m_sdata.m_u32in)
  {
    // LOG(INFO) << "in: " << m_sdata.m_u32in << " out: " << m_sdata.m_u32out;
    if (m_sdata.m_acdata[m_sdata.m_u32out] == 0x02 && m_sdata.m_acdata[m_sdata.m_u32out + 1] == 0x02 &&
        m_sdata.m_acdata[m_sdata.m_u32out + 2] == 0x02 && m_sdata.m_acdata[m_sdata.m_u32out + 3] == 0x02)
    {
      unsigned l_u32reallen = (m_sdata.m_acdata[m_sdata.m_u32out + 4] << 24) |
                              (m_sdata.m_acdata[m_sdata.m_u32out + 5] << 16) |
                              (m_sdata.m_acdata[m_sdata.m_u32out + 6] << 8) |
                              (m_sdata.m_acdata[m_sdata.m_u32out + 7] << 0);
      l_u32reallen = l_u32reallen + 9;

      if (l_u32reallen <= (m_sdata.m_u32in - m_sdata.m_u32out + 1))
      {
        Error_manager code = on_recv_process(&m_sdata.m_acdata[m_sdata.m_u32out], l_u32reallen);
        // LOG(INFO) << "in: " << m_sdata.m_u32in << " out: " << m_sdata.m_u32out << " frame size: " << l_u32reallen;
        if (code == SUCCESS)
        {
          m_sdata.m_u32out =  m_sdata.m_u32out + l_u32reallen;
        }
        else
        {
          LOG(INFO) << "continuous frame" << l_u32reallen;
          int i;
          for (i == 1; i < l_u32reallen; i++)
          {
            if ((m_sdata.m_acdata[m_sdata.m_u32out + i] == 0x02) &&
                (m_sdata.m_acdata[m_sdata.m_u32out + i + 1] == 0x02) &&
                (m_sdata.m_acdata[m_sdata.m_u32out + i + 2] == 0x02) &&
                (m_sdata.m_acdata[m_sdata.m_u32out + i + 3] == 0x02))
            {
              m_sdata.m_u32out += i;
              break;
            }
            if (i == l_u32reallen)
            {
              m_sdata.m_u32out += l_u32reallen;
            }
          }
        }
      }
      else if (l_u32reallen >= MAX_LENGTH_DATA_PROCESS)
      {
        LOG(INFO) << "l_u32reallen >= MAX_LENGTH_DATA_PROCESS";
        LOG(INFO) << "reallen: " << l_u32reallen;
        memset(&m_sdata, 0, sizeof(m_sdata));
      }
      else
      {
        LOG(INFO)<<"reallen: "<<l_u32reallen<<" indata: "<<m_sdata.m_u32in<<" outdata: "<<m_sdata.m_u32out;
        break;
      }
    }
    else
    {
      m_sdata.m_u32out++;
    }
  } //end while(m_sdata.m_u32out < m_sdata.m_u32in)

  if (m_sdata.m_u32out >= m_sdata.m_u32in)
  {
    memset(&m_sdata, 0, sizeof(m_sdata));
  }
  return Error_manager(Error_code::SUCCESS);
}

/**
 * 获取消息处理, 判断参数合法性，完整性检验并调用协议栈解析
 * */
Error_manager Wj_716_lidar_protocol::on_recv_process(char *data, int len)
{
  if (len > 0)
  {
    Error_manager code = check_xor(data, len);
    if (code == SUCCESS)
    {
      code = protocol(data, len);
      if (code != SUCCESS)
      {
        LOG(WARNING) << "万集雷达协议解析失败";
      }
    }
    else
    {
      LOG(WARNING) << "万集雷达信息完整性验证失败";
    }
  }
  else
  {
    return Error_manager(Error_code::WJ_PROTOCOL_EMPTY_PACKAGE);
  }
  return Error_manager(Error_code::SUCCESS);
}

/**
 * 协议栈解析
 * */
Error_manager Wj_716_lidar_protocol::protocol(const char *data, const int len)
{
  // LOG(INFO) << "command type " << data[8] << data[9] << " package number: " << (data[50] << 8) + data[51];
  if ((data[8] == 0x73 && data[9] == 0x52) || (data[8] == 0x73 && data[9] == 0x53)) //command type:0x73(s) 0x52/0X53(R/S)
  {
    // static int s_n32ScanIndex;
    int l_n32PackageNo = (data[50] << 8) + data[51];                                              //shuju bao xu hao
    unsigned int l_u32FrameNo = (data[46] << 24) + (data[47] << 16) + (data[48] << 8) + data[49]; //quan hao

    if (l_n32PackageNo == 1)
    {
      s_n32ScanIndex = 0;
      m_g_u32PreFrameNo = l_u32FrameNo;

      for (int j = 0; j < 810; j = j + 2)
      {
        m_scandata[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
        m_scandata[s_n32ScanIndex] /= 1000.0;
        if (m_scandata[s_n32ScanIndex] >= 25 || m_scandata[s_n32ScanIndex] == 0)
        {
          m_scandata[s_n32ScanIndex] = NAN;
        }
        s_n32ScanIndex++;
      }
      //cout << "quan hao1: " << g_u32PreFrameNo << "  danzhen quanhao1: " << l_u32FrameNo<< endl;
    }
    else if (l_n32PackageNo == 2)
    {
      if (m_g_u32PreFrameNo == l_u32FrameNo)
      {
        m_scan_mutex.lock();
        //  m_scan_points.clear();
        for (int j = 0; j < 812; j = j + 2)
        {
          m_scandata[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
          m_scandata[s_n32ScanIndex] /= 1000.0;
          if (m_scandata[s_n32ScanIndex] >= 25 || m_scandata[s_n32ScanIndex] == 0)
          {
            m_scandata[s_n32ScanIndex] = NAN;
          }
          // m_scan_points[s_n32ScanIndex].intensity = 0;
          //  scan.intensities[s_n32ScanIndex] = 0;
          s_n32ScanIndex++;
        }
        m_scan_mutex.unlock();
      }
      else
      {
        s_n32ScanIndex = 0;
        //cout << "quan hao2: " << g_u32PreFrameNo << "  danzhen quanhao2: " << l_u32FrameNo<< endl;
        return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
      }
    }
    else if (l_n32PackageNo == 3)
    {
      s_n32ScanIndex = 0;
      if (m_g_u32PreFrameNo == l_u32FrameNo)
      {
        for (int j = 0; j < 810; j = j + 2)
        {
          scaninden[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
          s_n32ScanIndex++;
        }
      }
      else
      {
        s_n32ScanIndex = 0;
        //cout << "quan hao3: " << g_u32PreFrameNo << "  danzhen quanhao3: " << l_u32FrameNo<< endl;
        return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
      }
    }
    else if (l_n32PackageNo == 4)
    {
      if (m_g_u32PreFrameNo == l_u32FrameNo)
      {
        for (int j = 0; j < 812; j = j + 2)
        {
          scaninden[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
          s_n32ScanIndex++;
        }

        // adjust angle_min to min_ang config param
        int index_min = (m_lidar_params.angle_min() + 2.35619449) / m_lidar_params.angle_increment();
        // adjust angle_max to max_ang config param
        int index_max = 811 - ((2.35619449 - m_lidar_params.angle_max()) / m_lidar_params.angle_increment());
        m_scan_mutex.lock();
        mp_scan_points->clear();
        // mp_scan_points->resize(index_max - index_min);
        //  scan.ranges.resize(index_max-index_min);
        //  scan.intensities.resize(index_max-index_min);

        for (int j = index_min; j <= index_max; ++j)
        {
          pcl::PointXYZ point;
          point.x = m_scandata[j];
          point.y = 0;
          point.z = 0;
          mp_scan_points->push_back(point);
          // mp_scan_points->points[j - index_min].x = m_scandata[j];
          // mp_scan_points->points[j - index_min].y = 0;
          // mp_scan_points->points[j - index_min].z = 0;

          // m_scan_points[j - index_min].intensity = scaninden[j];
          //  scan.ranges[j - index_min] = scandata[j];
          //  scan.intensities[j - index_min]=scaninden[j];
        }
        m_scan_mutex.unlock();
        m_scan_cloud_time = std::chrono::steady_clock::now();
        // LOG(INFO) << "protocol cloud size: " << mp_scan_points->size();
      }
      else
      {
        s_n32ScanIndex = 0;
        //cout << "quan hao4: " << g_u32PreFrameNo << "  danzhen quanhao4: " << l_u32FrameNo<< endl;
        return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
      }
    }
    else if (l_n32PackageNo == 5)
    {
      s_n32ScanIndex = 0;
      if (m_g_u32PreFrameNo == l_u32FrameNo)
      {
        for (int j = 0; j < 810; j = j + 2)
        {
          m_scandata_te[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
          m_scandata_te[s_n32ScanIndex] /= 1000.0;
          if (m_scandata_te[s_n32ScanIndex] >= 55 || m_scandata_te[s_n32ScanIndex] == 0)
          {
            m_scandata_te[s_n32ScanIndex] = NAN;
          }
          s_n32ScanIndex++;
        }
      }
      else
      {
        s_n32ScanIndex = 0;
        //cout << "quan hao5: " << g_u32PreFrameNo << "  danzhen quanhao5: " << l_u32FrameNo<< endl;
        return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
      }
    }
    else if (l_n32PackageNo == 6)
    {
      if (m_g_u32PreFrameNo == l_u32FrameNo)
      {
        for (int j = 0; j < 812; j = j + 2)
        {
          m_scandata_te[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
          m_scandata_te[s_n32ScanIndex] /= 1000.0;
          if (m_scandata_te[s_n32ScanIndex] >= 55 || m_scandata_te[s_n32ScanIndex] == 0)
          {
            m_scandata_te[s_n32ScanIndex] = NAN;
          }
          s_n32ScanIndex++;
        }

        int index_min = (m_lidar_params.angle_min() + 2.35619449) / m_lidar_params.angle_increment();
        // adjust angle_max to max_ang config param
        int index_max = 811 - ((2.35619449 - m_lidar_params.angle_max()) / m_lidar_params.angle_increment());
        m_two_echo_mutex.lock();
        mp_two_echo_points->clear();
        // mp_two_echo_points->resize(index_max - index_min);
        //  scan_TwoEcho.ranges.resize(index_max-index_min);
        //  scan_TwoEcho.intensities.resize(index_max-index_min);

        for (int j = index_min; j <= index_max; ++j)
        {
          pcl::PointXYZ point;
          point.x = m_scandata_te[j];
          point.y = 0;
          point.z = 0;
          mp_two_echo_points->push_back(point);
          // mp_two_echo_points->points[j - index_min].x = m_scandata_te[j];
          // mp_two_echo_points->points[j - index_min].y = 0;
          // mp_two_echo_points->points[j - index_min].z = 0;

          // m_two_echo_points[j - index_min].intensity = 0;
          //  scan_TwoEcho.ranges[j - index_min] = scandata_te[j];
          //  scan_TwoEcho.intensities[j - index_min] =0;
        }
        m_two_echo_mutex.unlock();
        m_two_echo_cloud_time = std::chrono::steady_clock::now();
      }
      else
      {
        s_n32ScanIndex = 0;
        //cout << "quan hao6: " << g_u32PreFrameNo << "  danzhen quanhao6: " << l_u32FrameNo<< endl;
        return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
      }
    }
    return Error_manager(Error_code::SUCCESS);
  }
  else
  {
    return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
  }
}

/**
 * 数据包完整性检查 
 **/
Error_manager Wj_716_lidar_protocol::check_xor(char *recvbuf, int recvlen)
{
  int i = 0;
  char check = 0;
  char *p = recvbuf;
  int len;
  if (*p == (char)0x02)
  {
    p = p + 8;
    len = recvlen - 9;
    for (i = 0; i < len; i++)
    {
      check ^= *p++;
    }
    if (check == *p)
    {
      return Error_manager(Error_code::SUCCESS);
    }
    else
      return Error_manager(Error_code::WJ_PROTOCOL_INTEGRITY_ERROR);
  }
  else
  {
    return Error_manager(Error_code::WJ_PROTOCOL_INTEGRITY_ERROR);
  }
}

/**
 * 初始化状态获取
 * */
bool Wj_716_lidar_protocol::get_initialize_status()
{
  return mb_initialized;
}

} // namespace wj_lidar