%step 1 get position and heading estimates and write to 60 row csv
close all
clear all
%proj39=proj39collection;
%distance([1,1;2,2],[1,0;0,0])
%userpath('C:\Users\wmou\SynologyDrive\zhichun\CircStat2012a\');
newpath='\\129.128.52.128\labmeeting\labmeeting\circular statistics\circular statistic for matlab\CircStat2012a'
userpath(newpath);
            
VectorCal=collectiveVectorCal;

buildingCoorinateFile='Coordinates of 12 environments and 5 targets.xlsx';
buildingCoorinateSheet='sheet1';

% get buildingCoorindate

fTablebuildingCoorinate= readtable(buildingCoorinateFile,'Sheet',buildingCoorinateSheet);
%let's use administration(outside) as 0,0, and calculate the other
%locations
% dis=1
% bearing=0
%[x,y]=getxyfromdisandbearing(dis,bearing)

%d=2R × arcsin(sqrt([sin((θ₂ - θ₁)/2)^2 + cosθ₁ × cosθ₂ × sin((φ₂ - φ₁)/2)^2]))
% A=[27.9881, 86.9250];
% B=[40.7484, -73.9857];
% AxBy=[A(1),B(2)];
% d=earthD(A,B);
% 
% dy=earthD(A,AxBy);
% [x,y,d1]=XYfromLatLong(A,B);

%we should use the most familiar outside location for origin
%index=(strcmp(fTablebuildingCoorinate.location,'CCIS(outside)'))
index=(strcmp(fTablebuildingCoorinate.location,'Cameron Library(outside)'))
%index=(strcmp(fTablebuildingCoorinate.location,'Administration(outside)'));
A=[fTablebuildingCoorinate(index,:).Latitude, fTablebuildingCoorinate(index,:).Longitude];

fTablebuildingCoorinate.X=fTablebuildingCoorinate.Latitude*0;
fTablebuildingCoorinate.Y=fTablebuildingCoorinate.Latitude*0;
fTablebuildingCoorinate.d=fTablebuildingCoorinate.Latitude*0;
fTablebuildingCoorinate.bearing=fTablebuildingCoorinate.Latitude*0;
fTablebuildingCoorinate.dxy=fTablebuildingCoorinate.Latitude*0;
for i=1:size(fTablebuildingCoorinate,1)
    B=[fTablebuildingCoorinate(i,:).Latitude, fTablebuildingCoorinate(i,:).Longitude];
    [x,y,d,theta,d1]=XYfromLatLong(A,B)
    fTablebuildingCoorinate(i,:).X=x;
    fTablebuildingCoorinate(i,:).Y=y;
    fTablebuildingCoorinate(i,:).d=d1;
    fTablebuildingCoorinate(i,:).bearing=theta;
    fTablebuildingCoorinate(i,:).dxy=d1;
end 

writetable(fTablebuildingCoorinate,'buildingCoorinateCameronoutside.csv')
% Latitude
% Longitude

%[x,y]=XYfromLatLong(A,B);

%%
%
% calculate position error and heading error
% fileName='64 rows (with continuous familarity).xlsx';
% %fileName='PIobjects20202021.xlsx';
% sheetName='merged_data';
% fTable= readtable(fileName,'Sheet',sheetName);
fileName='merged_data.txt';
fTable=readtable(fileName);
%fTable= convertvars(fTable,{'indoor_outdoor'},'categorical');
index=(fTable.participant_number~=0) %& fTable.participant_number~=33);
fTable=fTable(index,:);

Uni=unique(fTable.envrionment_building);
% AyCmean=[];
rhoAll=[];
pvalAll=[];
md4All={};
Enviornmentname= cell(size(fTable,1),1);
indoor_outdoorValue=cell(size(fTable,1),1);
groupenvrionmentFamilarity= zeros(size(fTable,1),1);

tb1=table();
tb1.PTT=fTable.PTT;
tb1.MRT=fTable.MRT;
tb1.abs_pointerror=fTable.abs_pointerror;

% mdregression = fitlm(tb1,'ResponseVar','abs_pointerror');
% 
% mdLME=fitglme(fTable, 'abs_pointerror~1+PTT+MRT+(1|participant_number)');
for i=1:numel(Uni)
    %get the enviornmentname
     nameinsideindoor_outdoor=strsplit(Uni{i},'(');
     name=nameinsideindoor_outdoor{1};
     Value=strsplit(nameinsideindoor_outdoor{2},')');
    Value=Value{1};
    
    %get the index of trials stanidng at the environment
     index=find(strcmp(fTable.envrionment_building,Uni(i)));
    [Enviornmentname{index}]=deal(name);
    [indoor_outdoorValue{index}]=deal(Value);

    % [groupenvrionmentFamilarity(index)]=deal(mean(fTable.envrionmentFamilarity(index)));
    
     
