Rewrite the following code in order to implement the CheckSingularityAlgorithm in the PathInterpolation function in order to check each time the joint point is calculated with CalcJointT is a point of singularity. Hence at the end the PathInterpolation function should return true if a singularity is detected in the path
MODULE PolySingV0_66_PROVA(SYSMODULE)
    ! PolySing : This module combines Singularity Detection, Direct Kinematic (through DH params), Geometric Jacobian Calculation, SVD Computation.
    !
    ! Last Update: 1/10/2024
    ! Author: Paolo Furia (<PRESIDIO_ANONYMIZED_EMAIL_ADDRESS>)
    ! 
    ! ChangeLog:
    !       V0_1 (22/08/24): First version of Singularity detection and movement based on the determinant imported from external software
    !       V0_2 (1/08/24): Added comments and code clarifications
	!       V0_3 (6/08/24): Added calculation of direct kinematics via Denavit-Hartenberg parameters -> rototranslation matrix for each joint up to TCP; added QR algorithm for calculation of eigenvalues.
	!       V0_4 (7/08/24): Used internal robotstudio functions for some steps. Added Singular Value Decomposition -> for redundant robots (#joint<>#DOF); added condition number nIndex for matrix J;
    !       V0_5 (9/08/24): The number of interpolated points depends on the nDistance of the points (minimum nDistance between the point can be changed). All calculation (determinant too) is made here.
    !       V0_6 (13/08/2024): Added reachability checks for the interpolated point. Change function names for clarification
    !       V0_61 (11/09/2024): Fixed SLERP bugs and MoveAbsJ bugs
    !       V0_62 (12/09/2024): Added SingArea \Wrist and ConfJ \Off in the joint movement to prevent changes of configuration for better performance
    !       V0_63 (12/09/2024): Fixed interpolation bug
    !       V0_64 (19/09/2024): Implemented ConfJ \Off manually. Can be still used the the RobStudio ConfJ \Off
    !       V0_65 (27/09/2024): Implemented Sing MoveL in only those point that satisfies singularity condition
    !       V0_66 (1/10/2024): Introduced METHODS in order to DEBUG 
    
    
    ! STANDARD PREFIX  
    ! t     tooldata        Tool
    ! w     wobjdata        WorkObject
    ! s     speeddata       Speed 
    ! p     robtarget       Punto x,y,z
    ! j     jointtarget       Punto x,y,z
    ! n     num             Numeric
    ! dn    dnum            Double numeric
    ! b     bool            Boolean
    ! i     intnum          Interrupt
    ! clk   clock           Timer
    ! wz    wzstationary    WorldZone
    ! dev   iodevice        I/O Device
    ! raw   rawbytes        Data for communication
    ! by    byte            Bytes
    ! s     string          String
    ! sp    shapedata       Shape Of WorldZone
    ! pos   Pos             Position x,y,z
    ! trg   triggdata       Trigger
    ! dh    DH              Denavit Hartenberg struct 
    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # CONSTANTS AND VARIABLES!                                                                         #
    ! #                                                                                                  #
    ! ####################################################################################################
    
    !Denavit-Hartenberg parameters struct
    RECORD DH
        num n_theta;
        num n_d;
        num n_a;
        num n_alpha;
    ENDRECORD
    
    ! VERBOSE
    LOCAL CONST bool VERBOSE:=TRUE;                        ! [-]   The code is verbose or not (i.e. it print out DEBUG messages or not)
    LOCAL CONST num METHOD:=2;
    
    ! Size vals
    LOCAL CONST num nMaxSize:= 1E+04;                       ! [-]   Max nSize for each array
    LOCAL VAR num nSize;                                    ! [-]   Size calculated according to the nDistance between the points
    
    ! Interpolation parameters
    LOCAL VAR num nds:= 20;                                 ! [mm]  Default Distance between interpolated points  
    LOCAL VAR num ndt;                                      ! [-]   Reciprocal of the nSize variable
    VAR robtarget pInterpolated{nMaxSize};                  ! [-]   Array of interpolated path points in robtarget
    VAR jointtarget jInterpolated{nMaxSize};                ! [-]   Array of interpolated path points in jointtarget
    LOCAL VAR bool bIsReachableInterpolatedPoint:=TRUE;     ! [-]   Check if there are not reachable point in the path
    
    ! Degrees Of Freedom (DOF) of the robot
    LOCAL CONST num nDOF := 6;                              ! [-]   # of DOF of the robot
    
    !!! UNCOMMENT THE CORRECT ROBOT !!!
    ! IRB7600_325_310
    ! Convention: theta, d, a, alpha
    LOCAL CONST DH dhParams_init{nDOF}:=
        [
            [0,     0.78,   0.41,   -90 ],
            [-90,   0,      1.075,  0   ],
            [0,     0,      0.165,  -90 ],
            [0,     1.556,  0,      90  ],
            [0,     0,      0,      -90 ],
            [180,   0.25,   0,      0   ]
        ];                                                  ! [-] DH Parameters for the SPECIFIC ROBOT. !!! CHANGE ACCORDINGLY TO THE ROBOT USED !!!!

    ! IRB4600_40_255 
!    LOCAL CONST DH dhParams_init{nDOF}:=
!        [
!            [0,     0.495,     0.175,   -90 ],
!            [-90,   0,         1.095,  0   ],
!            [0,     0,         0.175,  -90 ],
!            [0,     1.270,     0,      90  ],
!            [180,     0,       0,      90 ],
!            [0,    0.085,      0,      0   ]
!        ];

! IRB4600_20_250 
!    LOCAL CONST DH dhParams_init{nDOF}:=
!        [
!            [0,     0.495,     0.175,   -90 ],
!            [-90,   0,         1.095,  0   ],
!            [0,     0,         0.175,  -90 ],
!            [0,     1.2305,     0,      90  ],
!            [180,     0,       0,      90 ],
!            [0,    0.085,      0,      0   ]
!        ];
    LOCAL VAR DH dhParams{nDOF};                            ! [-] DH Parameters var that changes during direct kinematic calculation 
    
    ! Index condition
    LOCAL CONST num nIndexCondition := 0.01;                ! [-] Singularity condition threshold value. Ratio of smallest to larger singular value of the jacboian matrix. If the nIndex is smallest than nIndexCondition we are in singularity condition
    
    ! DEBUG values
    VAR num nJacobian_vals{6,nDOF,nMaxSize};                ! [-] Matrix that holds all the Geometric Jacobian calculated in all the interpolated points
    VAR num nIndex_vals{nMaxSize};                          ! [-] Array that holds Singularity Condition values.
    VAR num nDet_vals{nMaxSize};                            ! [-] Array that holds all the values of the determinant calculated in all the interpolated points
    VAR num nSingular_vals{nDOF, nMaxSize};                 ! [-] Matrix that stores the singular values for each robtarget interpolated point
    
    VAR num nIndexCondition_counter:=0;                     ! [-] Counter for algortithm purposes
    VAR num nIndexCondition_vals{nMaxSize};                 ! [-] This array store only the indeces of the array of the interpolated point nIndex that satisfies the singularity condition
    
    ! Errors
    VAR errnum  ERR_MAT_SIZE:=-1;                           ! [-] Error for matrix operations
    
    ! PROVA Variable for implementing custom ConfJ
    VAR jointtarget jResultPrev;
    VAR robtarget dnResultPrev;
    
    !DH_Init : Initialize DH parameters for the SPECIFIC ROBOT MODEL.  
    LOCAL PROC DH_Init()
        ! Convention: n_theta, n_d, n_a, n_alpha
        dhParams{1}:=dhParams_init{1};
        dhParams{2}:=dhParams_init{2};
        dhParams{3}:=dhParams_init{3};
        dhParams{4}:=dhParams_init{4};
        dhParams{5}:=dhParams_init{5};
        dhParams{6}:=dhParams_init{6};        
    ENDPROC
    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # END CONSTANTS AND VARIABLES                                                                      #
    ! #                                                                                                  #
    ! ####################################################################################################
    
    !________________________________________________________________________________________________________________________________________________________________________
    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # START PUBLIC FUNCTION AND PROC                                                                   #
    ! #                                                                                                  #
    ! ####################################################################################################
    
    !CheckSingularity: Check singularity along n_a linear path
    ! Input:
    !   - <PARAM> pFinal:           Final/Destination robtarget point
    !   - <PARAM> vel :             velocity at which both the robot and the external axes move
    !   - <PARAM> zone :            How close to the programmed position the axes must be before moving towards the next position
    !   - <PARAM> tToolToUse :      Characteristics of the toola as position and orientation of the tool center point (TCP) and the physical characteristics of the tool load
    !   - <PARAM> wobjToUse :       Work object data
    !   - <PARAM_OPT> nds_in :      [mm] Distance between interpolated points  
    FUNC bool CheckSingularity(robtarget pFinal, speeddata vel, zonedata zone, PERS tooldata tToolToUse, PERS wobjdata wobjToUse, \num nds_in)
        
        ! Support variables
        VAR bool bIsSing:=FALSE;
        VAR num nDistance;
        VAR robtarget pInitial;
        
        ! Change distance between interpolated point
        IF Present(nds_in) THEN
            nds := nds_in;    
        ENDIF
        
        ! Initialize DH parameters for the specific robot
        DH_Init;
        
        ! Get the current robotarget pose
        pInitial := CRobT(\Tool:=tToolToUse \WObj:=wobjToUse);
        
        ! Calculate nDistance between the actual robotarget point pInitial and the end robotarget point pFinal:
        nDistance := Euclidean_distance(pInitial, pFinal);
        
        ! Calculate the interpolating nSize (number of interpolated points) according to the nDistance. The nDistance between the interpolated point is equal to nds
        nSize := Round(nDistance/nds)+1;
        ndt := 1/nSize;
        
        ! Make sure the calculated nSize doesn't exceed the maximum nSize
        IF nSize > nMaxSize THEN
            ErrWrite \I, "POLYSING:", "Warning: Calculated nSize exceeds nMaxSize, limiting to nMaxSize.";
            nSize := nMaxSize;
            ndt := 1/nSize;
        ENDIF

        ! Interpolate the path
        PathInterpolation pInitial, pFinal, pInterpolated, tToolToUse \wobjToUse:= wobjToUse ;
        
        ! Check for singularity along the interpolated path
        bIsSing := CheckSingularityAlgorithm(pInterpolated, tToolToUse, wobjToUse);
        
        RETURN bIsSing;
    ENDFUNC
    
    !MoveL_Sing : Move the TCP according to the singularity
    ! Input:
    !   - <PARAM> pFinal :          Final/Destination robtarget point
    !   - <PARAM> vel :             Velocity at which both the robot and the external axes move
    !   - <PARAM> zone :            How close to the programmed position the axes must be before moving towards the next position
    !   - <PARAM> tToolToUse :      Characteristics of the toola as position and orientation of the tool center point (TCP) and the physical characteristics of the tool load
    !   - <PARAM> wobjToUse :       Work object data
    !   - <PARAM_OPT> nds_in :      [mm]  Distance between interpolated points
    PROC MoveL_Sing(robtarget pFinal, speeddata vel, zonedata zone, PERS tooldata tToolToUse, PERS wobjdata wobjToUse, \num nds_in)
        
        ! Support variables
        VAR string sOut;
        VAR bool bIsSing:=FALSE;
        
        ! Calculate the singularity
        IF Present(nds_in)=TRUE     THEN bIsSing := CheckSingularity(pFinal, vel, zone,  tToolToUse,  wobjToUse, \nds_in:=nds_in);  ENDIF
        IF Present(nds_in)=FALSE    THEN bIsSing := CheckSingularity(pFinal, vel, zone,  tToolToUse,  wobjToUse);                   ENDIF
        
        ! Move the robot accordingly to the singularity
        IF bIsSing AND bIsReachableInterpolatedPoint=TRUE THEN
            
            !!! UNCOMMENT THE DESIRESD METHOD
            ! METHOD 1: Use MoveL with SingArea until a singularity condition point is met, after that uses interpolated points.
            IF METHOD=1 THEN
                FOR t FROM 1 TO nIndexCondition_counter DO
                    SingArea \Wrist;
                    ConfL \Off;
                    MoveL pInterpolated{nIndexCondition_vals{t}},vel,z1,tToolToUse\WObj:=wobjToUse;
                ENDFOR
                MoveL pFinal, vel, zone, tToolToUse \WObj:=wobjToUse;
            ENDIF
            
            ! METHDO 2: Use MoveJ with SingArea.
            IF METHOD=2 THEN
                FOR t FROM 1 TO nSize STEP 1 DO
                    !DEBUG
    !                TPWrite "J val: " + NumToStr(jInterpolated{t}.robax.rax_1,6) + "; " + NumToStr(jInterpolated{t}.robax.rax_2,6)+ "; " + NumToStr(jInterpolated{t}.robax.rax_3,6)+ "; " + NumToStr(jInterpolated{t}.robax.rax_4,6)+ "; " + NumToStr(jInterpolated{t}.robax.rax_5,6)+ "; " + NumToStr(jInterpolated{t}.robax.rax_6,6);
    !                sOut:="[";
    !                FOR n FROM 1 TO nDOF DO
    !                    sOut:=sOut + NumToStr(nSingular_vals{n,t},4) + "   ";
    !                ENDFOR
    !                sOut:=sOut + "] ";
    !                TPWrite sOut;
    !                TPWrite "nIndex: " + NumToStr(nIndex_vals{t},4);
    !                TPWrite " nDet: " + NumToStr(nDet_vals{t},4);
    !                TPWrite " ";
                    SingArea \Wrist;
                    ConfJ \Off;
                    MoveJ pInterpolated{t},vel,z1,tToolToUse\WObj:=wobjToUse;
    !                MoveAbsJ jInterpolated{t},vel,z1,tToolToUse\WObj:=wobjToUse;
                ENDFOR
            ENDIF
            
            ! METHDO 3: Use MoveAbsJ 
            IF METHOD=3 THEN
                FOR t FROM 1 TO nSize STEP 1 DO
                    MoveABSJ jInterpolated{t},vel,z1,tToolToUse\WObj:=wobjToUse;
                ENDFOR
                MoveAbsJ jInterpolated{nSize},vel,zone,tool0\WObj:=wobj0;
            ENDIF
            !DEBUG
!            sOut:="[";
!            FOR n FROM 1 TO nDOF DO
!                    sOut:=sOut + NumToStr(nSingular_vals{n,nSize},4) + "   ";
!            ENDFOR
!            sOut:=sOut + "] ";
!            sOut:=sOut +"nIndex: " + NumToStr(nIndex_vals{nSize},4);
            !TPWrite sOut;
            
            ! Restore SingArea Off
            SingArea \Off;
        ELSE 
            MoveL pFinal, vel, zone, tToolToUse \WObj:=wobjToUse;
        ENDIF
    ENDPROC
    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # END PUBLIC FUNCTION AND PROC                                                                     #
    ! #                                                                                                  #
    ! ####################################################################################################
    
    !________________________________________________________________________________________________________________________________________________________________________
    
    ! ####################################################################################################
    ! #                                                                                                  #
    ! # START LOCAL FUNCTION AND PROC                                                                    #
    ! #                                                                                                  #
    ! ####################################################################################################
    
    !CheckSingularityAlgorithm : Check if there is n_a singularity in the array of points
    ! Input: 
    !   - <PARAM> pInterpolatedIN{*} :      Trajectory path interpolated
    !   - <PARAM> tToolToUse :              Tool used
    !   - <PARAM> wobjToUse :               Work obj used
    ! Output:
    !   - <PARAM> bIsSing:                  TRUE if n_a singularity is present, FALSE otherwise
    LOCAL FUNC bool CheckSingularityAlgorithm(INOUT robtarget pInterpolatedIN{*}, PERS tooldata tToolToUse, PERS wobjdata wobjToUse) 
        
        ! Support variables
        VAR jointtarget jSupport;
        VAR bool bIsSing := FALSE;      
        VAR dnum dnJac{6,nDOF};
        VAR num nIndex;              ! Index : Ratio of the smallest to the largest singular value. Under conditions of singularity you will have 0/x=0
        VAR num nDet;
        ! Support variables for SVD decomposition
        VAR dnum dnU{6,6};
        VAR dnum dnS{nDOF};
        VAR dnum dnV{nDOF, nDOF};
        
        ! Main Loop : Check singularity in all the interpolated points
        FOR i FROM 1 TO nSize DO
            
            ! Initialize the determinant
            nDet := 1;
            
            jSupport := jInterpolated{i};
            
            ! Calculate Geometric Jacobian for the given joint configuration
            !TODO: Warning, if the robot is redundant this goes in error.          
            CalcJac dnJac, dhParams, [jSupport.robax.rax_1, jSupport.robax.rax_2, jSupport.robax.rax_3, jSupport.robax.rax_4, jSupport.robax.rax_5, jSupport.robax.rax_6];
            
            ! DEBUG : Save jacobian
            FOR row FROM 1 TO 6 DO
                FOR col FROM 1 TO nDOF DO
                    nJacobian_vals{row,col,i}:=DnumToNum(dnJac{row,col});
                ENDFOR
            ENDFOR
            
            ! Perform the SVD
            MatrixSVD dnJac, dnU, dnS, dnV;
            
            ! Calculate the determinant and save it
            FOR k FROM 1 TO Dim(dnS,1) DO
                ! Calculate the determinant
                nDet:=nDet*DNumToNum(dnS{k});
                ! Store the eigenvalues
                nSingular_vals{k,i}:=DnumToNum(dnS{k});
            ENDFOR
            nDet_vals{i}:=ABS(nDet);
            
            ! Calculate the nIndex
            nIndex := DnumToNum(dnS{Dim(dnS,1)}/dnS{1});
            
            ! DEBUG Store the nIndex value
            nIndex_vals{i}:=nIndex;
            
            ! Check singularity condition
            IF Abs(nIndex) < nIndexCondition THEN
                ! Store which interpolated point that satisfies the nIndexCondition
                Incr nIndexCondition_counter;
                nIndexCondition_vals{nIndexCondition_counter}:=i;
                ! Set the Singularity condition to true
                bIsSing := TRUE;
            ENDIF
        ENDFOR      
        
        ! Return Singularity check result
        IF bIsSing THEN
            ErrWrite\W, "POLYSING:", "Singularity detected in the path.";
            IF VERBOSE THEN Print_pTarget_sing; ENDIF
            RETURN bIsSing;
        ELSEIF bIsSing=FALSE THEN 
            ErrWrite \W, "POLYSING:", "No singularity in the path.";
            RETURN bIsSing;
        ENDIF
        
        ! If the algorithm goes in error means that a not reachable point in the path
        ERROR 
            IF ERRNO=ERR_ROBLIMIT  OR ERRNO=ERR_OUTSIDE_REACH OR ERRNO=ERR_WOBJ_MOVING THEN
                ErrWrite \W, "POLYSING:", "There is a NOT REACHABLE point IN the path!";
                bIsReachableInterpolatedPoint:=FALSE;
                bIsSing:=TRUE;
                RETURN bIsSing;
            ENDIF
            TRYNEXT;
    ENDFUNC
        
    !CalcJac : Compute the direct kinematic rototranslation matrix and the Geometric Jacobian
    ! Input:
    !   - <PARAM> dnJac{*,*} :          Jacobian matrix
    !   - <PARAM> dhParameters{*} :     Denavit-Hartenberg parameters for the SPECIFIC ROBOT
    !   - <PARAM> n_q_vals{*,*} :       joint values
    LOCAL PROC CalcJac(INOUT dnum dnJac{*,*}, INOUT DH dhParameters{*}, num n_q_vals{*})
        
        ! Support parameters
        VAR dnum dnZ{3, nDOF + 1};
        VAR dnum dnP{3, nDOF + 1};
        VAR dnum dnA0iall{4, 4, nDOF + 1};
        VAR dnum dnAim1iall{4, 4, nDOF + 1};
        VAR dnum dnA06_temp{4, 4};
        VAR dnum dnAi{4, 4};
        VAR dnum dnZi{3,1};
        VAR dnum dnPi{3,1};
        VAR dnum dnPn{3,1};
        VAR dnum dnCrossProductRes{3,1};
        ! Rototranslation matrix
        VAR dnum dnA06{4, 4}; 
        
        ! Add actual current joint values
        FOR i FROM 1 TO nDOF DO
            dhParameters{i}.n_theta := n_q_vals{i} + dhParams_init{i}.n_theta;
        ENDFOR
        
        ! Initialize dnA06 to identity matrix
        Init_mat_identity dnA06;

        dnZ{3, 1} := 1; ! Initial dnZ vector

        FOR i FROM 1 TO nDOF DO
            
            ! Compute the roto-translation matrices according to DH params
            DHTransform dhParameters{i}.n_alpha, dhParameters{i}.n_a, dhParameters{i}.n_theta, dhParameters{i}.n_d, dnAi ;
            
            !Calculate the reference frame wrt the previous one.  !!!! IF ROBOTWARE VERSION >= 6.15.04 USE BUILT-IN FUNCTION MatrixMult intstead of Matrix_Mult
            Matrix_Mult  dnA06, dnAi, dnA06_temp ;
            !MatrixMult  dnA06, dnAi, dnA06_temp ;
            
            dnA06 := dnA06_temp;
            
            !DEBUG
!            FOR r FROM 1 TO 4 DO
!                FOR c FROM 1 TO 4 DO
!                    dnA0iall{r, c, i} := dnA06{r, c};
!                    dnAim1iall{r, c, i} := dnAi{r, c};
!                ENDFOR
!            ENDFOR

            FOR j FROM 1 TO 3 DO
                dnZ{j, i + 1} := dnA06{j, 3};
                dnP{j, i + 1} := dnA06{j, 4};
            ENDFOR
            
        ENDFOR
        
        dnPn{1,1} := dnA06{1, 4};
        dnPn{2,1} := dnA06{2, 4};
        dnPn{3,1} := dnA06{3, 4};

        FOR i FROM 1 TO nDOF DO
            
            FOR j FROM 1 TO 3 DO
                dnZi{j,1} := dnZ{j, i};
                dnPi{j,1} := dnP{j, i};
            ENDFOR
            
            Matrix_Sub dnPn,dnPi,dnCrossProductRes; !!!! IF ROBOTWARE VERSION >= 6.15.04 USE BUILT-IN FUNCTION MatrixSub intstead of Matrix_Sub
            !MatrixSub dnPn,dnPi,dnCrossProductRes;
            
            CrossProduct dnZi, dnCrossProductRes, dnCrossProductRes;

            FOR j FROM 1 TO 3 DO
                dnJac{j, i} := dnCrossProductRes{j,1};
                dnJac{j + 3, i} := dnZi{j,1};
            ENDFOR
            
        ENDFOR
    ENDPROC
    
    !DHTransform : Perform rototranslational matrix from frame i-1 to frame i using Denavit-Hartenberg convention
    ! Input:
    !   - <PARAM> n_alpha : 
    !   - <PARAM> n_a :
    !   - <PARAM> n_theta:
    !   - <PARAM> n_d:
    ! Output:
    !   - <PARAM> matrix{*,*} : Rototranslation matrix for the given frame
    LOCAL PROC DHTransform(num n_alpha, num n_a, num n_theta, num n_d, INOUT dnum matrix{*,*})
        ! Support parameters
        VAR dnum dnCosTheta;
        VAR dnum dnSinTheta;
        VAR dnum dnCosAlpha;
        VAR dnum dnSinAlpha;
        
        dnCosTheta := NumToDNum(Cos(n_theta));
        dnSinTheta := NumToDNum(Sin(n_theta));
        dnCosAlpha := NumToDNum(Cos(n_alpha));
        dnSinAlpha := NumToDNum(Sin(n_alpha));

        matrix{1, 1} := dnCosTheta;
        matrix{1, 2} := -dnSinTheta * dnCosAlpha;
        matrix{1, 3} := dnSinTheta * dnSinAlpha;
        matrix{1, 4} := NumToDNum(n_a) * dnCosTheta;

        matrix{2, 1} := dnSinTheta;
        matrix{2, 2} := dnCosTheta * dnCosAlpha;
        matrix{2, 3} := -dnCosTheta * dnSinAlpha;
        matrix{2, 4} := NumToDNum(n_a) * dnSinTheta;

        matrix{3, 1} := 0;
        matrix{3, 2} := dnSinAlpha;
        matrix{3, 3} := dnCosAlpha;
        matrix{3, 4} := NumToDNum(n_d);

        matrix{4, 1} := 0;
        matrix{4, 2} := 0;
        matrix{4, 3} := 0;
        matrix{4, 4} := 1;
    ENDPROC
    
    !CrossProduct : perform cross product between two 3D vectors
    ! INFO: https://en.wikipedia.org/wiki/Cross_product. 
    ! Input:
    !   - <PARAM> A{*} : First vector
    !   - <PARAM> B{*} : Second vector
    ! Output:
    !   - <PARAM> dnResult{*} : Save the dnResult. AxB=dnResult
    LOCAL PROC CrossProduct(dnum A{*,*}, dnum B{*,*}, INOUT dnum dnResult{*,*})
        
        IF Dim(A,1)=Dim(A,1) THEN
            dnResult{1,1} := A{2,1} * B{3,1} - A{3,1} * B{2,1};
            dnResult{2,1} := A{3,1} * B{1,1} - A{1,1} * B{3,1};
            dnResult{3,1} := A{1,1} * B{2,1} - A{2,1} * B{1,1};
        ELSE
            ErrWrite \W, "POLYSING:", "ERROR: VECTOR MUST BE OF EQUAL SIZE";
            !TODO: Handle correctly the error
        ENDIF
    ENDPROC
    
    !PrintArray : function to print an array on the TP
    ! Input:
    !   - <PARAM> dnArr{*} : Array to be printed
    LOCAL PROC PrintArray(dnum dnArr{*})
        ! Support variables
        VAR string sOut;
        sOut:="[";
        FOR i FROM 1 TO Dim(dnArr,1) DO
            sOut:= sOut + DNumToStr(dnArr{i},4) + "   ";
        ENDFOR
        TPWrite sOut + "]";
    ENDPROC

    !PrintMatrix2D : function to print n_a 2D matrix on the TP
    ! Input:
    !   - <PARAM> dnMat{*} : Matrix to be printed
    LOCAL PROC PrintMatrix2D(dnum dnMat{*,*})
        ! Support variables
        VAR string sOut;
        
        FOR i FROM 1 TO Dim(dnMat,1) DO
            sOut:="";
            FOR j FROM 1 TO Dim(dnMat,2) DO
                sOut:= sOut + DNumToStr(dnMat{i,j},4) + "  ";
            ENDFOR
            TPWrite sOut + ";";
        ENDFOR
    ENDPROC
    
    !PrintMatrix3D : function to print n_a 2D matrix on the TP
    ! Input:
    !   - <PARAM> nMat{*} : Matrix to be printed
    LOCAL PROC PrintMatrix3D(num nMat{*, *, *}, num t)
        ! Support variables
        VAR string sOut;
        
        FOR i FROM 1 TO Dim(nMat,1) DO
            sOut:="";
            FOR j FROM 1 TO Dim(nMat,2) DO
                sOut:= sOut + NumToStr(nMat{i,j,t},4) + "  ";
            ENDFOR
            TPWrite sOut + ";";
        ENDFOR
    ENDPROC
    
    !Init_mat_identity: Initialize to an identity matrix n_a given 2D matrix
    ! Input: 
    !   - <PARAM> dnMat{*,*} : Input matrix
    LOCAL PROC Init_mat_identity(INOUT dnum dnMat{*,*})
        FOR i FROM 1 TO Dim(dnMat,1) DO
            FOR j FROM 1 TO Dim(dnMat,2) DO
                IF i = j THEN
                    dnMat{i, j} := 1;
                ELSE
                    dnMat{i, j} := 0;
                ENDIF
            ENDFOR
        ENDFOR
    ENDPROC

    
    !PathInterpolation :  SLERP interpolation path given two point 
    ! INFO: https://en.wikipedia.org/wiki/Slerp
    ! Input:
    !   - <PARAM> p1 : Starting path point
    !   - <PARAM> p2 : End path point
    !   - <PARAM> pInterpolatedIN{*} : Array of interpolated robotarget point
    LOCAL PROC PathInterpolation(robtarget p1, robtarget p2, INOUT robtarget pInterpolatedIN{*}, PERS tooldata tToolToUse \PERS wobjdata wobjToUse )
        !Support variables
        VAR robtarget dnResult;
        VAR num count := 1;
        VAR num t:= 0;
        VAR jointtarget jResult;
        
        VAR bool cond1:=FALSE;
        VAR bool cond2:=FALSE;

        ! PROVA
        VAR confdata confTemp;
            
        WHILE count<=nSize DO
            t:= (count-1)*ndt;
            
            ! Linear interpolation for positions
            dnResult.trans.x := (1 - t) * p1.trans.x + t * p2.trans.x;
            dnResult.trans.y := (1 - t) * p1.trans.y + t * p2.trans.y;
            dnResult.trans.z := (1 - t) * p1.trans.z + t * p2.trans.z;
            
            ! Slerp interpolation for orientations
            dnResult.rot := Slerp(p1.rot, p2.rot, t);
            !dnResult.rot := p1.rot;
            
            ! Copy the other attributes
!            dnResult.robconf.cf4 := p1.robconf.cf4;
!            dnResult.robconf.cf6 := p1.robconf.cf6;
!            dnResult.robconf:=p1.robconf;
            !confTemp:=p1.robconf;
            dnResult.extax := p1.extax;
            IF METHOD=2 OR METHOD=1 THEN
                dnResult.robconf:=p1.robconf; 
                jResult := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
                ! Save the joint variables in an array for DEBUG Purposes
                jInterpolated{count}:=jResult;
                pInterpolatedIN{count} := dnResult;
            ENDIF
                
            IF METHOD=3 THEN
                ! DEBUG - PROVA
                IF count<> 1 THEN
                    ! Calculate the joint values at the current TCP pose
                    jResult := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
                    ! Save the joint variables in an array for DEBUG Purposes
                    jInterpolated{count}:=jResult;
                    ! Check if 
!                    cond1 := jResultPrev.robax.rax_5>0 AND jResult.robax.rax_5<=0;
!                    cond2 := jResultPrev.robax.rax_5<0 AND jResult.robax.rax_5>=0;
!                    cond2 := Abs(jResult.robax.rax_5)<=0.2; ![°]
!                    cond1 := (jResultPrev.robax.rax_4 - jResult.robax.rax_4) + (jResultPrev.robax.rax_6 - jResult.robax.rax_6)<=0.5;
                    cond1 := Abs(jResultPrev.robax.rax_4 - jResult.robax.rax_4)>=60  AND Abs(jResultPrev.robax.rax_6 - jResult.robax.rax_6)>=60;
                    !cond2 := jResultPrev.robax.rax_5<0 AND jResult.robax.rax_5>=0;
                    !IF Abs(jResultPrev.robax.rax_4 - jResult.robax.rax_4)>=120  AND Abs(jResultPrev.robax.rax_6 - jResult.robax.rax_6)>=120 THEN 
                    IF cond1 THEN
                        ! DEBUG
                        TPWrite "I'm here";
                        TPWrite "Prev 4: "+ NumToStr(jResultPrev.robax.rax_4,4)+ "conf:" + NumToStr(pInterpolatedIN{count-1}.robconf.cf4,2)+ "actual 4: "+ NumToStr(jResult.robax.rax_4,4) + "conf:" + NumToStr(dnResult.robconf.cf4,2);
                        TPWrite "Prev 5: "+ NumToStr(jResultPrev.robax.rax_5,4)+ "conf:" + NumToStr(pInterpolatedIN{count-1}.robconf.cf6,2)+ "actual 4: " + NumToStr(jResult.robax.rax_5,4)+ "conf:" + NumToStr(dnResult.robconf.cf6,2);
                        TPWrite "Prev 6: "+ NumToStr(jResultPrev.robax.rax_6,4)+ "actual 6: "+ NumToStr(jResult.robax.rax_6,4);
                        
                        dnResult.robconf.cfx := p2.robconf.cfx;
    !                    dnResult.robconf.cfx := dnResult.robconf.cfx+1;
                        dnResult.robconf.cf4:=pInterpolated{count-1}.robconf.cf4;
                        dnResult.robconf.cf6:=pInterpolated{count-1}.robconf.cf6;
    !                    jResult.robax.rax_4:=jResultPrev.robax.rax_4;
    !                    jResult.robax.rax_6:=jResultPrev.robax.rax_6;
                        
                        jResult := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
                        dnResult:=CalcRobT(jResult, tToolToUse\WObj:=wobjToUse);
                        
                        jInterpolated{count}:=jResult;
                        pInterpolatedIN{count} := dnResult;
                        
                        jResultPrev:=jResult;
                        
    !                ELSEIF cond2 THEN
    !                    dnResult.robconf.cfx := dnResult.robconf.cfx-1;
    !                    jResult := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
                        
    !                    pInterpolatedIN{count} := dnResult;
    !                    jInterpolated{count}:=jResult;
                        
    !                    jResultPrev:=jResult;
                        
                    ELSE
!                        dnResult.robconf := pInterpolatedIN{count-1}.robconf;
                        dnResult.robconf.cfx := pInterpolatedIN{count-1}.robconf.cfx;
                        pInterpolatedIN{count} := dnResult;
                        jResultPrev:=jResult;
                    ENDIF
                ELSEIF count = 1 THEN
                    dnResult.robconf:=p1.robconf;
                    jResultPrev := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
                    ! Save the joint variables in an array for DEBUG Purposes
                    jInterpolated{count}:=jResultPrev;
                    pInterpolatedIN{count} := dnResult;
                ENDIF
            ENDIF
            ! PROVA
!            IF count <> 1 THEN
!                confTemp:= BestConfiguration(pInterpolatedIN{count-1},dnResult,tToolToUse \wobjToUse:=wobjToUse);
!                dnResult.robconf:=confTemp;
                
!                jResult := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
!                ! Save the joint variables in an array for DEBUG Purposes
!                jInterpolated{count}:=jResult;
!                pInterpolatedIN{count} := dnResult;
!            ELSE
!                jResult := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
!                ! Save the joint variables in an array for DEBUG Purposes
!                jInterpolated{count}:=jResult;
!                pInterpolatedIN{count} := dnResult;
!            ENDIF
!            jResult := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
!            ! Save the joint variables in an array for DEBUG Purposes
!            jInterpolated{count}:=jResult;
!            pInterpolatedIN{count} := dnResult;

            ! Increment the counter
            Incr count;
        ENDWHILE
        
        ! Ensure the last point exactly matches p2
        dnResult := p2;
        dnResult.robconf:=pInterpolatedIN{count-1}.robconf;
!        dnResult.robconf.cf4:=pInterpolatedIN{count-1}.robconf.cf4;
!        dnResult.robconf.cf6:=pInterpolatedIN{count-1}.robconf.cf6;
        jResult := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
        
        ! Save the joint variables in an array for DEBUG Purposes
        jInterpolated{nSize}:=jResult;
        pInterpolatedIN{nSize} := dnResult;
        
    ENDPROC

    !Slerp : Perform SLERP interpolation for ORIENTATION
    LOCAL FUNC orient Slerp(orient p1, orient p2, num t)
        ! Support variables
        VAR num dot ;
        VAR num n_theta;
        VAR num dnSinTheta;
        VAR num scale1;
        VAR num scale2;
        VAR orient dnResult;
        VAR num magnitude;
        
        ! Compute the dot product (cosine of the angle between the quaternions)
        dot := p1.q1*p2.q1 + p1.q2*p2.q2 + p1.q3*p2.q3 + p1.q4*p2.q4;
        
        ! If the dot product is negative, negate one quaternion to take the shorter path
        IF dot < 0 THEN
            p1.q1 := -p1.q1;
            p1.q2 := -p1.q2;
            p1.q3 := -p1.q3;
            p1.q4 := -p1.q4;
            dot := -dot;
        ENDIF
    
        ! If the quaternions are too close, use linear interpolation
        IF dot > 0.9995 THEN
            dnResult.q1 := (1 - t) * p1.q1 + t * p2.q1;
            dnResult.q2 := (1 - t) * p1.q2 + t * p2.q2;
            dnResult.q3 := (1 - t) * p1.q3 + t * p2.q3;
            dnResult.q4 := (1 - t) * p1.q4 + t * p2.q4;
        ELSE
            ! Compute the angle and sin(n_theta)
            n_theta := ACOS(dot);
            dnSinTheta := SIN(n_theta);
            
            ! Compute the scaling factors
            scale1 := SIN((1 - t) * n_theta) / dnSinTheta;
            scale2 := SIN(t * n_theta) / dnSinTheta;
            
            ! Compute the interpolated quaternion
            dnResult.q1 := scale1 * p1.q1 + scale2 * p2.q1;
            dnResult.q2 := scale1 * p1.q2 + scale2 * p2.q2;
            dnResult.q3 := scale1 * p1.q3 + scale2 * p2.q3;
            dnResult.q4 := scale1 * p1.q4 + scale2 * p2.q4;
        ENDIF
        
        ! Normalize the resulting quaternion
        magnitude := SQRT(dnResult.q1*dnResult.q1 + dnResult.q2*dnResult.q2 + dnResult.q3*dnResult.q3 + dnResult.q4*dnResult.q4);
        dnResult.q1 := dnResult.q1 / magnitude;
        dnResult.q2 := dnResult.q2 / magnitude;
        dnResult.q3 := dnResult.q3 / magnitude;
        dnResult.q4 := dnResult.q4 / magnitude;
        
        ! Return the normalized result
        RETURN dnResult;
    ENDFUNC

    
    !Euclidean_distance : Calculate the euclidean distance between two robtarget points p1 and p2
    LOCAL FUNC num Euclidean_distance(robtarget p1, robtarget p2)
        ! Support variables
        VAR num nDistance;
        ! Calculate Euclidean distance
        nDistance := SQRT(
            Pow(p2.trans.x - p1.trans.x, 2) +
            Pow(p2.trans.y - p1.trans.y, 2) +
            Pow(p2.trans.z - p1.trans.z, 2)
        );
        RETURN nDistance;
    ENDFUNC
    
    !Print_pTarget_sing :  Write on TP all the robotarget that satisfies the singularity condition 
    LOCAL PROC Print_pTarget_sing()
        
        ! Support variable
        VAR string sOut;
        VAR num i;
        
        sOut:= "There are " + NumToStr(nIndexCondition_counter,1) + " interpolated point in Singularity Condition";
        TPWrite sOut;
        
        FOR k FROM 1 TO nIndexCondition_counter DO
            i:=nIndexCondition_vals{k};
            TPWrite "["+NumToStr(k,0) + "]: "+ "[[" + NumToStr(pInterpolated{i}.trans.x,3)+", "+ NumToStr(pInterpolated{i}.trans.y,3)+ ", " + NumToStr(pInterpolated{i}.trans.z,3)+"],["+ NumToStr(EulerZYX(\X, pInterpolated{i}.rot),3) + ", "+ NumToStr(EulerZYX(\Y, pInterpolated{i}.rot),3) + ", " + NumToStr(EulerZYX(\Z, pInterpolated{i}.rot),3)+"]]" + " indx: " + NumToStr(nIndex_vals{i}, 5); 
        ENDFOR
        
    ENDPROC
    
    !Matrix_Mult : Procedure to multiply 2D matrices
    LOCAL PROC Matrix_Mult(dnum dnMatA{*,*}, dnum dnMatB{*,*}, INOUT dnum dnMatResult{*,*})
    
        !Support variables
        VAR num nRowA;
        VAR num nColA;
        VAR num nRowB;
        VAR num nColB;
        VAR num nRowResult;
        VAR num nColResult;
    
        BookErrNo  ERR_MAT_SIZE;
    
        nRowA := Dim(dnMatA, 1);
        nColA := Dim(dnMatA, 2);
        nRowB := Dim(dnMatB, 1);
        nColB := Dim(dnMatB, 2);
        nRowResult := Dim(dnMatResult, 1);
        nColResult := Dim(dnMatResult, 2);
    
        ! Check if the dimensions match for multiplication
        IF nColA <> nRowB OR nRowA <> nRowResult OR nColB <> nColResult THEN
            RAISE ERR_MAT_SIZE;
        ENDIF
    
        ! Perform multiplication
        FOR i FROM 1 TO nRowA DO
            FOR j FROM 1 TO nColB DO
                dnMatResult{i,j} := 0;
                FOR k FROM 1 TO nColA DO
                    dnMatResult{i,j} := dnMatResult{i,j} + dnMatA{i,k} * dnMatB{k,j};
                ENDFOR
            ENDFOR
        ENDFOR
    
        ! Error handling
        ERROR
            IF ERRNO = ERR_MAT_SIZE THEN
                ErrWrite \W, "SING_CHECK:", "Wrong dimensions or wrong values in 'Matrix_Mult'";
            ENDIF 
    ENDPROC

    
    !Matrix_Sub : Procedure to subtract 2D matrices
    LOCAL PROC Matrix_Sub(dnum dnMatA{*,*}, dnum dnMatB{*,*}, INOUT dnum dnMatResult{*,*})
    
        !Support variables
        VAR num nRowA;
        VAR num nColA;
        VAR num nRowB;
        VAR num nColB;
        VAR num nRowResult;
        VAR num nColResult;
        BookErrNo  ERR_MAT_SIZE;
    
        nRowA := Dim(dnMatA, 1);
        nColA := Dim(dnMatA, 2);
        nRowB := Dim(dnMatB, 1);
        nColB := Dim(dnMatB, 2);
        nRowResult := Dim(dnMatResult,1);
        nColResult := Dim(dnMatResult,2);
    
        ! Check if dimensions match
        IF nRowA <> nRowB OR nColA <> nColB OR nRowA <> nRowResult OR nColA <> nColResult THEN
            RAISE ERR_MAT_SIZE;
        ENDIF
    
        ! Perform subtraction
        FOR row FROM 1 TO nRowA DO
            FOR col FROM 1 TO nColA DO
                dnMatResult{row,col} := dnMatA{row,col} - dnMatB{row,col};
            ENDFOR
        ENDFOR
    
        ! Error handling
        ERROR
            IF ERRNO = ERR_MAT_SIZE THEN
                ErrWrite \W, "SING_CHECK:", "Wrong dimensions or wrong values in 'Matrix_Sub'";
            ENDIF 
    ENDPROC
    
    ! TODO
    LOCAL FUNC confdata BestConfiguration(VAR robtarget pOld,VAR robtarget pNew, PERS tooldata tToolToUse \PERS wobjdata wobjToUse)
        VAR jointtarget jOld;
        VAR jointtarget jNew;
        VAR robtarget pTemp;
        VAR jointtarget jTemp;
        VAR confdata confTemp;
        !VAR confdata confGuess;
        VAR num diff;
        VAR num total;
        VAR num threshold;
        
        VAR bool bERRORFLAG:=FALSE;
        
        VAR num c1_pOld;
        VAR num cfx_pOld;
        VAR num cfx_L;
        VAR num cfx_R;
        
        pTemp:= pNew;
        confTemp:=pOld.robconf;
        pTemp.robconf:=confTemp;
        c1_pOld:= pTemp.robconf.cf1;
        cfx_pOld:= pTemp.robconf.cfx;
        
        jOld:=CalcJointT(pOld,tToolToUse \WObj:=wobjToUse);
        jNew:=CalcJointT(pNew,tToolToUse \WObj:=wobjToUse);
        
        total:=1E6;
        threshold:= 20;
        TPWrite "Before:"+ NumToStr(confTemp.cf1,1)+","+ NumToStr(confTemp.cf4,1)+","+ NumToStr(confTemp.cf6,1)+","+ NumToStr(confTemp.cfx,1);
        
        jTemp:= CalcJointT(pTemp,tToolToUse\WObj:=wobjToUse);
        IF bERRORFLAG=FALSE THEN
            diff := Abs(SumJointDifference(jOld, jNew));
            TPWrite NumToStr(diff,2);
            IF(diff < total) THEN
                total := diff;
                confTemp :=pTemp.robconf;
            ENDIF
            IF diff<threshold THEN
                TPWrite "After:"+ NumToStr(confTemp.cf1,1)+","+ NumToStr(confTemp.cf4,1)+","+ NumToStr(confTemp.cf6,1)+","+ NumToStr(confTemp.cfx,1);
                RETURN confTemp;
            ENDIF
        ENDIF
        bERRORFLAG:=FALSE;
        
        FOR c1 FROM confTemp.cf1-1 TO confTemp.cf1+1 STEP 1 DO
!        FOR c1 FROM -4 TO +4 STEP 1 DO
            FOR c4 FROM confTemp.cf4-3 TO confTemp.cf4+3 STEP 1 DO
!            FOR c4 FROM -4 TO +4 STEP 1 DO
                FOR c6 FROM confTemp.cf6-3 TO confTemp.cf6+3 STEP 1 DO
!                FOR c6 FROM -4 TO +4 STEP 1 DO
                    IF cfx_pOld>0 THEN
                        cfx_L:=cfx_pOld-1;
                    ELSEIF cfx_pOld=0 THEN
                        cfx_L := 0;
                    ENDIF
                    FOR cfx FROM cfx_L TO cfx_pOld+1 STEP 1 DO 
                        pTemp.robconf := [c1, c4, c6, cfx];
                        jTemp:= CalcJointT(pTemp,tToolToUse\WObj:=wobjToUse);
                        IF bERRORFLAG=FALSE THEN
                            diff := Abs(SumJointDifference(jOld, jNew));
                            TPWrite NumToStr(diff,2);
                            IF(diff < total) THEN
                                total := diff;
                                confTemp := [c1, c4, c6, cfx];
                            ENDIF
                            IF diff<threshold THEN
                                TPWrite "After:"+ NumToStr(confTemp.cf1,1)+","+ NumToStr(confTemp.cf4,1)+","+ NumToStr(confTemp.cf6,1)+","+ NumToStr(confTemp.cfx,1);
                                RETURN confTemp;
                            ENDIF
                        ENDIF
                        bERRORFLAG:=FALSE;
                    ENDFOR
                ENDFOR
            ENDFOR
        ENDFOR
        
        TPWrite "After:"+ NumToStr(confTemp.cf1,1)+","+ NumToStr(confTemp.cf4,1)+","+ NumToStr(confTemp.cf6,1)+","+ NumToStr(confTemp.cfx,1);
        RETURN confTemp;
        
        ERROR 
            IF ERRNO=ERR_ROBLIMIT  OR ERRNO=ERR_OUTSIDE_REACH OR ERRNO=ERR_WOBJ_MOVING THEN
                bERRORFLAG:=TRUE;
                !TPWrite "BestConfiguration error";
            ENDIF
            TRYNEXT;
    ENDFUNC
    
    LOCAL FUNC num SumJointDifference(VAR jointtarget jOld, VAR jointtarget jNew)
        VAR num diff;
        !diff := (jNew.robax.rax_1-jOld.robax.rax_1)+(jNew.robax.rax_2-jOld.robax.rax_2)+(jNew.robax.rax_3-jOld.robax.rax_3)+(jNew.robax.rax_4-jOld.robax.rax_4)+(jNew.robax.rax_5-jOld.robax.rax_5)+(jNew.robax.rax_6-jOld.robax.rax_6);
        diff :=(jNew.robax.rax_4-jOld.robax.rax_4)-(jNew.robax.rax_6-jOld.robax.rax_6);
        RETURN diff;
    ENDFUNC

    ! ####################################################################################################
    ! #                                                                                                  #
    ! # END LOCAL FUNCTION AND PROC                                                                      #
    ! #                                                                                                  #
    ! ####################################################################################################

ENDMODULE