function[Response] = robModelNoVel3D(c,d,L2,Lapse,NP)
%set up parameters for simulation
%% default Parameters
Vdist=1.75; % viewing distance
if nargin<3
    L2=0.9;
    Lapse=0;
    NP=3;
    c=0.0027;
    d=0.0075; %depth Noise scaling
end
lSeed=1; %load seed file (same random variables)

%% parameters of simulation
    Ssav=[];
    msav=[];
    Sample=[];

if lSeed==1
    load('modelSeed','s')
    rng(s)
    clear('s')
    disp('Model seed loaded')
end


sigV=[0.2,0.1567,0.12,0.1,0.08,0.065,0.05,0.035,0.02,0.005 linspace(0.2,0.005,10) linspace(0.2,0.005,10) ]; %velocity sigma
sigX=[repmat(2,1,10), repmat(3,1,10), repmat(4,1,10)];%position Sigma


sigV=visAngle2m(Vdist,sigV); %sigma in metres
sigX=visAngle2m(Vdist,sigX);

T = 5; %length of simualtion /s
hz=60; %simulation rate /hz
runs=1000; % number of monte carlo repeats
time=T*hz; %total number of time steps

a=0.0; % slope of velocity noise (not used currently)
b=0.05; %weber fraction velocity noise

NT=6; %Number of tracked targets
nDim=6; % number of state dimensions

%% Initalize variables
%variables to be stored if parfor is used
probtarget=nan(NT,runs,length(sigV)); %probability of target given sample
answer=nan(length(sigV),runs); % target =1 or non-target =0

