/*
 * ClippedFrame.cpp
 *
 *  Created on: Jul 27, 2014
 *      Author: jared
 */

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

#include <cmath>

ClippedFrame::ClippedFrame(int x, int y, int width, int height, int fps) {
	_x = x;
	_y = y;
	_width = width;
	_height= height;
	_fps = fps;

	_isMoving = false;
	_motionFrameStart = 0;
	_motionFrameMid = 0;
	_motionFrameEnd = 0;
	_motionFrameCurrent = 0;
	_motionXStart = 0;
	_motionYStart = 0;
	_motionXEnd = 0;
	_motionYEnd = 0;
	_motionAccelerationPercent = 0.4; //accelerate this fraction of the time
}

ClippedFrame::~ClippedFrame() {
}

double ClippedFrame::getTheta(int x1, int y1, int x2, int y2) {
	double theta;
	if (x2==x1 && y2==y1) return 0;
	if (x2 == x1) {
		if (y2 > y1) theta = 3.1415926536*.5;
		else theta = 3.1415926536*1.5;
	}
	else theta = atan2(y2-y1,x2-x1);
	return theta;
}

cv::Mat ClippedFrame::getAsMat(cv::Mat canvas) {
	int canvasWidth = canvas.cols;
	int canvasHeight = canvas.rows;
	int x = _x;
	int y = _y;
	snapToWithinBounds(x, y, canvasWidth, canvasHeight);

	//For some reason we need to create a copy of canvas and crop that before writing to video...
	cv::Mat croppedRef(canvas, cv::Rect(x-0.5*_width, y-0.5*_height, _width, _height));
	cv::Mat cropped;
	croppedRef.copyTo(cropped);

	return cropped;
}

double ClippedFrame::getDistance(double x1, double y1, double x2, double y2) {
    return sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1));
}

//Move from current _x, _y co-ordinates to x, y in dt seconds
void ClippedFrame::moveTo(int x, int y, double dt) {

	_isMoving = true;
	_motionXStart = _x;
	_motionYStart = _y;
	_motionXEnd = x;
	_motionYEnd = y;
	_motionFrameStart = 0;
	_motionFrameEnd = _motionFrameStart + dt*_fps;
	_motionFrameMid = (1-_motionAccelerationPercent)*_motionFrameStart + _motionAccelerationPercent*_motionFrameEnd;
	_motionFrameCurrent = 0;
}

//Update the position of the frame
void ClippedFrame::update() {
	if (_isMoving) {
		_motionFrameCurrent++;
		if (_motionFrameCurrent > _motionFrameEnd) {
			_motionFrameCurrent = 0;
			_x = _motionXEnd; _y = _motionYEnd;
			_isMoving = false;
			std::cout << "Motion has stopped" << std::endl;
		} else {
			double dt = (double)(_motionFrameCurrent - _motionFrameStart)/((double)_fps);
			double p = _motionAccelerationPercent;
			double dS = getDistance(_motionXStart,_motionYStart,_motionXEnd,_motionYEnd);
			double dT = (_motionFrameEnd - _motionFrameStart)/((double)_fps);

			double ds;

			if (_motionFrameCurrent < _motionFrameMid) {
				ds = dS/(p*dT*dT)*dt*dt;
			}else{
				double x_1 = p*dS;
				ds = x_1 + 2*dS/dT*(dt - p*dT) - dS/((1-p)*dT*dT)*(dt - p*dT)*(dt - p*dT);
			}
			//ds = v*(dt/dT)*dS; // Simple linear velocity

			//Now decompose motion into x and y axis
			double theta = getTheta(_motionXStart, _motionYStart, _motionXEnd, _motionYEnd);
			_x = _motionXStart + ds*cos(theta);
			_y = _motionYStart + ds*sin(theta);
		}
	}
}

//sets this frame as close to x,y as possible without moving out of the canvasWidth x canvasHeight bounding box
void ClippedFrame::setPosition(int x, int y, int canvasWidth, int canvasHeight) {
	snapToWithinBounds(x, y, canvasWidth, canvasHeight);
	_x = x;
	_y = y;
	_isMoving = false;
}

//Takes in an x and a y and mutates them into values such that a clipped frame
//of width canvasWidth and height canvasHeight can be centered at the new x,y.
void ClippedFrame::snapToWithinBounds(int& x, int& y, int canvasWidth, int canvasHeight) {
	x = fmin( fmax(x, 0.5*((float)_width)),
			  canvasWidth-0.5*((float)_width));
	y = fmin( fmax(y, 0.5*((float)_height)),
			  canvasHeight-0.5*((float)_height));
	//*Alternate Code:*//
	//	if (x < 0.5*((float)_width))
	//		x = 0.5*((float)_width);
	//	if (x >= canvasWidth-0.5*((float)_width))
	//		x = canvasWidth-0.5*((float)_width) + 1;
	//	if (y < 0.5*((float)_height))
	//		y = 0.5*((float)_height);
	//	if (y >= canvasHeight-0.5*((float)_height))
	//		y = canvasHeight-0.5*((float)_height) + 1;
}

void ClippedFrame::drawTo(cv::Mat canvas, cv::Scalar colour) {
	int x = _x;
	int y = _y;
	snapToWithinBounds(x, y, canvas.cols, canvas.rows);
	cv::rectangle(canvas, cv::Point(x-0.5*_width, y-0.5*_height), cv::Point(x+0.5*_width-1, y+0.5*_height-1), colour);
}
