#include "crDevPhysics.h"

#include <SDL3/SDL_log.h>

#include "crApp.h"
#include "crBatchedLines3D.h"
#include "crGraphics.h"
#include "crMath.h"
#include "crPhysics.h"
#include "crPostProcess.h"

static constexpr int32_t CIRCLE_SEGMENTS = 16;   // per great circle — 3 circles per sphere

void crDevPhysics::Init( crApp* app )
{
    _debugDraw = b3DefaultDebugDraw();

    _debugDraw.DrawShapeFcn     = &crDevPhysics::DrawShape;
    _debugDraw.DrawSegmentFcn   = &crDevPhysics::DrawSegment;
    _debugDraw.DrawTransformFcn = &crDevPhysics::DrawTransform;
    _debugDraw.DrawPointFcn     = &crDevPhysics::DrawPoint;
    _debugDraw.DrawSphereFcn    = &crDevPhysics::DrawSphere;
    _debugDraw.DrawCapsuleFcn   = &crDevPhysics::DrawCapsule;
    _debugDraw.DrawBoundsFcn    = &crDevPhysics::DrawBounds;
    _debugDraw.DrawBoxFcn       = &crDevPhysics::DrawBox;
    _debugDraw.DrawStringFcn    = &crDevPhysics::DrawString;

    _debugDraw.drawShapes = true;
    _debugDraw.drawJoints = false;
    _debugDraw.drawBounds = false;

    _debugDraw.context = this;

    _lineBatch = new crBatchedLines3D;
    _lineBatch->Init();

    _app = app;
}
void crDevPhysics::Cleanup()
{
    if( _lineBatch != nullptr )
    {
        _lineBatch->Cleanup();
        delete _lineBatch;
        _lineBatch = nullptr;
    }

    _app = nullptr;
}
void crDevPhysics::TryRenderDebugDraw()
{
    if( _debugDrawEnabled == false )
        return;

    _lineBatch->Begin();

    b3World_Draw( _app->physics->World(), &_debugDraw, UINT64_MAX );

    // post-PP overlay: no hardware depth here — the shader tests against the scene depth texture
    const GLuint program = _app->graphics->Program( EShader::LINE_3D_OCCLUDED );

    glUseProgram( program );
    glUniform1i( glGetUniformLocation( program, "u_sceneDepth" ), 0 );

    GLint viewport[ 4 ];
    glGetIntegerv( GL_VIEWPORT, viewport );
    glUniform2f( glGetUniformLocation( program, "u_invViewport" ), 1.0f / static_cast<float>( viewport[ 2 ] ), 1.0f / static_cast<float>( viewport[ 3 ] ) );

    glActiveTexture( GL_TEXTURE0 );
    glBindTexture( GL_TEXTURE_2D, _app->graphics->PostProcess()->SceneDepthTexture() );

    _lineBatch->Flush( program, false );

    glBindTexture( GL_TEXTURE_2D, 0 );
}

bool crDevPhysics::GetDebugDraw() const
{
    return _debugDrawEnabled;
}
void crDevPhysics::SetDebugDraw( bool enable )
{
    _debugDrawEnabled = enable;

    if( enable == false )
        _lineBatch->ClearUnusedMemory();   // ~8MB CPU+GPU at 3000 bodies — release while dormant
}

/*static*/ void* crDevPhysics::CreateDebugShape( const b3DebugShape* debugShape, void* ctx )
{
    ( void )ctx;

    DebugShape* ds = static_cast<DebugShape*>( SDL_malloc( sizeof( DebugShape ) ) );
    SDL_memset( ds, 0, sizeof( DebugShape ) );
    ds->type = debugShape->type;

    switch( debugShape->type )
    {
    case b3_sphereShape:
        ds->sphere = *debugShape->sphere;
        break;

    case b3_capsuleShape:
        ds->capsule = *debugShape->capsule;
        break;

    case b3_hullShape:
    {
        const b3HullData*     hull  = debugShape->hull;
        const b3Vec3*         pts   = b3GetHullPoints( hull );
        const b3HullHalfEdge* edges = b3GetHullEdges( hull );
        if( ( pts == nullptr ) || ( edges == nullptr ) )
            break;   // hullEdgeVerts stays 0 — nothing to draw

        const int32_t uniqueEdges = hull->edgeCount / 2;   // edgeCount is the half-edge count
        ds->hullEdges = static_cast<float3*>( SDL_malloc( static_cast<size_t>( uniqueEdges ) * 2 * sizeof( float3 ) ) );

        int32_t n = 0;
        for( int32_t i = 0; i < hull->edgeCount; ++i )
        {
            if( i < edges[ i ].twin )   // visit each undirected edge once
            {
                const b3Vec3 a = pts[ edges[ i ].origin ];
                const b3Vec3 b = pts[ edges[ edges[ i ].twin ].origin ];
                ds->hullEdges[ n ]     = float3( a.x, a.y, a.z );
                ds->hullEdges[ n + 1 ] = float3( b.x, b.y, b.z );
                n += 2;
            }
        }
        ds->hullEdgeVerts = n;
        break;
    }

    default:   // mesh / compound / height field — no wireframe support yet
        SDL_LogWarn( CR_LOG_CATEGORY_PHYSICS, "crDevPhysics: unsupported debug shape type( %d ) — skipped", static_cast<int32_t>( debugShape->type ) );
        SDL_free( ds );
        return nullptr;   // box3d treats null as "skip draw"
    }

    return ds;
}
/*static*/ void crDevPhysics::DestroyDebugShape( void* userShape, void* ctx )
{
    ( void )ctx;

    DebugShape* ds = static_cast<DebugShape*>( userShape );
    if( ds == nullptr )
        return;

    SDL_free( ds->hullEdges );
    SDL_free( ds );
}

