% What this is modelling:
% psiN(x) = aN + bNixi
% aN = 1/6C * alternating(NRST) * alternating(ijk)*xRi*xSj*xTk
% bNi = -1(6C) * alternating(NRST) *
%         alternating(mjk)*betaRm(i)*betaSj(i)*betaTk(i)


% Tetrahedron points
% p1 = [2,4,3]
% p2 = [4,4,9]
% p3 = [8,12,3]
% p4 = [8,4,3]

% p1 = [4,0,0]
% p2 = [1,1,0]
% p3 = [0,1,1]
% p4 = [1,0,1]

% p1 = [2,0,8]
% p2 = [2,0,0]
% p3 = [12,0,0]
% p4 = [2,8,0]

p1 = [2,0,8]
p2 = [2,0,0]
p3 = [12,0,0]
p4 = [12,10,0]



testPoint = [11,8,1]

nodalVals = [20,25,32,50]

points = buildPoints(p1,p2,p3,p4)
shapeCoeff = buildShapeCoeff(p1,p2,p3,p4)



% Not in use yet, these will be values at nodes
% c1 = 255
% c2 = 180
% c3 = 240
% c4 = 170



aCoeffPrefix = buildACoeffPrefix(shapeCoeff)
bCoeffPrefix = buildBCoeffPrefix(shapeCoeff)

aVals = calcAVals(aCoeffPrefix, points)

bVals = calcBVals(bCoeffPrefix, points)


% Alternating tensor testing examples
% a = [ 4 3 2 1] % 1
% b = [ 1 3 2 ] % -1
% c = [ 2 3 2 ] % 0
% res = alternating(a)
% res = alternating(b)
% res = alternating(c)


% Test out the shape functions with test values
nIndex = 1
resVal = 0
while (nIndex <=4)
    iIndex = 1
    bAccum = 0
    while (iIndex <=3)
        bAccum = bAccum + bVals(nIndex,1)*testPoint(iIndex)
        iIndex = iIndex + 1
    end
    resVal = resVal + nodalVals(nIndex)*(aVals(nIndex)+bAccum)
    nIndex = nIndex+1
end

% Nonsense line so you can set a breakpoint here when debugging
setDebuggingBreakpointHere = 1;



% Alternating tensor, takes array as input
function res = alternating(input)
    I = speye(length(input));
    res = det(I(:,input));
end

% build i x j C shape coefficients
function shapeCoeffRes = buildShapeCoeff(point1, point2, point3, point4)
    shapeCoeffRes = zeros(4,4) % row count, col count
    shapeCoeffRes(1,:) = [1, point1(1),point1(2),point1(3)]%[1,point1(1),point1(2),point1(3)]
    shapeCoeffRes(2,:) = [1,point2(1),point2(2),point2(3)]%[1,point2(1),point2(2),point2(3)]
	shapeCoeffRes(3,:) = [1,point3(1),point3(2),point3(3)]%[1,point3(1),point3(2),point3(3)]
    shapeCoeffRes(4,:) = [1,point4(1),point4(2),point4(3)]%[1,point4(1),point4(2),point4(3)]
    
    %shapeCoeffRes(1,:) = [point1(1),point1(2),point1(3),1]%[1,point1(1),point1(2),point1(3)]
    %shapeCoeffRes(2,:) = [point2(1),point2(2),point2(3),1]%[1,point2(1),point2(2),point2(3)]
	%shapeCoeffRes(3,:) = [point3(1),point3(2),point3(3),1]%[1,point3(1),point3(2),point3(3)]
    %shapeCoeffRes(4,:) = [point4(1),point4(2),point4(3),1]%[1,point4(1),point4(2),point4(3)]
    
    shapeCoeffRes = shapeCoeffRes % Transpose rowmajor versus colmajor
end

% i x j points
function pointsRes = buildPoints(point1, point2, point3, point4)
    pointsRes = zeros(4,3)
    pointsRes(1,:) = point1
    pointsRes(2,:) = point2
    pointsRes(3,:) = point3
	pointsRes(4,:) = point4

end

% 1 / 6C 
function aCoeff = buildACoeffPrefix(shapeCoeffVal)

    detShapeCoeff = det(shapeCoeffVal)
    %aCoeff = detShapeCoeff/6;

    aCoeff = (1/(6*detShapeCoeff))

end

