#include "API_VideoStitch.h"
#include "Arith_VideoStitch.h"
#include "Arith_Utils.h"
#include "Arith_CoordModule.h"
#include <opencv2/opencv.hpp>
#include <omp.h>

using namespace std;
using namespace cv;

API_VideoStitch * API_VideoStitch::Create(SINT32 nWidth, SINT32 nHeight)
{
    return new VideoStitch(nWidth,nHeight);
}


void API_VideoStitch::Destroy(API_VideoStitch * obj)
{
    delete obj;
}


VideoStitch::VideoStitch(SINT32 nWidth, SINT32 nHeight)
{
	m_GeoStitcher = new ExtrinsicStitcher();
	memset(&m_pan, 0, sizeof(GD_VIDEO_FRAME_S));
}

VideoStitch::~VideoStitch()
{
	delete m_GeoStitcher;
}

#include "ceres/ceres.h"
using namespace ceres;
struct CostFunctor {
  template <typename T>
  bool operator()(const T* const x, T* residual) const {
    residual[0] = 10.0 - x[0];
    return true;
  }
};

void VideoStitch::Test()
{
  google::InitGoogleLogging("ceres");

  // The variable to solve for with its initial value. It will be
  // mutated in place by the solver.
  double x = 0.5;
  const double initial_x = x;

  // Build the problem.
  ceres::Problem problem;

  // Set up the only cost function (also known as residual). This uses
  // auto-differentiation to obtain the derivative (jacobian).
  ceres::CostFunction* cost_function =
      new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
  problem.AddResidualBlock(cost_function, nullptr, &x);

  // Run the solver!
  ceres::Solver::Options options;
  options.minimizer_progress_to_stdout = true;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  std::cout << summary.BriefReport() << "\n";
  std::cout << "x : " << initial_x << " -> " << x << "\n";
}

PanInfo VideoStitch::Init(FrameInfo info)
{
	m_panPara = m_GeoStitcher->InitMap(info);

	m_pan = cv::Mat::zeros(m_panPara.m_pan_height, m_panPara.m_pan_width, CV_8UC1);

	return m_panPara;
}

BYTE8 VideoStitch::Updata(GD_VIDEO_FRAME_S img, FrameInfo para)
{
	Proj t_Proj = m_GeoStitcher->AnlayseTform(para);

	// 计算帧的map四至
	cv::Point2f leftTop_map = m_GeoStitcher->back_project(cv::Point2f(0,0), t_Proj, m_panPara);
	cv::Point2f rightTop_map = m_GeoStitcher->back_project(cv::Point2f(img.u32Width,0), t_Proj, m_panPara);
	cv::Point2f rightBottom_map = m_GeoStitcher->back_project(cv::Point2f(img.u32Width,img.u32Height), t_Proj, m_panPara);
	cv::Point2f leftBottom_map = m_GeoStitcher->back_project(cv::Point2f(0,img.u32Height), t_Proj, m_panPara);


	// 计算全景图的范围
	int right = max(max(max(leftTop_map.x, leftBottom_map.x), rightTop_map.x), rightBottom_map.x);
	int left = min(min(min(leftTop_map.x, leftBottom_map.x), rightTop_map.x), rightBottom_map.x);
	int top = min(min(min(leftTop_map.y, leftBottom_map.y), rightTop_map.y), rightBottom_map.y);
	int bottom = max(max(max(leftTop_map.y, leftBottom_map.y), rightTop_map.y), rightBottom_map.y);


	int xRange = right - left;
	int yRnage = bottom - top;

	//反映射到像素坐标
	int valid_top = std::max(0, top);
	int valid_bottom = std::min(m_pan.cols, bottom);
	int valid_left = std::max(0, left);
	int valid_right = std::min(m_pan.rows, right);

#pragma omp parallel for
	for (int i = valid_top; i < valid_bottom; i++)
	{
		for (int j = valid_left; j < valid_right; j++)
		{
			//转换为pixel坐标
			cv::Point2f p_img = m_GeoStitcher->project(Point2f(j, i), t_Proj, m_panPara);

			if (p_img.x >= 0 && p_img.y >= 0 && p_img.x < img.u32Width && p_img.y < img.u32Height)
			{
				m_pan.data[i * m_pan.rows + j] =
					FourPointInterpolation(img.u64VirAddr[0], img.u32Width, img.u32Height, p_img.x, p_img.y);
			}
		}
	}


    return 0;
}

GD_VIDEO_FRAME_S VideoStitch::ExportPanAddr()
{
	GD_VIDEO_FRAME_S pan_out;

	pan_out.enPixelFormat = GD_PIXEL_FORMAT_GRAY_Y8;
	pan_out.u32Width = m_panPara.m_pan_width;
	pan_out.u32Height = m_panPara.m_pan_height;
	pan_out.u64VirAddr[0] = m_pan.data;

	return pan_out;
}