parfor p=1:length(sigV) %run for all velocities
    %% generate trajectories
    if lSeed==1
        stream = RandStream.getGlobalStream();
        stream.Substream = p;
        disp('loading parrallel seed')
    end
 
    disp(['running sigV=', num2str(sigV(p)),' sigX=',num2str(sigX(p))])
    
    L=0.9; %Inertia for generating trajectories
    k=((L+1)*sigV(p)^2)/(2*sigX(p)^2);
    sig=0.5*sqrt(sigV(p)^2*(4*sigX(p)^2-4*L^2*sigX(p)^2-sigV(p)^2+L^2*sigV(p)^2)/sigX(p)^2);
    A=([[1-k,0,0, L,0,0] ;[0,1-k, 0,0,L,0];[0,0, 1-k,0,0,L];[-k,0,0 L,0,0];[0,-k, 0,0,L,0];[0,0, -k,0,0,L]]);
    Q=([[sig^2,0 0, sig^2,0,0] ;[0,sig^2,0 0,sig^2,0];[0,0,sig^2 0,0,sig^2]; [sig^2,0 0, sig^2,0,0] ;[0,sig^2,0 0,sig^2,0];[0,0,sig^2 0,0,sig^2]]);
    
    
    prepv=zeros(NP,runs);
    logprop=zeros(NP,runs,time);
    S=zeros(length(A),time,NT,runs);
    m=zeros(length(A),time,NT,runs);
    
    mnoise=randn(nDim,time,NT,runs);
    proNoise=mvnrnd(zeros(nDim,1),Q,time*NT*runs)';
    proNoise=reshape(proNoise,nDim,time,NT,runs);
    S(1:nDim/2,1,:,:,:)=normrnd(0,sigX(p),[nDim/2,1,NT,runs]);
    S((nDim/2)+1:nDim,1,:,:,:)=normrnd(0,sigV(p),[nDim/2,NT,runs]);
    M=zeros(nDim,nDim,time,NT,runs);
    
    
    %generate trajectories
        for r =1:runs
            %can probably calc across targerts
            for j=1:NT
                M(:,:,1,j,r)=covM(S(:,1,j,r),a,b,c,d);
                m(:,1,j,r)=S(:,1,j,r)+mnoise(:,1,j,r).*diag(M(:,:,1,j,r));
                
                for i=1:time-1
                    S(:,i+1,j,r)=A*S(:,i,j,r)+proNoise(:,i,j,r);
                    M(:,:,i+1,j,r)=covM(S(:,i,j,r),a,b,c,d);% generate data according to "true" covariance
                    m(:,i+1,j,r)=S(:,i+1,j,r)+mnoise(:,i+1,j,r).*diag(M(:,:,i+1,j,r));
                    M(:,:,i+1,j,r)=covM(m(:,i,j,r),a,b,c,d);% observer doesent know true measurment covariance use the measurments as base
                end
            end
        end
   
    %% filter parameters
    L=L2;
    k=((L+1)*sigV(p)^2)/(2*sigX(p)^2);
    sig=0.5*sqrt(sigV(p)^2*(4*sigX(p)^2-4*L^2*sigX(p)^2-sigV(p)^2+L^2*sigV(p)^2)/sigX(p)^2);
    A=([[1-k,0,0] ;[0,1-k, 0];[0,0, 1-k]]);
    Q=([[sig^2,0 0] ;[0,sig^2,0];[0,0,sig^2]]);
    
    
    %% intialize variables for filter
    %dimension state,targets,runs,particles
    %define variables needed for kalman filter
    startEST=S(1:3,1,:,:);
    ESTstate=repmat(squeeze(startEST),1,1,1,NP); %set initial velocity state to zero
    ESTstate=reshape(ESTstate,nDim/2,1,NT,runs,NP);
    ESTstate=permute(ESTstate,[1,2,3,5,4]);

    
    %covariance of prediciton
    startCov=M(1:3,1:3,1,:,:);
    %startCov(3:4,3:4,1,:,:)=repmat(0.0001,size(startCov(3:4,1,:,:)));
    
    ESTcov=repmat(squeeze(startCov.^2),1,1,1,1,NP); %eliminate velocity set to small value, 0.0001
    %ESTcov=reshape(ESTcov,nDim,nDim,NT,NP,runs);
    ESTcov=permute(ESTcov,[1,2,3,5,4]);

    allgamma=perms(1:NT);
    gammaLen=length(allgamma); %length of gamma vector
    gammaMap=zeros(gammaLen,NT*NT);
    map=0:NT:NT*(NT-1);
    gammaMap(sub2ind(size(gammaMap),(1:gammaLen)'*ones(1,NT),allgamma+ones(gammaLen,1)*map))=1; %compute sum of log likelihood my mapping it
    gammaMap=sparse(gammaMap);
    LH=nan(NT,NP,runs,NT);
    s=zeros(gammaLen,NP,runs);
    i=nan(NP,runs);
    particleProb=ones(NP,runs)/NP;
    idx=[1 2 3 4 5 6] ;
    
    for k=2:time%from second time step (first used for intialization
        %prediction step for all targets
        ESTref=ESTstate; ESTcovref=ESTcov;
        [StateEstimate,CovEstimate]=kalPredMD(ESTstate,ESTcov,A,Q);
        StateRef=StateEstimate; StatecovRef=CovEstimate;
        
        
        %%
        for j=1:NT %go through each measurment
            
            LH(:,:,:,j)=kalMeasLik(m(1:3,k,idx(j),:),StateEstimate,CovEstimate,M(1:3,1:3,k,idx(j),:).^2,NT,NP,nDim);
            
        end

        for r=1:runs
            for e=1:NP
                lik=squeeze(LH(:,e,r,:));%
                loglik=lik;
                loglik=loglik(:);
                if k==2
                    s(:,e,r)=gammaMap*loglik; %probability for each partile
                else
                    %s(:,e,r)=particleProb(e,r)*exp(gammaMap*loglik); %probability for each partile
                    s(:,e,r)=logprop(e,r,k-1)+gammaMap*loglik; %probability for each partile
                    %s(:,e,r)=gammaMap*loglik; %probability for each partile
                end
            end
            
            if k==2
                i(:,r)= maxInd(s(:,1,r),NP); %if first time 5step only use one particel
            else
                i(:,r)=maxInd(s(:,:,r),NP); %find NP highest probaility assignment from all particles
            end
            
            assign=i;
            for e=1:NP
                prepv(e,r)=div(assign(e,r)-1,gammaLen)+1;
                i(e,r)=rem(i(e,r)-1,gammaLen)+1; % convert data association back.
                %               inverseGamma(:,e,r)=invgamma(allgamma(i(e,r),:)');
                ESTstate(:,:,:,e,r)=ESTref(:,:,:,prepv(e,r),r);
                ESTcov(:,:,:,e,r)=ESTcovref(:,:,:,prepv(e,r),r);
                
                StateEstimate(:,:,:,e,r)=StateRef(:,:,:,prepv(e,r),r);
                CovEstimate(:,:,:,e,r)=StatecovRef(:,:,:,prepv(e,r),r);
                
                %s(i(e,r),e,r)=s(i(e,r),prepv(e,r),r);
                logprop(e,r,k)=s(i(e,r),prepv(e,r),r);
            end
        end
        
       [ESTstate, ESTcov]= kalMeasUpdate(m(1:3,k,:,:),M(1:3,1:3,k,:,:).^2,ESTstate,ESTcov,StateEstimate,CovEstimate,i,NT,NP,nDim,runs,allgamma,idx);
          
    end
    for r=1:runs
        for e=1:NP
            %particleProb(e,r) = particleProb(e,r)* exp(s(i(e,r),e,r));
            %s(i(e,r),e,r)
           %particleProb(e,r) = exp(s(i(e,r),e,r)-s(i(1,r),1,r)); %particle prob slightly different to robs maybe due to likelihood computation
           particleProb(e,r) = exp(logprop(e,r,end)-logprop(1,r,end)); %correct one
        end
        particleProb(:,r)=particleProb(:,r)./sum(particleProb(:,r));
    end
    %% probability of sample given final estimate
    %load in sample or generate new ones

    
        testsample=squeeze(S(:,end,1,:))+mvnrnd(zeros(nDim,1),squeeze(M(:,:,end,1,:)).^2)';
    
    postobs=zeros(NT,runs);
    for r=1:runs
        for u=1:NT
            for e=1:NP
                mu=ESTstate(:,:,u,e,r);
                tmp=covM(testsample(:,r),a,b,c,d).^2;
                cm=ESTcov(:,:,u,e,r)+tmp(1:3,1:3);
                postobs(u,r)=postobs(u,r)+particleProb(e,r)*mvnpdf(testsample(1:3,r),mu,cm);
            end
        end
        postobs(:,r)=postobs(:,r)./sum(postobs(:,r));
    end
    
    probtarget(:,:,p)=postobs;
    
    
end

%% work out model respone using likelihood of measurment

for z = 1:length(sigV)
    for j= 1:runs
        if (rand<=Lapse)==1%trial is a lapse
            answer(z,j)= round(rand); %randomly respond
        else
            answer(z,j)=sum(probtarget(1:3,j,z))>sum(probtarget(4:end,j,z)); %find whether measurment came from target not
            answerRob(z,j)=nansum(probtarget(1:3,j,z));
        end
    end
end

sigX=m2visAngle(Vdist, sigX);
Response=[flipud(mean(answer(sigX==2,:)'));flipud(mean(answer(sigX==3,:)'));flipud(mean(answer(sigX==4,:)'))];
ResponseRob=[flipud(mean(answerRob(sigX==2,:)'));flipud(mean(answerRob(sigX==3,:)'));flipud(mean(answerRob(sigX==4,:)'))];