% -1 / 6C 
function bCoeff = buildBCoeffPrefix(shapeCoeffVal)

    detShapeCoeff = det(shapeCoeffVal)

    bCoeff = ((-1)/(6*detShapeCoeff))
    %bCoeff = detShapeCoeff/6;
    %bCoeff = bCoeff * (-1);
end

% Calculate aN quantities
function aValsRes = calcAVals(coeffPrefixVal, pointsVals)
    aValsRes = zeros(4,1)
    NInd = 1;
    RInd = 1;
    SInd = 1;
    TInd = 1;
    iInd = 1;
    jInd = 1;
    kInd = 1;
    

    

    while (NInd <=4)
        while (RInd <=4)
            while (SInd <=4)
                while (TInd <=4)
                    while (iInd <=3)
                        while (jInd <=3)
                            while (kInd <=3)
                                alternatingAVals = [NInd,RInd,SInd,TInd];
                                alternatingBVals = [iInd,jInd,kInd];
                                pointsIVal = pointsVals(RInd,iInd);
                                pointsJVal = pointsVals(SInd,jInd);
                                pointsKVal = pointsVals(TInd,kInd);
                                aValsRes(NInd) = aValsRes(NInd) + coeffPrefixVal*alternating(alternatingAVals)*alternating(alternatingBVals)*pointsIVal*pointsJVal*pointsKVal;
                                kInd = kInd + 1;
                            end
                            kInd = 1;

                            jInd = jInd + 1;
                        end
                        jInd = 1;

                        iInd = iInd + 1;
                    end
                    iInd = 1;
                    
                    TInd = TInd + 1;
                end
                TInd = 1;

                SInd = SInd + 1;
            end
            SInd = 1;

            RInd = RInd + 1;
        end
        RInd = 1;
        
        NInd = NInd + 1;
    end
    
    
end

% Beta resolving
function buildBValRes = buildBVal(pointsVals, RInd, iInd, mInd)
    if (mInd == iInd)
        buildBValRes = pointsVals(RInd,iInd);
    else
        buildBValRes = 1;
    end
end


% calc bNi quantities
function bValsRes = calcBVals(coeffPrefixVal, pointsVals)
    bValsRes = zeros(4,3)
    NInd = 1;
    RInd = 1;
    SInd = 1;
    TInd = 1;
    iInd = 1;
    jInd = 1;
    kInd = 1;
    mInd = 1;

    
    while (iInd <=3)
        while (NInd <=4)
            while (RInd <=4)
                while (SInd <=4)
                    while (TInd <=4)                        
                        while (mInd <=3)
                            while (jInd <=3)
                                while (kInd <=3)
                                    alternatingAVals = [NInd,RInd,SInd,TInd];
                                    alternatingBVals = [mInd,jInd,kInd];
                                    pointsIVal = buildBVal(pointsVals, RInd, iInd, mInd);
                                    pointsJVal = buildBVal(pointsVals, SInd, iInd, jInd);
                                    pointsKVal = buildBVal(pointsVals, TInd, iInd, kInd);
                                    alternatingNRST = alternating(alternatingAVals);
                                    alternatingMJK = alternating(alternatingBVals);
                                    %if (alternatingNRST ~= 0)&& (alternatingMJK ~= 0) % Debug
                                    %    ard = 31
                                    %    alternatingNRST
                                    %    alternatingMJK
                                    %    bValsRes
                                    %end
                                    newBValsRes = coeffPrefixVal*alternatingNRST*alternatingMJK*pointsIVal*pointsJVal*pointsKVal;
                                    bValsRes(NInd, iInd) = bValsRes(NInd, iInd) + newBValsRes;
                                    %if (alternatingNRST ~= 0)&& (alternatingMJK ~= 0) % Debug
                                    %    ard = 33
                                    %    alternatingNRST
                                    %    alternatingMJK
                                    %    bValsRes
                                    %    newBValsRes
                                    %end
                                    kInd = kInd + 1;
                                end
                                kInd = 1;

                                jInd = jInd + 1;
                            end
                            jInd = 1;

                            mInd = mInd + 1;
                        end
                        mInd = 1;

                        TInd = TInd + 1;
                    end
                    TInd = 1;

                    SInd = SInd + 1;
                end
                SInd = 1;

                RInd = RInd + 1;
            end
            RInd = 1;
            
            NInd = NInd + 1;
        end
        NInd = 1;
        
        iInd = iInd + 1;        
    end
    
    
end