function[] = BeePathReconstruct(stereocalib_data, left_vid_data, right_vid_data, feeder_position, Rotation, Translation,varargin)

% stereocalib_data should be a structure containing [om, T, fc_left, 
% cc_left,kc_left,alpha_c_left,fc_right, cc_right,kc_right, alpha_c_right], 
% which can be loaded from the stereo calibration results. 
% left/right_vid_data - pos files from the BeeTrackerGUI
% varargin{1,2} - StartPlotting, StopPlotting, in frame numbers. If start point is
% at beginning of sequence, enter 1.
% varargin{3} - colour of plot, useful if several traces will be on the
% same graph.
% Rotation, Translation: Rotation matrix and translation vector between the
% left camera co-ordinate system and the desired co-ordinate system. If the
% transformation is not required, enter empty vectors [].

% Read in the appropriate files
left_info = load(left_vid_data);
right_info = load(right_vid_data);

% Load the relevant data
left_xPos = left_info(:,2);
left_yPos = left_info(:,3);
left_angle = left_info(:,4);
left_angle = rad(left_angle);

right_xPos = right_info(:,2);
right_yPos = right_info(:,3);
right_angle = right_info(:,4);
right_angle = rad(right_angle);

% This bit is for reconstructing the body angle when the cameras are not
% perpendicular.
r_Temp = 20;
leftTail_xPos = r_Temp*cos(left_angle);
leftTail_yPos = r_Temp*sin(left_angle);

rightTail_xPos = r_Temp*cos(right_angle);
rightTail_yPos = r_Temp*sin(right_angle);

% Compute the position of the feeder
fL = feeder_position(1,:);
fR = feeder_position(2,:);

om = stereocalib_data.om;
T = stereocalib_data.T;
fc_left = stereocalib_data.fc_left;
cc_left = stereocalib_data.cc_left;
kc_left = stereocalib_data.kc_left;
alpha_c_left = stereocalib_data.alpha_c_left;
fc_right = stereocalib_data.fc_right;
cc_right = stereocalib_data.cc_right;
kc_right = stereocalib_data.kc_right;
alpha_c_right = stereocalib_data.alpha_c_right;

[FL,FR] = stereo_triangulation(fL',fR',om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right);
FL = FL';

% Use the stereo data to compute the 3D trajectory
xL = [left_xPos left_yPos]';
xR = [right_xPos right_yPos]';

xtL = [leftTail_xPos leftTail_yPos]';
xtR = [rightTail_xPos rightTail_yPos]';

[XL,XR] = stereo_triangulation(xL,xR,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right);

[XTL, XTR] = stereo_triangulation(xtL,xtR,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right);

% Which reference frame do we use? top is more static, but less intuitive.
% Go for left (side) for now, can always change this.

FeederPos = FL';

BeePath = XL';
BeeTailPath = XTL';
PathLength = size(BeePath);

% Remove the extraneous parts of the sequence (held in varargin)
if nargin > 6
    optargin = size(varargin);
    start_point = varargin{1};
    if optargin(2) > 1
        stop_point = varargin{2};
    else
        stop_point = PathLength(1);
    end
    BeePath = BeePath(start_point:stop_point,:);
    BeeTailPath = BeeTailPath(start_point:stop_point,:);
    BeeAngleR = right_angle(start_point:stop_point);
    BeeAngleL = left_angle(start_point:stop_point);
    framelist = start_point:stop_point;
else
    framelist = 1:PathLength(1);
end
PathLength = size(BeePath);
BeeLength = 15;

% Now, reconvert head/tail cartesian co-ordinates to a spherical system
for i=1:PathLength(1);
    PseudLength = norm(BeeTailPath(i,:)-BeePath(i,:));
    PseudZ = BeeTailPath(i,3) - BeePath(i,3);
    PseudX = BeeTailPath(i,1) - BeePath(i,1);
    PseudY = BeeTailPath(i,2) - BeePath(i,2);
    CamPhi(i) = acos(PseudZ/PseudLength);
    CamTheta(i) = atan2(PseudY,PseudX);
    DiffX = BeeLength*sin(CamPhi(i))*cos(CamTheta(i));
    DiffY = BeeLength*sin(CamPhi(i))*sin(CamTheta(i));
    DiffZ = BeeLength*cos(CamTheta(i));
    BeeTailPath(i,:) = BeePath(i,:) + [DiffX DiffY DiffZ];
