Untitled

std::vector<double> diffVector(const std::vector<double>& v) {
    std::vector<double> dv;
    for (size_t i = 1; i < v.size(); ++i)
        dv.push_back(v[i] - v[i - 1]);
    return dv;
}

// Helper: Convert std::vector<double> to Eigen::VectorXd
Eigen::VectorXd vectorToEigen(const std::vector<double>& v) {
    return Eigen::Map<const Eigen::VectorXd>(v.data(), v.size());
}

Eigen::MatrixXd geoLeakage(int ipitcv, double Axis, double R, double Alpha1, double ELR0, double WRAP, double FI1C,
    double GAPI, double GAPR, double GAPA,
    const std::vector<std::vector<double>>& contact,
    const vector<double>& zcoord) {
    // === Preliminary calculations ===
    double A = Axis;
    double D = A + R;
    double p0 = Alpha1 * 2;
    double p1 = PI - Alpha1;
    // Note: the expression in Python simplifies to acos(A/D)
    double f = std::acos(A / D);
    int cuspAngle = static_cast<int>(f /(PI)*180);
    int arcAngle = static_cast<int>(p1 / (PI) * 180);
    int archiAngle = static_cast<int>(p0 / (PI) * 180);

    Eigen::VectorXd zcoordVec(zcoord.size());
    for (size_t i = 0; i < zcoord.size(); ++i) {
        zcoordVec(i) = zcoord[i];
    }

    // === Process contact points (assume each row has at least 4 elements) ===
    std::vector<double> contact_x, contact_y, contact_z;
    for (const auto& row : contact) {
        contact_x.push_back(row[1]);
        contact_y.push_back(row[2]);
        contact_z.push_back(row[3]);
    }
    // (The “cusp” sub-vectors are not used later in this code.)

    // rotorWrap in degrees from WRAP (radians → deg)
    int rotorWrap = static_cast<int>(WRAP / (PI) * 180);

    // === Axial leakage ===
    int sizeLeading = rotorWrap + arcAngle + archiAngle + arcAngle;
    Eigen::VectorXd axialAreaLeading(sizeLeading);
    axialAreaLeading.segment(0, rotorWrap).setZero();
    axialAreaLeading.segment(rotorWrap, arcAngle).setConstant(2 * R);
    axialAreaLeading.segment(rotorWrap + arcAngle, archiAngle).setConstant(3 * R);
    axialAreaLeading.segment(rotorWrap + arcAngle + archiAngle, arcAngle).setConstant(2 * R);
    axialAreaLeading *= GAPA;

    int sizeTrailing = (rotorWrap - 360) + arcAngle + archiAngle + arcAngle;
    Eigen::VectorXd axialAreaTrailing(sizeTrailing);
    axialAreaTrailing.segment(0, rotorWrap - 360).setZero();
    axialAreaTrailing.segment(rotorWrap - 360, arcAngle).setConstant(2 * R);
    axialAreaTrailing.segment(rotorWrap - 360 + arcAngle, archiAngle).setConstant(3 * R);
    axialAreaTrailing.segment(rotorWrap - 360 + arcAngle + archiAngle, arcAngle).setConstant(2 * R);
    axialAreaTrailing *= GAPA;

    //// === Interlobe leakage ===
    int sliceLen = 2 * arcAngle + archiAngle + 1;
    std::vector<double> contact_x_interlobe(contact_x.begin() + cuspAngle,
        contact_x.begin() + cuspAngle + sliceLen);
    std::vector<double> contact_y_interlobe(contact_y.begin() + cuspAngle,
        contact_y.begin() + cuspAngle + sliceLen);
    // In Python: contact_z_interlobe = zcoord
    std::vector<double> contact_z_interlobe = zcoord;

    // --- Compute differences (like np.diff) ---
    Eigen::VectorXd interlobedx = vectorToEigen(contact_x_interlobe);
    Eigen::VectorXd interlobedy = vectorToEigen(contact_y_interlobe);
    Eigen::VectorXd interlobedz = vectorToEigen(contact_z_interlobe);


    Eigen::VectorXd vec_dx = HelperFunctions::diff(interlobedx);
    Eigen::VectorXd vec_dy = HelperFunctions::diff(interlobedy);
    Eigen::VectorXd vec_dz = HelperFunctions::diff(interlobedz);
    int offset = 720;
    std::vector<double> interlobeArea; // will store computed leakage areas
    int i_index = 0, j_index = 0;
    // Loop: rotated_angle in [offset+1, rotorWrap+720)
    for (int rotated_angle = 1 + offset; rotated_angle < rotorWrap + 358; ++rotated_angle) {
        double areaTemp = 0.0;
        if (rotated_angle < 720 + 360) {
            int lenSlice = rotated_angle - offset - 1;  // number of elements in first two slices
            Eigen::VectorXd slice_dx = vec_dx.segment(0, lenSlice);
            Eigen::VectorXd slice_dy = vec_dy.segment(0, lenSlice);
            int lenDz = i_index; // note: in first iteration when rotated_angle == 721, lenSlice==0, i_index==0.
            Eigen::VectorXd slice_dz = vec_dz.segment(720, lenDz);
            // Use the common length (should be equal if code is consistent)                
            Eigen::VectorXd sumVec = ((slice_dx.array().square() + slice_dy.array().square() + slice_dz.array().square()).sqrt());
            areaTemp = sumVec.sum();
            interlobeArea.push_back(areaTemp);
            i_index++;
        }
        else if (rotated_angle < rotorWrap) {
            int startDz = i_index + offset - 358;
            int segLen = i_index + 720 - 358;

			Eigen::VectorXd slice_dx = vec_dx;
            Eigen::VectorXd slice_dy = vec_dy;
            Eigen::VectorXd slice_dz = vec_dz.segment(segLen, 358);
            Eigen::VectorXd sumVec = ((slice_dx.array().square() + slice_dy.array().square() + slice_dz.array().square()).sqrt());
            areaTemp = sumVec.sum();
            //std::cout << "areaTemp: " << areaTemp << std::endl;
            interlobeArea.push_back(areaTemp);
            i_index++;
        }
        else {
            Eigen::VectorXd slice_dx = vec_dx.segment(j_index, 358 - j_index);
            Eigen::VectorXd slice_dy = vec_dy.segment(j_index, 358 - j_index);
            Eigen::VectorXd slice_dz = vec_dz.segment(i_index, 358 - j_index);
            Eigen::VectorXd sumVec = ((slice_dx.array().square() + slice_dy.array().square() + slice_dz.array().square()).sqrt());
            areaTemp = sumVec.sum();
            interlobeArea.push_back(areaTemp);
            //std::cout << "areaTemp: " << 358 - j_index << std::endl;
            //std::cout << "i: " << rotorWrap + 360 - rotated_angle << std::endl;
            j_index++;
        }
    }

    // Finally, prepend a zero vector of length 'offset' and multiply every element by GAPI.

    //std::vector<double> interlobeAreaFull(offset, 0.0);
    //interlobeAreaFull.insert(interlobeAreaFull.end(), interlobeArea.begin(), interlobeArea.end());
    //for (auto& val : interlobeAreaFull) { val *= GAPI; }
    // print interlobeArea

    Eigen::VectorXd zeros = Eigen::VectorXd::Zero(720);
    Eigen::VectorXd areaVec(interlobeArea.size());
    for (size_t idx = 0; idx < interlobeArea.size(); ++idx)
        areaVec(idx) = interlobeArea[idx];
    //Eigen::VectorXd interlobeAreaFull = concat(zeros, areaVec);
    Eigen::VectorXd interlobeAreaFull(zeros.size() + areaVec.size());
    interlobeAreaFull << zeros, areaVec;
    interlobeAreaFull *= GAPI;

    // === Radial leakage ===
    // angleCircle = range(cuspAngle, 360 - cuspAngle)
    int nCircle = 360 - 2 * cuspAngle;
    Eigen::VectorXd contact_x_circle(nCircle), contact_y_circle(nCircle);
    for (int idx = 0; idx < nCircle; idx++) {
        int angle = cuspAngle + idx;
        double rad = angle * PI / 180.0;
        contact_x_circle(idx) = D / 2 * std::cos(rad);
        contact_y_circle(idx) = D / 2 * std::sin(rad);
    }
    // Use first nCircle values of zcoordVec for the circle
    Eigen::VectorXd zcoord_circle = zcoordVec.head(nCircle);
    Eigen::VectorXd diff_x_circle = contact_x_circle.tail(nCircle - 1) - contact_x_circle.head(nCircle - 1);
    Eigen::VectorXd diff_y_circle = contact_y_circle.tail(nCircle - 1) - contact_y_circle.head(nCircle - 1);
    Eigen::VectorXd diff_z_circle = zcoord_circle.tail(nCircle - 1) - zcoord_circle.head(nCircle - 1);
    Eigen::VectorXd circleLength = ((diff_x_circle.array().square() +
        diff_y_circle.array().square() +
        diff_z_circle.array().square()).sqrt()).matrix();

    // Tip leakage arrays (gate and main)
    std::vector<double> TipAreaGate, TipAreaMain;
    i_index = 0; j_index = 0;
    // First loop for TipAreaGate over rotated_angle in [cuspAngle, rotorWrap+360-cuspAngle)
    for (int rotated_angle = cuspAngle; rotated_angle < rotorWrap + 360 - cuspAngle; rotated_angle++) {
        if (rotated_angle < 360 - cuspAngle) {
            int len = rotated_angle - cuspAngle;
            double sumVal = (len > 0) ? circleLength.head(len).sum() : 0.0;
            TipAreaGate.push_back(sumVal);
        }
        else if (rotated_angle < rotorWrap + cuspAngle) { // transition
            if (i_index + nCircle <= zcoordVec.size()) {
                Eigen::VectorXd zseg = zcoordVec.segment(i_index, nCircle);
                Eigen::VectorXd diff_z_seg = zseg.tail(nCircle - 1) - zseg.head(nCircle - 1);
                Eigen::VectorXd gateSegment = ((diff_x_circle.array().square() +
                    diff_y_circle.array().square() +
                    diff_z_seg.array().square()).sqrt()).matrix();
                TipAreaGate.push_back(gateSegment.sum());
            }
            else {
                TipAreaGate.push_back(0.0);
            }
            i_index++;
        }
        else {
            // Use tail of contact_x_circle etc.
            int rem = nCircle - j_index;
            if (rem > 1 && rotorWrap - nCircle + j_index - 1 >= 0 && (rotorWrap - 1) < zcoordVec.size()) {
                Eigen::VectorXd cx = contact_x_circle.tail(rem);
                Eigen::VectorXd cy = contact_y_circle.tail(rem);
                Eigen::VectorXd diff_cx = cx.tail(cx.size() - 1) - cx.head(cx.size() - 1);
                Eigen::VectorXd diff_cy = cy.tail(cy.size() - 1) - cy.head(cy.size() - 1);
                int start_z = rotorWrap - nCircle + j_index - 1;
                int len_z = cx.size() - 1;
                if (start_z >= 0 && start_z + len_z <= zcoordVec.size()) {
                    Eigen::VectorXd zseg = zcoordVec.segment(start_z, len_z);
                    Eigen::VectorXd diff_z_seg = zseg.tail(len_z - 1) - zseg.head(len_z - 1);

                    Eigen::VectorXd gateSegment = ((diff_cx.array().square() +
                        diff_cy.array().square() +
                        diff_z_seg.array().square()).sqrt()).matrix();
                    TipAreaGate.push_back(gateSegment.sum());
                }
                else {
                    TipAreaGate.push_back(0.0);
                }
            }
            else {
                TipAreaGate.push_back(0.0);
            }
            j_index++;
        }
    }
    // Next loop for TipAreaMain over rotated_angle in [arcAngle+cuspAngle, rotorWrap+360+arcAngle-cuspAngle)
    i_index = 0; j_index = 0;
    for (int rotated_angle = arcAngle + cuspAngle; rotated_angle < rotorWrap + 360 + arcAngle - cuspAngle; rotated_angle++) {
        if (rotated_angle < 360 + arcAngle - cuspAngle) {
            int len = rotated_angle - arcAngle - cuspAngle;
            double sumVal = (len > 0) ? circleLength.head(len).sum() : 0.0;
            TipAreaMain.push_back(sumVal);
        }
        else if (rotated_angle < rotorWrap + arcAngle + cuspAngle) {
            if (i_index + nCircle <= zcoordVec.size()) {
                Eigen::VectorXd zseg = zcoordVec.segment(i_index, nCircle);
                Eigen::VectorXd diff_z_seg = zseg.tail(nCircle - 1) - zseg.head(nCircle - 1);
                Eigen::VectorXd mainSegment = ((diff_x_circle.array().square() +
                    diff_y_circle.array().square() +
                    diff_z_seg.array().square()).sqrt()).matrix();
                TipAreaMain.push_back(mainSegment.sum());
            }
            else {
                TipAreaMain.push_back(0.0);
            }
            i_index++;
        }
        else {
            int rem = nCircle - j_index;
            if (rem > 1 && rotorWrap - nCircle + j_index - 1 >= 0 && (rotorWrap - 1) < zcoordVec.size()) {
                Eigen::VectorXd cx = contact_x_circle.tail(rem);
                Eigen::VectorXd cy = contact_y_circle.tail(rem);
                Eigen::VectorXd diff_cx = cx.tail(cx.size() - 1) - cx.head(cx.size() - 1);
                Eigen::VectorXd diff_cy = cy.tail(cy.size() - 1) - cy.head(cy.size() - 1);
                int start_z = rotorWrap - nCircle + j_index - 1;
                int len_z = cx.size() - 1;
                if (start_z >= 0 && start_z + len_z <= zcoordVec.size()) {
                    Eigen::VectorXd zseg = zcoordVec.segment(start_z, len_z);
                    Eigen::VectorXd diff_z_seg = zseg.tail(len_z - 1) - zseg.head(len_z - 1);
                    Eigen::VectorXd mainSegment = ((diff_cx.array().square() +
                        diff_cy.array().square() +
                        diff_z_seg.array().square()).sqrt()).matrix();
                    TipAreaMain.push_back(mainSegment.sum());
                }
                else {
                    TipAreaMain.push_back(0.0);
                }
            }
            else {
                TipAreaMain.push_back(0.0);
            }
            j_index++;
        }
    }

    // === Compose Tip Leakage vectors ===
    // leadingTipAreaGate: prepend cuspAngle zeros to TipAreaGate then multiply by GAPR.
    int len_leadingGate = cuspAngle + static_cast<int>(TipAreaGate.size());
    Eigen::VectorXd leadingTipAreaGate(len_leadingGate);
    leadingTipAreaGate.head(cuspAngle).setZero();
    {
        Eigen::VectorXd tmp = Eigen::Map<Eigen::VectorXd>(TipAreaGate.data(), TipAreaGate.size());
        leadingTipAreaGate.segment(cuspAngle, tmp.size()) = tmp;
    }
    leadingTipAreaGate *= GAPR;

    // trailingTipAreaGate: prepend (cuspAngle + 360 - arcAngle) zeros
    int len_trailingGate = (cuspAngle + 360 - arcAngle) + static_cast<int>(TipAreaGate.size());
    Eigen::VectorXd trailingTipAreaGate(len_trailingGate);
    trailingTipAreaGate.head(cuspAngle + 360 - arcAngle).setZero();
    {
        Eigen::VectorXd tmp = Eigen::Map<Eigen::VectorXd>(TipAreaGate.data(), TipAreaGate.size());
        trailingTipAreaGate.segment(cuspAngle + 360 - arcAngle, tmp.size()) = tmp;
    }
    trailingTipAreaGate *= GAPR;

    // leadingTipAreaMain: prepend (arcAngle + cuspAngle) zeros
    int len_leadingMain = (arcAngle + cuspAngle) + static_cast<int>(TipAreaMain.size());
    Eigen::VectorXd leadingTipAreaMain(len_leadingMain);
    leadingTipAreaMain.head(arcAngle + cuspAngle).setZero();
    {
        Eigen::VectorXd tmp = Eigen::Map<Eigen::VectorXd>(TipAreaMain.data(), TipAreaMain.size());
        leadingTipAreaMain.segment(arcAngle + cuspAngle, tmp.size()) = tmp;
    }
    leadingTipAreaMain *= GAPR;

    // trailingTipAreaMain: prepend (arcAngle + cuspAngle + 360 - arcAngle) zeros
    int len_trailingMain = (cuspAngle + 360) + static_cast<int>(TipAreaMain.size());
    Eigen::VectorXd trailingTipAreaMain(len_trailingMain);
    trailingTipAreaMain.head(cuspAngle + 360).setZero();
    {
        Eigen::VectorXd tmp = Eigen::Map<Eigen::VectorXd>(TipAreaMain.data(), TipAreaMain.size());
        trailingTipAreaMain.segment(cuspAngle + 360, tmp.size()) = tmp;
    }
    trailingTipAreaMain *= GAPR;

    // === Compose final leakageArea matrix ===
    int totalRows = rotorWrap + 720;
    Eigen::MatrixXd leakageArea = Eigen::MatrixXd::Zero(totalRows, 7);
    // Column assignments (use head(n) to assign the top n rows)
    {
        Eigen::VectorXd vec = leadingTipAreaMain;
        leakageArea.block(0, 0, vec.size(), 1) = vec;
    }
    {
        Eigen::VectorXd vec = trailingTipAreaMain;
        leakageArea.block(0, 1, vec.size(), 1) = vec;
    }
    {
        Eigen::VectorXd vec = leadingTipAreaGate;
        leakageArea.block(0, 2, vec.size(), 1) = vec;
    }
    {
        Eigen::VectorXd vec = trailingTipAreaGate;
        leakageArea.block(0, 3, vec.size(), 1) = vec;
    }
    {
        Eigen::VectorXd vec = Eigen::Map<Eigen::VectorXd>(interlobeAreaFull.data(), interlobeAreaFull.size());
        leakageArea.block(0, 4, vec.size(), 1) = vec;
    }
    leakageArea.block(0, 5, axialAreaLeading.size(), 1) = axialAreaLeading;
    leakageArea.block(0, 6, axialAreaTrailing.size(), 1) = axialAreaTrailing;

    return leakageArea;
}