end

%fTable=[fTable table(Enviornmentname,groupenvrionmentFamilarity)];
fTable=[fTable table(Enviornmentname,indoor_outdoorValue)];


%Uni=unique([fTable.envrionment_building,fTable.participant_number]);
[C,ia,ic] = unique(table(fTable.envrionment_building,fTable.participant_number,fTable.facing_direction));

fTable.positionX= ones(size(fTable,1),1)*1000;
fTable.positionY= ones(size(fTable,1),1)*1000;
fTable.TargetX=ones(size(fTable,1),1)*1000;
fTable.TargetY=ones(size(fTable,1),1)*1000;

fTable.headingprime=ones(size(fTable,1),1)*1000;
fTable.positionprimeX= ones(size(fTable,1),1)*1000;
fTable.positionprimeY= ones(size(fTable,1),1)*1000;



%fTable.heading=ones(size(fTable,1),1)*1000;
fTable.headingError= ones(size(fTable,1),1)*1000;
fTable.absheadingError= ones(size(fTable,1),1)*1000;

fTable.distanceError= ones(size(fTable,1),1)*1000;
fTable.positionAngularError=ones(size(fTable,1),1)*1000;
fTable.abspositionAngularError=ones(size(fTable,1),1)*1000;

fTable.leastErrSquared= ones(size(fTable,1),1)*1000;
fTable.bearing2estimatedPositionHeading=ones(size(fTable,1),1)*1000;
fTable.PointingErrorRemovingPositionHeadingErrors=ones(size(fTable,1),1)*1000;
fTable.absPointingErrorRemovingPositionHeadingErrors=ones(size(fTable,1),1)*1000;





iterationTimes=1;
method= '1-cossimilarity';%'leastsquarePointingError', '1-cossimilarity', 'meanabsolutePointingError'
options = optimset('MaxFunEvals',100000);
for i=1:length(ia) %((ia) stores unique categoray)
    index=ic==i;%get category i
    fTb=fTable(index,:);
    index1=strcmp(fTablebuildingCoorinate.location,fTb.envrionment_building{1,:});
    fTable.positionX(index)= fTablebuildingCoorinate.X(index1);
    fTable.positionY(index)= fTablebuildingCoorinate.Y(index1);

    testXYHeading =[fTablebuildingCoorinate.X(index1),fTablebuildingCoorinate.Y(index1)]; %used for initial values for search
    testXYHeading=[testXYHeading,fTb.facing_direction(1,:)];

    %fTbPointingAngleTargetName=table(fTb.target_building,fTb.correct_angle,'VariableNames', {'target_building', 'pointing_angle'});
    %fTbPointingAngleTargetName=table(fTb.target_building,fTb.bearing2estimatedPositionHeading,'VariableNames', {'target_building', 'pointing_angle'});

    fTbPointingAngleTargetName=table(fTb.target_building,fTb.pointing_angle,'VariableNames', {'target_building', 'pointing_angle'});

    fTablebuildingCoorinate1=table(fTablebuildingCoorinate.location,fTablebuildingCoorinate.X,fTablebuildingCoorinate.Y,'VariableNames', {'target_building', 'x','y'});
    fTbPointingAngleTargetXY=join(fTbPointingAngleTargetName,fTablebuildingCoorinate1);

    %we should add random initial testXYHeading, try 100 times first
    boundaryscale=1.5;
    buildingRange=16;
    xmin=min(fTablebuildingCoorinate1(1:buildingRange,:).x);
    xmax=max(fTablebuildingCoorinate1(1:buildingRange,:).x);
    ymin=min(fTablebuildingCoorinate1(1:buildingRange,:).y);
    ymax=max(fTablebuildingCoorinate1(1:buildingRange,:).y);
    rx = xmin + (xmax-xmin)*rand( iterationTimes,1);
    ry = ymin + (ymax-ymin)*rand( iterationTimes,1);
    rh = 360 *rand( iterationTimes,1);
    testXYHeadingAll=[rx,ry,rh];
    testXYHeadingAll(1,:)=testXYHeading;
    fvalmin=10^10;
    %xmin=[];
    boundary=[xmin,xmax,ymin,ymax]*boundaryscale;
    for i=1:iterationTimes
        testXYHeading= testXYHeadingAll(i,:);
        [x,fval] = fminsearch (@(x)HeadingPosition2Dpointing20231126 (x,fTbPointingAngleTargetXY.pointing_angle,[fTbPointingAngleTargetXY.x,fTbPointingAngleTargetXY.y],method,boundary), testXYHeading,options );%'leastsquarePointingError', '1-cossimilarity', 'meanabsolutePointingError'
        if fval<fvalmin
            xmin=x;
            fvalmin=fval;
        end 
    end


    fval=fvalmin;
    x=xmin;

    headingprime = x(3);

    convertangleto360(headingprime);
    % if headingprime<0
    %     headingprime=360+headingprime;
    % end 
    positionprimeX= x(1);
    positionprimeY= x(2);

    leastErrSquared=fval;%1- average of cos similiarity or leastquare error, or mean abs pointing error   
    fTable.headingprime(index)=headingprime ;
    fTable.positionprimeX(index)=positionprimeX ;
    fTable.positionprimeY(index)=positionprimeY ;
    fTable.leastErrSquared(index)=leastErrSquared ;
    fTable.TargetX(index)= fTbPointingAngleTargetXY.x;
    fTable.TargetY(index)= fTbPointingAngleTargetXY.y;