end

% BeeTailPath should now be an accurate reconstruction of where the end of
% a 20mm bee body would be given the measured angles from both cameras.

% Now, change everything to a global co-ordinate system

if ~isempty(Rotation)
    TFMatrix = [[Rotation Translation]; 0 0 0 1];
    for i=1:Pathlength(1)
        BeePathRect(i,:) = TFMatrix*BeePath(i,:);
        BeeTailRect(i,:) = TFMatrix*BeeTailPath(i,:);
        XTemp = BeeTailRect(i,1) - BeePathRect(i,1);
        YTemp = BeeTailRect(i,2) - BeePathRect(i,2);
        ZTemp = BeeTailRect(i,3) - BeePathRect(i,3);        
        Pitch(i) = acos(ZTemp/BeeLength); % Note that we should really recalculate the r vector but it SHOULD still be the same
        Yaw(i) = atan2(YTemp,XTemp);
    end
    FeederPosRect = TFMatrix*FeederPos;
    
else
    % Left camera frame is the desired global frame as well
    % Do nothing, call Pitch and Yaw the angles from the camera frame.
    Pitch = CamPhi;
    Yaw = CamTheta;
end

Pitch = Pitch';
Yaw = Yaw';


% Save bee path
save BeeHeadPos.dat BeePath

% Save body angles
save BeePitch.dat Pitch
save BeeYaw.dat Yaw

% Save feeder position
save FeederPos.dat FeederPos

% save framelist
save FrameList.dat framelist



% Reconstruct body angle:
% Ignore pitch for the moment - the camera was most certainly not
% perpendicular, and even if it was, the measurement method is skewy.
% Assume the right-hand camera is the top one.

% We calculate the relative x,y co-ordinates of the tail using BeeAngle 
% and the fudged body length:
TailX = BeeLength*cos(Yaw);
TailY = BeeLength*sin(Yaw);

size(TailX)
size(BeePath)
% Put into global co-ordinates:
TailX = TailX + BeePath(:,1);
TailY = TailY + BeePath(:,2);

% Assume bee is horizontal for now:
TailZ = BeePath(:,3);

BeeTail = [TailX, TailY, TailZ];
BeePath(:,3) = -BeePath(:,3);
BeeTail(:,3) = -BeeTail(:,3);

%--------------------------------------------------------------------------
% Smooth data points using gaussian filter
%--------------------------------------------------------------------------
w = gausswin_nob(7, 2); % Use norbert's gaussian window function
BeePath = filtfilt_nob(w, sum(w),BeePath);
BeeTail = filtfilt_nob(w, sum(w),BeeTail);
BeeAngle = filtfilt_nob(w, sum(w),unwrap(Yaw));

% Can also add colour info to varargin, in case we want to plot a start and
% return on the same axes
if nargin > 8
    col = varargin{3};
else col = 'b';
end
mark_info = [col '.'];
line_info = [col '-'];

% Check by plotting:
figure(1);
plot3(BeePath(:,1), BeePath(:,2), BeePath(:,3), mark_info, 'MarkerSize', 14);
hold on
axis equal
% Draw a circle around the feeder position.
for i=1:360
    circleX(i) = FeederPos(1) + 20*sin(i*pi/180);
    circleY(i) = FeederPos(2) + 20*cos(i*pi/180);
    circleZ(i) = FeederPos(3);
end
plot3(circleX, circleY, circleZ, 'r-', 'LineWidth', 2);
xlabel('X Axis');
ylabel('Y Axis');
    n = PathLength;
    for i=1:2:n(1),
        join(1,:)=BeePath(i,:);
        join(2,:)=BeeTail(i,:);
        plot3(join(:,1),join(:,2),join(:,3),line_info, 'LineWidth', 2);
    end    
grid on

BeeAngle = deg(anglepi(BeeAngle));
figure(2)
BeeAngle=flipud(BeeAngle);
plot(BeeAngle, line_info)
%plot(framelist,BeeAngle, line_info)
grid on
xlabel('frame number');
ylabel('Body angle (yaw)');
hold on
