Capsule::Capsule(int subdivisions, int tesselation, float height, float radius, int cap_subdivisions) :
    subdivisions(subdivisions),
    tesselation(tesselation),
    height(height),
    radius(radius),
    cap_subdivisions(cap_subdivisions)
{
    auto radialSegments = tesselation;
    auto heightSegments = subdivisions;

    auto radiusTop = radius;
    auto radiusBottom = radius;

    auto heightMinusCaps = height - (radiusTop + radiusBottom);

    auto thetaStart = 0.0f;
    constexpr auto thetaLength = 2.0f * glm::pi<float>();

    auto capsTopSegments = cap_subdivisions;
    auto capsBottomSegments = cap_subdivisions;

    auto alpha = glm::acos((radiusBottom - radiusTop) / height);

    std::vector<std::vector<unsigned int>> index_array;
    int index = 0;

    auto halfHeight = heightMinusCaps * 0.5f;

    constexpr auto pi2 = glm::pi<float>() * 0.5f;

    int x = 0;
    int y = 0;
    auto normal = glm::vec3(0.0f);
    auto vertex = glm::vec3(0.0f);

    auto cosAlpha = glm::cos(alpha);
    auto sinAlpha = glm::sin(alpha);

    auto cone_a = glm::vec2(radiusTop * sinAlpha, halfHeight + radiusTop * cosAlpha);
    auto cone_b = glm::vec2(radiusBottom * sinAlpha, -halfHeight + radiusBottom * cosAlpha);
    auto coneLength = glm::length(cone_a - cone_b);

    auto vl = radiusTop * alpha + coneLength + radiusBottom * (pi2 - alpha);

    float v = 0.0f;

    for (y = 0; y <= capsTopSegments; y++) {
        std::vector<unsigned int> index_row;

        auto a = pi2 - alpha * ((float)y / capsTopSegments);

        v += (radiusTop * alpha) / capsTopSegments;

        auto cosA = glm::cos(a);
        auto sinA = glm::sin(a);

        auto _radius = cosA * radiusTop;

        for (x = 0; x <= radialSegments; ++x) {
            auto u = (float)x / radialSegments;
            auto theta = u * thetaLength + thetaStart;
            auto sinTheta = glm::sin(theta);
            auto cosTheta = glm::cos(theta);

            vertex.x = _radius * sinTheta;
            vertex.y = halfHeight + sinA * radiusTop;
            vertex.z = _radius * cosTheta;
            vertices.push_back(vertex.x);
            vertices.push_back(vertex.y);
            vertices.push_back(vertex.z);

            normal.x = cosA * sinTheta;
            normal.y = sinA;
            normal.z = cosA * cosTheta;
            normals.push_back(normal.x);
            normals.push_back(normal.y);
            normals.push_back(normal.z);

            uvs.push_back(u);
            uvs.push_back(v / vl);

            index_row.push_back(index);
            index++;
        }

        index_array.push_back(index_row);
    }

    auto coneHeight = height - radiusTop - radiusBottom + cosAlpha * radiusTop - cosAlpha * radiusBottom;
    auto slope = (sinAlpha * (radiusBottom - radiusTop)) / coneHeight;

    for (y = 1; y <= heightSegments; y++) {
        std::vector<unsigned int> index_row;

        v += coneLength / (float)heightSegments;

        auto _radius = sinAlpha * (((float)y * (radiusBottom - radiusTop)) / (float)heightSegments + radiusTop);

        for (x = 0; x <= radialSegments; x++) {
            auto u = (float)x / radialSegments;
            auto theta = u * thetaLength + thetaStart;
            auto sinTheta = glm::sin(theta);
            auto cosTheta = glm::cos(theta);

            vertex.x = _radius * sinTheta;
            vertex.y = halfHeight + cosAlpha * radiusTop - ((float)y * coneHeight) / (float)heightSegments;
            vertex.z = _radius * cosTheta;
            vertices.push_back(vertex.x);
            vertices.push_back(vertex.y);
            vertices.push_back(vertex.z);

            normal.x = sinTheta;
            normal.y = slope;
            normal.z = cosTheta;
            normal = glm::normalize(normal);
            normals.push_back(normal.x);
            normals.push_back(normal.y);
            normals.push_back(normal.z);

            uvs.push_back(u);
            uvs.push_back(v / vl);

            index_row.push_back(index);

            index++;
        }

        index_array.push_back(index_row);
    }

    for (y = 1; y <= capsBottomSegments; y++) {
        std::vector<unsigned int> index_row;

        auto a = pi2 - alpha - (glm::pi<float>() - alpha) * ((float)y / capsBottomSegments);
        v += (radiusBottom * alpha) / capsBottomSegments;
        auto cosA = glm::cos(a);
        auto sinA = glm::sin(a);

        auto _radius = cosA * radiusBottom;

        for (x = 0; x <= radialSegments; ++x) {
            auto u = (float)x / radialSegments;
            auto theta = u * thetaLength + thetaStart;
            auto sinTheta = glm::sin(theta);
            auto cosTheta = glm::cos(theta);

            vertex.x = _radius * sinTheta;
            vertex.y = -halfHeight + sinA * radiusBottom;
            vertex.z = _radius * cosTheta;
            vertices.push_back(vertex.x);
            vertices.push_back(vertex.y);
            vertices.push_back(vertex.z);

            normal.x = cosA * sinTheta;
            normal.y = sinA;
            normal.z = cosA * cosTheta;
            normals.push_back(normal.x);
            normals.push_back(normal.y);
            normals.push_back(normal.z);

            uvs.push_back(u);
            uvs.push_back(v / vl);
            index_row.push_back(index);

            index++;
        }

        index_array.push_back(index_row);
    }

    for (x = 0; x < radialSegments; ++x) {
        for (y = 0; y < capsTopSegments + heightSegments + capsBottomSegments; y++) {
            auto i1 = index_array[y][x];
            auto i2 = index_array[y + 1][x];
            auto i3 = index_array[y + 1][x + 1];
            auto i4 = index_array[y][x + 1];

            indices.push_back(i1);
            indices.push_back(i2);
            indices.push_back(i4);

            indices.push_back(i2);
            indices.push_back(i3);
            indices.push_back(i4);
        }
    }

    std::reverse(indices.begin(), indices.end());
}