/*
 * eobject.cpp
 *
 *  Created on: Jul 5, 2014
 *      Author: Daniel Wesierski
 */


#include "helpers.h"
#include "eobject.h"

void EObject::init_chain(std::vector<std::vector<unsigned int> > partsIdx) {
	partSegments = partsIdx;
}

void EObject::init_CS() {
	// get orientations for each segment
	get_orientation();
}

void EObject::get_orientation()
{
	segCS.resize(parts.size()-1);

	for (unsigned int i = 0; i != partSegments.size(); ++i) {
		cv::Point2d orient = parts[partSegments[i].back()].center - parts[partSegments[i].front()].center;
		double length = norm(orient);
		orient.x /= length;
		orient.y /= length;
			
		for (unsigned int j = 0; j != partSegments[i].size() - 1; ++j) {
			segCS[partSegments[i][j]].x = orient.x;
			segCS[partSegments[i][j]].y = orient.y;
		}
	}
}

void EObject::get_orientation_SVD()
{
	for (unsigned int i = 0; i != parts.size() - 1; ++i)
		segCS.push_back(cv::Point2d(0, 1.0));  // assuming eobjects are initially oriented vertically

	for (unsigned int i = 0; i != partSegments.size(); ++i) {

		cv::Mat vectors(2, partSegments[i].size(), CV_64F);

		for (unsigned int j = 0; j != partSegments[i].size(); ++j)
			cv::Mat(parts[partSegments[i][j]].center).copyTo(vectors.col(j));

		cv::Mat covar, mean;
		cv::calcCovarMatrix(vectors, covar, mean, CV_COVAR_COLS + CV_COVAR_NORMAL);
		covar = (1.0 / (vectors.cols - 1))*covar;

		cv::SVD axes(covar);

		// reverse vector to be closer (in angular sense) to previous vector [0;-1]
		int sign = sgn(segCS[partSegments[i][0]].ddot(cv::Point2d(axes.u.col(0))));
		for (unsigned int j = 0; j != partSegments[i].size() - 1; ++j) {
			segCS[partSegments[i][j]].x = sign*axes.u.at<double>(0, 0);
			segCS[partSegments[i][j]].y = sign*axes.u.at<double>(1, 0);
		}
	}
}

void EObject::init_pose_params_1D() {
	// get spatial parameters for neighbor parts
	// get angular offsets between 'neighbor parts' and 'global orientation of the corresponding segment'
	for (unsigned int i=0; i != parts.size()-1; ++i) {
		cv::Point2d localLink = parts[i+1].center - parts[i].center;		
		double length = cv::norm(localLink); // initialized by initial distance between two parts		
		kinMean.push_back(length);

		double stdDev = 0.5*(0.5*parts[i+1].bbox.height + 0.5*parts[i].bbox.height);
		kinStdDev.push_back(stdDev);
		
		// 1D object
		kinAngOff.push_back(0);
	}
}

void EObject::init_pose_params_2D() {
	// get spatial parameters for neighbor parts
	// get angular offsets between 'neighbor parts' and 'global orientation of the corresponding segment'
	for (unsigned int i = 0; i != parts.size() - 1; ++i) {
		cv::Point2d localLink = parts[i + 1].center - parts[i].center;		
		double length = cv::norm(localLink); // initialized by initial distance between two parts		
		kinMean.push_back(length);

		double stdDev = 0.5*(0.5*parts[i + 1].bbox.height + 0.5*parts[i].bbox.height); 
		kinStdDev.push_back(stdDev);

		// normalize
		localLink.x /= length;
		localLink.y /= length;

		double angularOffset = std::min(1.0, localLink.ddot(segCS[i]));

		//angular_offset = CV_PI/2 - acos(angularOffset);
		angularOffset = std::acos(angularOffset);

		if (segCS[i].cross(localLink) < 0)
			angularOffset = 2 * CV_PI - angularOffset;
	
		// 2D object
		kinAngOff.push_back(angularOffset);
	}
}