end

% writetable(fTable,fileName,'Sheet','T60');
writetable(fTable,'T6020231201CosSimilarity500.csv');


function d=earthD(A,B) %A,B in degree
    %https://www.omnicalculator.com/other/latitude-longitude-distance
    % θ₁, φ₁ – First point latitude and longitude coordinates;
    % θ₂, φ₂ – Second point latitude and longitude coordinates;
    % R – Earth's radius (R = 6371 km); and
    % d – Distance between them along Earth's surface.
    %d = 2R × sin⁻¹(√[sin²((θ₂ - θ₁)/2) + cosθ₁ × cosθ₂ × sin²((φ₂ - φ₁)/2)])
    R = 6371000 %m
    A=A*pi()/180;
    B=B*pi()/180;
    theta1=A(:,1);  phi1=A(:,2);
    theta2=B(:,1);  phi2=B(:,2);
   
    d= 2*R*asin( sqrt( (sin((theta2-theta1)/2))^2 + cos(theta1)*cos(theta2)*(sin((phi2-phi1)/2))^2 ) );
end


function theta = earthbearing(A,B)
      
        lat1=A(:,1);
        long1=A(:,2);
        lat2=B(:,1);
        long2=B(:,2);

        %   % MATLAB
        long1 = long1 * (pi/180);
        long2 = long2 * (pi/180);
        lat1 = lat1 * (pi/180);
        lat2 = lat2 * (pi/180);
        
        theta = atan2(sin(long2 - long1) * cos(lat2), cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(long2 - long1));
        theta = theta * 180 / pi;
        
        if theta < 0
            theta = theta + 360;
        end

end

function [x,y,d,theta,d1]=XYfromLatLong(referenceLocation,B)
    A=referenceLocation;
    theta = earthbearing(A,B);
    d=earthD(A,B);
    [x,y]=getxyfromdisandbearing(d,theta);
    d1=sqrt(x^2+y^2);    
    %get x
    

    % A=referenceLocation;
    % BxAy=[B(:,1),A(:,2)];
    % AxBy=[A(:,1),B(:,2)];
    % x=earthD(A,BxAy);
    % y=earthD(A,AxBy);
    % d=sqrt(x^2+y^2)


    %get y

end
function [x,y]=getxyfromdisandbearing(dis,bearing)%bearing in degree
    %bearing in terms of z positive or north
    x=dis*sind(bearing);
    y=dis*cosd(bearing);
end

function T=pilotTable(fTable, unicateM, varNames)
    tbCate=table();
    for i=1:size(unicateM,2)
        tbCate.(unicateM{i})=fTable.(unicateM{i});
    end

    % tbCate.indoor_outdoor=fTable.indoor_outdoor;
    % %tbCate.Enviornmentname=fTable.Enviornmentname;
    % tbCate.participant_number=fTable.participant_number;
    A=[];
    for i=1:size(varNames,2)
        A=[A,fTable.(varNames{i})];
    end
    [meanM,unicateM]=getMeanORP1(A,tbCate);

    
    %[meanM,unicateM]=getMeanORP1([fTable.abs_pointerror,fTable.PTT,fTable.MRT,fTable.continuous_unfam_fam,fTable.euclidian_distance,fTable.campus_experience,fTable.absheadingError,fTable.distanceError,fTable.leastErrSquared],tbCate);
    %[meanM,unicateM]=getMeanORP([fTable.abs_pointerror,fTable.PTT,fTable.MRT,fTable.euclidian_distance,fTable.campus_experience,fTable.envrionmentFamilarity],[fTable.indoor_outdoor,fTable.groupenvrionmentFamilarity,fTable.participant_number]);
    %A=[meanM,unicateM];
    
    T = array2table(meanM,...
        'VariableNames',varNames);
    
    T=[unicateM,T];
end 