/*static*/ bool crDevPhysics::DrawShape( void* userShape, b3WorldTransform transform, b3HexColor color, void* ctx )
{
    if( userShape == nullptr )
        return true;

    crDevPhysics*     self  = static_cast<crDevPhysics*>( ctx );
    crBatchedLines3D* batch = self->_lineBatch;
    const DebugShape* ds    = static_cast<const DebugShape*>( userShape );

    switch( ds->type )
    {
    case b3_sphereShape:
    {
        const b3Vec3 c = b3TransformPoint( transform, ds->sphere.center );
        DrawSphere( c, ds->sphere.radius, color, 1.0f, ctx );
        break;
    }
    case b3_capsuleShape:
    {
        const b3Vec3 p1 = b3TransformPoint( transform, ds->capsule.center1 );
        const b3Vec3 p2 = b3TransformPoint( transform, ds->capsule.center2 );
        DrawCapsule( p1, p2, ds->capsule.radius, color, 1.0f, ctx );
        break;
    }
    case b3_hullShape:
    {
        const color4 c = color;
        for( int32_t i = 0; i < ds->hullEdgeVerts; i += 2 )
        {
            const b3Vec3 a = b3TransformPoint( transform, b3Vec3{ ds->hullEdges[ i ].x, ds->hullEdges[ i ].y, ds->hullEdges[ i ].z } );
            const b3Vec3 b = b3TransformPoint( transform, b3Vec3{ ds->hullEdges[ i + 1 ].x, ds->hullEdges[ i + 1 ].y, ds->hullEdges[ i + 1 ].z } );
            batch->Line( float3( a.x, a.y, a.z ), float3( b.x, b.y, b.z ), c );
        }
        break;
    }
    default:
        break;
    }

    return true;
}
/*static*/ void crDevPhysics::DrawSegment( b3Pos p1, b3Pos p2, b3HexColor color, void* ctx )
{
    crBatchedLines3D* batch = static_cast<crDevPhysics*>( ctx )->_lineBatch;

    batch->Line( float3( p1.x, p1.y, p1.z ), float3( p2.x, p2.y, p2.z ), color4( color ) );
}
/*static*/ void crDevPhysics::DrawTransform( b3WorldTransform transform, void* ctx )
{
    crBatchedLines3D* batch = static_cast<crDevPhysics*>( ctx )->_lineBatch;

    const float  k = 0.5f;
    const float3 o = float3( transform.p.x, transform.p.y, transform.p.z );

    const b3Vec3 ax = b3RotateVector( transform.q, b3Vec3{ k, 0.0f, 0.0f } );
    const b3Vec3 ay = b3RotateVector( transform.q, b3Vec3{ 0.0f, k, 0.0f } );
    const b3Vec3 az = b3RotateVector( transform.q, b3Vec3{ 0.0f, 0.0f, k } );

    batch->Line( o, float3( o.x + ax.x, o.y + ax.y, o.z + ax.z ), color4( 1.0f, 0.0f, 0.0f, 1.0f ) );
    batch->Line( o, float3( o.x + ay.x, o.y + ay.y, o.z + ay.z ), color4( 0.0f, 1.0f, 0.0f, 1.0f ) );
    batch->Line( o, float3( o.x + az.x, o.y + az.y, o.z + az.z ), color4( 0.0f, 0.0f, 1.0f, 1.0f ) );
}
/*static*/ void crDevPhysics::DrawPoint( b3Pos p, float size, b3HexColor color, void* ctx )
{
    crBatchedLines3D* batch = static_cast<crDevPhysics*>( ctx )->_lineBatch;

    const float  h = size * 0.01f;   // size is nominally pixels — approximate with a small world-space cross
    const color4 c = color;

    batch->Line( float3( p.x - h, p.y, p.z ), float3( p.x + h, p.y, p.z ), c );
    batch->Line( float3( p.x, p.y - h, p.z ), float3( p.x, p.y + h, p.z ), c );
    batch->Line( float3( p.x, p.y, p.z - h ), float3( p.x, p.y, p.z + h ), c );
}
/*static*/ void crDevPhysics::DrawSphere( b3Pos p, float radius, b3HexColor color, float alpha, void* ctx )
{
    crBatchedLines3D* batch = static_cast<crDevPhysics*>( ctx )->_lineBatch;

    color4 c = color;
    c.a = alpha;

    const float step = ( 2.0f * crMath::PI ) / static_cast<float>( CIRCLE_SEGMENTS );

    float2 prev = crMath::CosSin( 0.0f );
    for( int32_t i = 1; i <= CIRCLE_SEGMENTS; ++i )
    {
        const float2 cur = crMath::CosSin( step * static_cast<float>( i ) );

        // XY plane
        batch->Line( float3( p.x + ( prev.x * radius ), p.y + ( prev.y * radius ), p.z ),
                     float3( p.x + ( cur.x * radius ),  p.y + ( cur.y * radius ),  p.z ), c );
        // XZ plane
        batch->Line( float3( p.x + ( prev.x * radius ), p.y, p.z + ( prev.y * radius ) ),
                     float3( p.x + ( cur.x * radius ),  p.y, p.z + ( cur.y * radius ) ), c );
        // YZ plane
        batch->Line( float3( p.x, p.y + ( prev.x * radius ), p.z + ( prev.y * radius ) ),
                     float3( p.x, p.y + ( cur.x * radius ),  p.z + ( cur.y * radius ) ), c );

        prev = cur;
    }
}
/*static*/ void crDevPhysics::DrawCapsule( b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* ctx )
{
    crBatchedLines3D* batch = static_cast<crDevPhysics*>( ctx )->_lineBatch;

    color4 c = color;
    c.a = alpha;

    batch->Line( float3( p1.x, p1.y, p1.z ), float3( p2.x, p2.y, p2.z ), c );

    DrawSphere( p1, radius, color, alpha, ctx );   // end caps as full spheres — cheap approximation
    DrawSphere( p2, radius, color, alpha, ctx );
}
/*static*/ void crDevPhysics::DrawBounds( b3AABB aabb, b3HexColor color, void* ctx )
{
    crBatchedLines3D* batch = static_cast<crDevPhysics*>( ctx )->_lineBatch;

    const color4 c  = color;
    const float3 lo = float3( aabb.lowerBound.x, aabb.lowerBound.y, aabb.lowerBound.z );
    const float3 hi = float3( aabb.upperBound.x, aabb.upperBound.y, aabb.upperBound.z );

    // bottom rectangle, top rectangle, verticals
    batch->Line( float3( lo.x, lo.y, lo.z ), float3( hi.x, lo.y, lo.z ), c );
    batch->Line( float3( hi.x, lo.y, lo.z ), float3( hi.x, lo.y, hi.z ), c );
    batch->Line( float3( hi.x, lo.y, hi.z ), float3( lo.x, lo.y, hi.z ), c );
    batch->Line( float3( lo.x, lo.y, hi.z ), float3( lo.x, lo.y, lo.z ), c );

    batch->Line( float3( lo.x, hi.y, lo.z ), float3( hi.x, hi.y, lo.z ), c );
    batch->Line( float3( hi.x, hi.y, lo.z ), float3( hi.x, hi.y, hi.z ), c );
    batch->Line( float3( hi.x, hi.y, hi.z ), float3( lo.x, hi.y, hi.z ), c );
    batch->Line( float3( lo.x, hi.y, hi.z ), float3( lo.x, hi.y, lo.z ), c );

    batch->Line( float3( lo.x, lo.y, lo.z ), float3( lo.x, hi.y, lo.z ), c );
    batch->Line( float3( hi.x, lo.y, lo.z ), float3( hi.x, hi.y, lo.z ), c );
    batch->Line( float3( hi.x, lo.y, hi.z ), float3( hi.x, hi.y, hi.z ), c );
    batch->Line( float3( lo.x, lo.y, hi.z ), float3( lo.x, hi.y, hi.z ), c );
}
/*static*/ void crDevPhysics::DrawBox( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* ctx )
{
    crBatchedLines3D* batch = static_cast<crDevPhysics*>( ctx )->_lineBatch;

    const color4 c = color;

    float3 corners[ 8 ];
    for( int32_t i = 0; i < 8; ++i )
    {
        const b3Vec3 local = { ( ( i & 1 ) != 0 ) ? extents.x : -extents.x,
                               ( ( i & 2 ) != 0 ) ? extents.y : -extents.y,
                               ( ( i & 4 ) != 0 ) ? extents.z : -extents.z };
        const b3Vec3 world = b3TransformPoint( transform, local );
        corners[ i ] = float3( world.x, world.y, world.z );
    }

    // 12 edges: pairs of corners differing by exactly one bit
    const int32_t edges[ 12 ][ 2 ] =
    {
        { 0, 1 }, { 2, 3 }, { 4, 5 }, { 6, 7 },   // x
        { 0, 2 }, { 1, 3 }, { 4, 6 }, { 5, 7 },   // y
        { 0, 4 }, { 1, 5 }, { 2, 6 }, { 3, 7 },   // z
    };
    for( int32_t e = 0; e < 12; ++e )
        batch->Line( corners[ edges[ e ][ 0 ] ], corners[ edges[ e ][ 1 ] ], c );
}
/*static*/ void crDevPhysics::DrawString( b3Pos p, const char* s, b3HexColor color, void* ctx )
{
    ( void )p;
    ( void )s;
    ( void )color;
    ( void )ctx;
}