cv::Point2d EObject::get_center()
{
	cv::Point2d center(0,0);
	for (unsigned int i=0; i != parts.size(); ++i)
		center += parts[i].center;

	center.x /= parts.size();
	center.y /= parts.size();

	return center;
}

void EObject::draw(cv::Mat image, int opt, bool withCS, cv::Point ulCorner, int whatColor, int lineWidth, int lineType)
{
	int whatColorLink = 0;

	for (unsigned int i=0; i != partSegments.size(); ++i) {
		whatColor = i;
		for (unsigned int j=0; j != partSegments[i].size(); ++j) {
			unsigned int k = partSegments[i][j];
			if (opt == 0) { // draw centers
				cv::circle(image, parts[k].center, lineWidth , color(whatColor), 1.5*lineWidth, lineType);
			} else if (opt == 1) { // draw windows
				parts[k].draw(image, color(whatColor), lineWidth);
			} else if (opt == 2) { // draw windows or centers
				cv::circle(image, parts[k].center, lineWidth , color(whatColor), 1.5*lineWidth, lineType);

				if (j<partSegments[i].size()-1) {
					unsigned int l = partSegments[i][j+1];
					cv::line(image, parts[k].center, parts[l].center, color(whatColorLink), lineWidth, lineType);
				}
			}
		}
	}

	if(withCS) {
		int r = image.rows / 20;
		int num = image.rows / (3 * r);
		for (unsigned int i = 0; i != partSegments.size(); ++i) {			
			int xoff = (3 * r)*int(i / num);
			int yoff = (3 * r)*(i % num);
			cv::Point  ulCorner2 = ulCorner + cv::Point(xoff,yoff);
			draw_seg_CS(image, ulCorner2, i, r, color(i));			
			show_img(image, true);
		}
	}
}

// computes ROI patch of elongated object
cv::Mat EObject::get_patch(const cv::Mat& image,
						   const cv::Rect roi)
{
	cv::Point center = get_center();
	cv::Rect roiShifted(std::max(0,center.x-roi.width/2-std::max(0,center.x+roi.width/2-image.cols)),
			            std::max(0,center.y-roi.height/2-std::max(0,center.y+roi.height/2-image.rows)),
			            roi.width, roi.height);
	return image(roiShifted);
}

void EObject::draw_seg_CS(cv::Mat& image, cv::Point ulCorner, unsigned int segIdx, int radius, cv::Scalar colorOrient)
{
	double shading = 0.6;

	cv::Rect roi(ulCorner.x,ulCorner.y,3*radius,3*radius);
	cv::Mat imageCS = image(roi);

	cv::Point2d center(imageCS.cols/2, imageCS.rows/2);
	cv::Point2d rotated(segCS[partSegments[segIdx][0]].y, -segCS[partSegments[segIdx][0]].x);

	cv::Point2d p1 = center + radius*segCS[partSegments[segIdx][0]];
	cv::Point2d p2 = center + radius*rotated;

	cv::line(imageCS, center, center+cv::Point2d(radius*10,0), shading*color(0), 2); // x-axis
	cv::line(imageCS, center, center+cv::Point2d(-radius*10,0), shading*color(0), 2); // x-axis

	cv::line(imageCS, center, center+cv::Point2d(0,radius*10), shading*color(0), 2); // y-axis
	cv::line(imageCS, center, center+cv::Point2d(0,-radius*10), shading*color(0), 2); // y-axis

	cv::circle(imageCS, center, radius, color(0), 1, CV_AA);

	cv::circle(imageCS, p1, 4, colorOrient, 3, CV_AA);  // mark seg's orientation
	cv::line(imageCS, center, p1, colorOrient, 2, CV_AA); // seg orientation

	cv::line(imageCS, center, p2, color(5), 2, CV_AA);

	imageCS = shading*imageCS;
	imageCS.copyTo(image(roi));
}

