HexapiBackground.IK.InverseKinematics.Gait C# (CSharp) Method

        private static double[][] Gait(int legIndex, bool travelRequest, double travelLengthX, double travelLengthZ, double travelRotationY,
            double[] gaitPosX, double[] gaitPosY, double[] gaitPosZ, double[] gaitRotY,
            int numberOfLiftedPositions, int gaitLegNr,
            double legLiftHeight, int liftDivFactor, double halfLiftHeight, int stepsInGait, int tlDivFactor)
        {
            var gaitXyZrotY = new double[4][];

            if ((travelRequest &&
                 (numberOfLiftedPositions == 1 || numberOfLiftedPositions == 3 || numberOfLiftedPositions == 5) && _gaitStep == gaitLegNr) ||
                (!travelRequest &&
                 _gaitStep == gaitLegNr && ((Math.Abs(gaitPosX[legIndex]) > 2) || (Math.Abs(gaitPosZ[legIndex]) > 2) || (Math.Abs(gaitRotY[legIndex]) > 2))))
            {
                gaitPosX[legIndex] = 0;
                gaitPosY[legIndex] = -legLiftHeight;
                gaitPosZ[legIndex] = 0;
                gaitRotY[legIndex] = 0;
            }
            //Optional Half height Rear (2, 3, 5 lifted positions)
            else if (travelRequest &&
                     ((numberOfLiftedPositions == 2 && _gaitStep == gaitLegNr) || (numberOfLiftedPositions >= 3 && (_gaitStep == gaitLegNr - 1 || _gaitStep == gaitLegNr + (stepsInGait - 1)))))
            {
                gaitPosX[legIndex] = -travelLengthX/liftDivFactor;
                gaitPosY[legIndex] = -3*legLiftHeight/(3 + halfLiftHeight);
                gaitPosZ[legIndex] = -travelLengthZ/liftDivFactor;
                gaitRotY[legIndex] = -travelRotationY/liftDivFactor;
            }
            // Optional Half height front (2, 3, 5 lifted positions)
            else if (travelRequest &&
                     (numberOfLiftedPositions >= 2) &&
                     (_gaitStep == gaitLegNr + 1 || _gaitStep == gaitLegNr - (stepsInGait - 1)))
            {
                gaitPosX[legIndex] = travelLengthX/liftDivFactor;
                gaitPosY[legIndex] = -3*legLiftHeight/(3 + halfLiftHeight);
                gaitPosZ[legIndex] = travelLengthZ/liftDivFactor;
                gaitRotY[legIndex] = travelRotationY/liftDivFactor;
            }
            //Optional Half heigth Rear 5 LiftedPos (5 lifted positions)
            else if (travelRequest &&
                     ((numberOfLiftedPositions == 5 && (_gaitStep == gaitLegNr - 2))))
            {
                gaitPosX[legIndex] = -travelLengthX/2;
                gaitPosY[legIndex] = -legLiftHeight/2;
                gaitPosZ[legIndex] = -travelLengthZ/2;
                gaitRotY[legIndex] = -travelRotationY/2;
            }
            //Optional Half heigth Front 5 LiftedPos (5 lifted positions)
            else if (travelRequest &&
                     (numberOfLiftedPositions == 5) &&
                     (_gaitStep == gaitLegNr + 2 || _gaitStep == gaitLegNr - (stepsInGait - 2)))
            {
                gaitPosX[legIndex] = travelLengthX/2;
                gaitPosY[legIndex] = -legLiftHeight/2;
                gaitPosZ[legIndex] = travelLengthZ/2;
                gaitRotY[legIndex] = travelRotationY/2;
            }
            //Leg front down position
            else if ((_gaitStep == gaitLegNr + numberOfLiftedPositions || _gaitStep == gaitLegNr - (stepsInGait - numberOfLiftedPositions)) &&
                     gaitPosY[legIndex] < 0)
            {
                gaitPosX[legIndex] = travelLengthX/2;
                gaitPosZ[legIndex] = travelLengthZ/2;
                gaitRotY[legIndex] = travelRotationY/2;
                gaitPosY[legIndex] = 0;
            }
            //Move body forward      
            else
            {
                gaitPosX[legIndex] = gaitPosX[legIndex] - (travelLengthX/tlDivFactor);
                gaitPosY[legIndex] = 0;
                gaitPosZ[legIndex] = gaitPosZ[legIndex] - (travelLengthZ/tlDivFactor);
                gaitRotY[legIndex] = gaitRotY[legIndex] - (travelRotationY/tlDivFactor);
            }

            gaitXyZrotY[0] = gaitPosX;
            gaitXyZrotY[1] = gaitPosY;
            gaitXyZrotY[2] = gaitPosZ;
            gaitXyZrotY[3] = gaitRotY;

            if (!_lastLeg)
                return gaitXyZrotY;

            _gaitStep = _gaitStep + 1;

            if (_gaitStep > stepsInGait) //The last leg in this step
                _gaitStep = 1;

            return gaitXyZrotY;
        }