function [ Leg, PBase, ErrorMsg ] = inverse_kinematic(  Base, Platform, ConMatrix, LegLimits, JointLimits  )
%inverse kinematics of Stewart platform with Connection Matrix
%output:    Leg.Length :Leg length of each leg
%           PBase : Platform connection points coordinates in terms of base
%                   coordinate frame
%           Error Message
%
%
%input:     Platform Connection points coordinate in terms of platform coordinate frame
%           ConMatrix: Connection Matrix
%           LegLimits: minumum and maximum leg limits of each leg
%           JointLimits: minimum and maximum joint angle values of each leg
%           
%
%

%initialize leg length matrix 
    PBase = ones(6,3) * NaN;
    Leg.L = ones(6,1) * NaN;
    Leg.unit_Lvector = ones(6,3) * NaN;
     Leg.L_vector = ones(6,3) * NaN;
     
% Platform.Translate
%%Platform.Rotation = Rotation( Platform.alpha, Platform.beta, Platform.gamma , 'RPY' ) ;
    
%We should do these in order to be sure that Platform connection points are
%given in terms of base connection 
for index = 1:1:6
        %platform connection points are given in terms of platform
        %coordinate frame as PTable 
        
        %PBase is the platform connection points in terms of platform
        %coordinate frame 
        
        PBase(index,:) = ...
            Platform.Rotation * Platform.PTable(index,:)' + Platform.Translate;
end

ErrorMsg = NaN;

yapi_sorunlumu = false;
yapi_sorunlari = ones(6,1)*NaN;
Leg.L = ones(6,1);

%for each leg do the inverse kinematic 
for index = 1:1:6
    %ConMatrix first column is the number of BASE connection point
    %ConMatrix second column is the number of PLATFORM connection point
    if ~isnan(ConMatrix(1,index)) && ~isnan(ConMatrix(2,index))
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
        %inverse kinematics = leg lengths is the norm of leg vector
        %Leg Vector is platform connection point - base connection point       
        Leg.L_vector(index,:) = PBase(ConMatrix(2,index),:) - Base.BTable(ConMatrix(1,index),:);
        Leg.L(index) = norm ( PBase(ConMatrix(2,index),:) - Base.BTable(ConMatrix(1,index),:) );
        Leg.unit_Lvector(index,:) =  Leg.L_vector(index,:)/ Leg.L(index);
        ErrorMsg = 0 ;
 
        if Leg.L(index) >= LegLimits(index,2)
            yapi_sorunlumu = true;
            yapi_sorunlari(index) = index;    
            ErrorMsg = -2;
%              ErrorMsg = [ 'Leg ' num2str(index) ' has a length ' num2str(Leg.L(index)) ' smaller than the mininum length limit' ]
        elseif Leg.L(index)<= LegLimits(index,1)
            yapi_sorunlumu = true;
            yapi_sorunlari(index) = index;
            ErrorMsg = -1;
%              ErrorMsg = [ 'Leg ' num2str(index) ' has a length ' num2str(Leg.L(index)) ' bigger than the maximum length limit' ]        
        end
    end
end

%return Platform.PBase;
% if ErrorMsg == 0 
%     'No ERROR' ; 
% elseif ErrorMsg == -1 
%     'Leg(s) are below the Legs Limits' ; 
% elseif ErrorMsg == -2 
%     'Leg(s) are exceed the Legs Limits' ; 

end

