//
//  CCDrawNode3D3D.cpp
//  Redream2
//
//  Created by 徐俊杰 on 2021/9/6.
//

#include "2d/CCDrawNode3D.h"
#include "base/CCEventType.h"
#include "base/CCConfiguration.h"
#include "renderer/CCRenderer.h"
#include "renderer/ccGLStateCache.h"
#include "renderer/CCGLProgramState.h"
#include "renderer/CCGLProgramCache.h"
#include "base/CCDirector.h"
#include "base/CCEventListenerCustom.h"
#include "base/CCEventDispatcher.h"
#include "2d/CCActionCatmullRom.h"
#include "platform/CCGL.h"

NS_CC_BEGIN

struct CC_DLL V3F_C4B_T2F_Triangle
{
    V3F_C4B_T2F a;
    V3F_C4B_T2F b;
    V3F_C4B_T2F c;
};

// Vec3 == CGPoint in 32-bits, but not in 64-bits (OS X)
// that's why the "v3f" functions are needed
static Vec3 v3fzero(0.0f,0.0f,0.0f);

static inline Vec3 v3f(float x, float y, float z)
{
    Vec3 ret(x, y, z);
    return ret;
}

static inline Vec3 v3fadd(const Vec3 &v0, const Vec3 &v1)
{
    return v3f(v0.x+v1.x, v0.y+v1.y, v0.z+v1.z);
}

static inline Vec3 v3fsub(const Vec3 &v0, const Vec3 &v1)
{
    return v3f(v0.x-v1.x, v0.y-v1.y, v0.z-v1.z);
}

static inline Vec3 v3fmult(const Vec3 &v, float s)
{
    return v3f(v.x * s, v.y * s, v.z * s);
}

static inline Vec3 v3fperp(const Vec3 &p0)
{
    return v3f(-p0.y, p0.x, p0.z);
}

static inline Vec3 v3fneg(const Vec3 &p0)
{
    return v3f(-p0.x, -p0.y, -p0.z);
}

static inline float v3fdot(const Vec3 &p0, const Vec3 &p1)
{
    return  p0.x * p1.x + p0.y * p1.y + p0.z * p1.z;
}

static inline Vec3 v3fnormalize(const Vec3 &p)
{
    Vec3 r(p.x, p.y, p.z);
    r.normalize();
    return v3f(r.x, r.y, r.z);
}

static inline Vec3 __v3f(const Vec3 &v)
{
//#ifdef __LP64__
    return v3f(v.x, v.y, v.z);
// #else
//     return * ((Vec3*) &v);
// #endif
}

static inline Tex2F __t(const Vec3 &v)
{
    return Tex2F(v.x, v.y);
}

static Quaternion AxisAngleToQuaternionSafe (const Vec3& axis, float angle)
{
    Quaternion q;
    float mag = Magnitude (axis);
    if (mag > 0.000001F)
    {
        float halfAngle = angle * 0.5F;
        
        q.w = cos (halfAngle);

        float s = sin (halfAngle) / mag;
        q.x = s * axis.x;
        q.y = s * axis.y;
        q.z = s * axis.z;
        return q;
    }
    else
    {
        return Quaternion::identity ();
    }
}

static void setDiscSectionPoints( Vec3 dest[], int count, const Vec3& center, const Vec3& normal, const Vec3& from, float angle, float radius) {
    Vec3 fromn = Normalize(from);
    Quaternion r = AxisAngleToQuaternionSafe( normal, CC_DEGREES_TO_RADIANS(angle / (count - 1)) );
    Vec3 tangent = fromn * radius;
    for( int i = 0; i < count; i++) {
        dest[i] = center + tangent;
        tangent = RotateVectorByQuat (r, tangent);
    }
}

// implementation of DrawNode3D

DrawNode3D::DrawNode3D(GLfloat lineWidth, bool drawOnce)
: _vao(0)
, _vbo(0)
, _vaoGLPoint(0)
, _vboGLPoint(0)
, _vaoGLLine(0)
, _vboGLLine(0)
, _bufferCapacity(0)
, _bufferCount(0)
, _buffer(nullptr)
, _bufferCapacityGLPoint(0)
, _bufferCountGLPoint(0)
, _bufferGLPoint(nullptr)
, _bufferCapacityGLLine(0)
, _bufferCountGLLine(0)
, _bufferGLLine(nullptr)
, _dirty(false)
, _dirtyGLPoint(false)
, _dirtyGLLine(false)
, _drawOnce(drawOnce)
, _lineWidth(lineWidth)
, _defaultLineWidth(lineWidth)
{
    _blendFunc = BlendFunc::ALPHA_PREMULTIPLIED;
}

DrawNode3D::~DrawNode3D()
{
    free(_buffer);
    _buffer = nullptr;
    free(_bufferGLPoint);
    _bufferGLPoint = nullptr;
    free(_bufferGLLine);
    _bufferGLLine = nullptr;
    
    glDeleteBuffers(1, &_vbo);
    glDeleteBuffers(1, &_vboGLLine);
    glDeleteBuffers(1, &_vboGLPoint);
    _vbo = 0;
    _vboGLPoint = 0;
    _vboGLLine = 0;
    
    if (Configuration::getInstance()->supportsShareableVAO())
    {
        GL::bindVAO(0);
        glDeleteVertexArrays(1, &_vao);
        glDeleteVertexArrays(1, &_vaoGLLine);
        glDeleteVertexArrays(1, &_vaoGLPoint);
        _vao = _vaoGLLine = _vaoGLPoint = 0;
    }
}

DrawNode3D* DrawNode3D::create(GLfloat defaultLineWidth, bool drawOnce)
{
    DrawNode3D* ret = new (std::nothrow) DrawNode3D(defaultLineWidth, drawOnce);
    if (ret && ret->init())
    {
        ret->autorelease();
    }
    else
    {
        CC_SAFE_DELETE(ret);
    }
    
    return ret;
}

void DrawNode3D::ensureCapacity(int count)
{
    CCASSERT(count>=0, "capacity must be >= 0");
    
    if(_bufferCount + count > _bufferCapacity)
    {
        _bufferCapacity += MAX(_bufferCapacity, count);
        _buffer = (V3F_C4B_T2F*)realloc(_buffer, _bufferCapacity*sizeof(V3F_C4B_T2F));
    }
}

void DrawNode3D::ensureCapacityGLPoint(int count)
{
    CCASSERT(count>=0, "capacity must be >= 0");
    
    if(_bufferCountGLPoint + count > _bufferCapacityGLPoint)
    {
        _bufferCapacityGLPoint += MAX(_bufferCapacityGLPoint, count);
        _bufferGLPoint = (V3F_C4B_T2F*)realloc(_bufferGLPoint, _bufferCapacityGLPoint*sizeof(V3F_C4B_T2F));
    }
}

void DrawNode3D::ensureCapacityGLLine(int count)
{
    CCASSERT(count>=0, "capacity must be >= 0");
    
    if(_bufferCountGLLine + count > _bufferCapacityGLLine)
    {
        _bufferCapacityGLLine += MAX(_bufferCapacityGLLine, count);
        _bufferGLLine = (V3F_C4B_T2F*)realloc(_bufferGLLine, _bufferCapacityGLLine*sizeof(V3F_C4B_T2F));
    }
}

bool DrawNode3D::init()
{
    _blendFunc = BlendFunc::ALPHA_PREMULTIPLIED;

    setGLProgramState(GLProgramState::getOrCreateWithGLProgramName(GLProgram::SHADER_NAME_POSITION_LENGTH_TEXTURE_COLOR));
    
    ensureCapacity(512);
    ensureCapacityGLPoint(64);
    ensureCapacityGLLine(256);
    
    if (Configuration::getInstance()->supportsShareableVAO())
    {
        glGenVertexArrays(1, &_vao);
        GL::bindVAO(_vao);
        glGenBuffers(1, &_vbo);
        glBindBuffer(GL_ARRAY_BUFFER, _vbo);
        glBufferData(GL_ARRAY_BUFFER, sizeof(V3F_C4B_T2F)* _bufferCapacity, _buffer, GL_STREAM_DRAW);
        // vertex
        glEnableVertexAttribArray(GLProgram::VERTEX_ATTRIB_POSITION);
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_POSITION, 3, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, vertices));
        // color
        glEnableVertexAttribArray(GLProgram::VERTEX_ATTRIB_COLOR);
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_COLOR, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, colors));
        // texcoord
        glEnableVertexAttribArray(GLProgram::VERTEX_ATTRIB_TEX_COORD);
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_TEX_COORD, 2, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, texCoords));
        
        glGenVertexArrays(1, &_vaoGLLine);
        GL::bindVAO(_vaoGLLine);
        glGenBuffers(1, &_vboGLLine);
        glBindBuffer(GL_ARRAY_BUFFER, _vboGLLine);
        glBufferData(GL_ARRAY_BUFFER, sizeof(V3F_C4B_T2F)*_bufferCapacityGLLine, _bufferGLLine, GL_STREAM_DRAW);
        // vertex
        glEnableVertexAttribArray(GLProgram::VERTEX_ATTRIB_POSITION);
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_POSITION, 3, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, vertices));
        // color
        glEnableVertexAttribArray(GLProgram::VERTEX_ATTRIB_COLOR);
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_COLOR, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, colors));
        // texcoord
        glEnableVertexAttribArray(GLProgram::VERTEX_ATTRIB_TEX_COORD);
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_TEX_COORD, 2, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, texCoords));
        
        glGenVertexArrays(1, &_vaoGLPoint);
        GL::bindVAO(_vaoGLPoint);
        glGenBuffers(1, &_vboGLPoint);
        glBindBuffer(GL_ARRAY_BUFFER, _vboGLPoint);
        glBufferData(GL_ARRAY_BUFFER, sizeof(V3F_C4B_T2F)*_bufferCapacityGLPoint, _bufferGLPoint, GL_STREAM_DRAW);
        // vertex
        glEnableVertexAttribArray(GLProgram::VERTEX_ATTRIB_POSITION);
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_POSITION, 3, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, vertices));
        // color
        glEnableVertexAttribArray(GLProgram::VERTEX_ATTRIB_COLOR);
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_COLOR, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, colors));
        // Texture coord as pointsize
        glEnableVertexAttribArray(GLProgram::VERTEX_ATTRIB_TEX_COORD);
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_TEX_COORD, 2, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, texCoords));
        
        GL::bindVAO(0);
        glBindBuffer(GL_ARRAY_BUFFER, 0);
        
    }
    else
    {
        glGenBuffers(1, &_vbo);
        glBindBuffer(GL_ARRAY_BUFFER, _vbo);
        glBufferData(GL_ARRAY_BUFFER, sizeof(V3F_C4B_T2F)* _bufferCapacity, _buffer, GL_STREAM_DRAW);
        
        glGenBuffers(1, &_vboGLLine);
        glBindBuffer(GL_ARRAY_BUFFER, _vboGLLine);
        glBufferData(GL_ARRAY_BUFFER, sizeof(V3F_C4B_T2F)*_bufferCapacityGLLine, _bufferGLLine, GL_STREAM_DRAW);
        
        glGenBuffers(1, &_vboGLPoint);
        glBindBuffer(GL_ARRAY_BUFFER, _vboGLPoint);
        glBufferData(GL_ARRAY_BUFFER, sizeof(V3F_C4B_T2F)*_bufferCapacityGLPoint, _bufferGLPoint, GL_STREAM_DRAW);

        glBindBuffer(GL_ARRAY_BUFFER, 0);
    }
    
    CHECK_GL_ERROR_DEBUG();
    
    _dirty = true;
    _dirtyGLLine = true;
    _dirtyGLPoint = true;
    
#if CC_ENABLE_CACHE_TEXTURE_DATA
    // Need to listen the event only when not use batchnode, because it will use VBO
    auto listener = EventListenerCustom::create(EVENT_RENDERER_RECREATED, [this](EventCustom* event){
   /** listen the event that renderer was recreated on Android/WP8 */
        this->init();
    });

    _eventDispatcher->addEventListenerWithSceneGraphPriority(listener, this);
#endif
    
    return true;
}

void DrawNode3D::draw(Renderer *renderer, const Mat4 &transform, uint32_t flags)
{
    if(_bufferCount)
    {
        _customCommand.init(_globalZOrder, transform, flags);
        _customCommand.func = CC_CALLBACK_0(DrawNode3D::onDraw, this, transform, flags);
        renderer->addCommand(&_customCommand);
    }
    
    if(_bufferCountGLPoint)
    {
        _customCommandGLPoint.init(_globalZOrder, transform, flags);
        _customCommandGLPoint.func = CC_CALLBACK_0(DrawNode3D::onDrawGLPoint, this, transform, flags);
        renderer->addCommand(&_customCommandGLPoint);
    }
    
    if(_bufferCountGLLine)
    {
        _customCommandGLLine.init(_globalZOrder, transform, flags);
        _customCommandGLLine.func = CC_CALLBACK_0(DrawNode3D::onDrawGLLine, this, transform, flags);
        renderer->addCommand(&_customCommandGLLine);
    }
}

void DrawNode3D::onDraw(const Mat4 &transform, uint32_t /*flags*/)
{
    getGLProgramState()->apply(transform);
    auto glProgram = this->getGLProgram();
    glProgram->setUniformLocationWith1f(glProgram->getUniformLocation("u_alpha"), _displayedOpacity / 255.0);
    GL::blendFunc(_blendFunc.src, _blendFunc.dst);

    if (_dirty)
    {
        glBindBuffer(GL_ARRAY_BUFFER, _vbo);
        glBufferData(GL_ARRAY_BUFFER, sizeof(V3F_C4B_T2F)*_bufferCapacity, _buffer, GL_STREAM_DRAW);
        
        _dirty = false;
    }
    if (Configuration::getInstance()->supportsShareableVAO())
    {
        GL::bindVAO(_vao);
    }
    else
    {
        GL::enableVertexAttribs(GL::VERTEX_ATTRIB_FLAG_POS_COLOR_TEX);

        glBindBuffer(GL_ARRAY_BUFFER, _vbo);
        // vertex
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_POSITION, 3, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, vertices));
        // color
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_COLOR, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, colors));
        // texcoord
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_TEX_COORD, 2, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, texCoords));
    }

    glDrawArrays(GL_TRIANGLES, 0, _bufferCount);
    glBindBuffer(GL_ARRAY_BUFFER, 0);
    
    if (Configuration::getInstance()->supportsShareableVAO())
    {
        GL::bindVAO(0);
    }
    
    CC_INCREMENT_GL_DRAWN_BATCHES_AND_VERTICES(1, _bufferCount);
    CHECK_GL_ERROR_DEBUG();
    
    if (_drawOnce)
    {
        _bufferCount = 0;
        _dirty = true;
    }
}

void DrawNode3D::onDrawGLLine(const Mat4 &transform, uint32_t /*flags*/)
{
    auto glProgram = GLProgramCache::getInstance()->getGLProgram(GLProgram::SHADER_NAME_POSITION_LENGTH_TEXTURE_COLOR);
    glProgram->use();
    glProgram->setUniformsForBuiltins(transform);
    glProgram->setUniformLocationWith1f(glProgram->getUniformLocation("u_alpha"), _displayedOpacity / 255.0);

    GL::blendFunc(_blendFunc.src, _blendFunc.dst);

    if (_dirtyGLLine)
    {
        glBindBuffer(GL_ARRAY_BUFFER, _vboGLLine);
        glBufferData(GL_ARRAY_BUFFER, sizeof(V3F_C4B_T2F)*_bufferCapacityGLLine, _bufferGLLine, GL_STREAM_DRAW);
        _dirtyGLLine = false;
    }
    if (Configuration::getInstance()->supportsShareableVAO())
    {
        GL::bindVAO(_vaoGLLine);
    }
    else
    {
        glBindBuffer(GL_ARRAY_BUFFER, _vboGLLine);
        GL::enableVertexAttribs(GL::VERTEX_ATTRIB_FLAG_POS_COLOR_TEX);
        // vertex
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_POSITION, 3, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, vertices));
        // color
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_COLOR, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, colors));
        // texcoord
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_TEX_COORD, 2, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, texCoords));
    }

    glLineWidth(_lineWidth);
    glDrawArrays(GL_LINES, 0, _bufferCountGLLine);
    
    if (Configuration::getInstance()->supportsShareableVAO())
    {
        GL::bindVAO(0);
    }
    
    glBindBuffer(GL_ARRAY_BUFFER, 0);
    CC_INCREMENT_GL_DRAWN_BATCHES_AND_VERTICES(1,_bufferCountGLLine);

    CHECK_GL_ERROR_DEBUG();
    
    if (_drawOnce)
    {
        _bufferCountGLLine = 0;
        _dirtyGLLine = true;
    }
}

void DrawNode3D::onDrawGLPoint(const Mat4 &transform, uint32_t /*flags*/)
{
    auto glProgram = GLProgramCache::getInstance()->getGLProgram(GLProgram::SHADER_NAME_POSITION_COLOR_TEXASPOINTSIZE);
    glProgram->use();
    glProgram->setUniformsForBuiltins(transform);
    glProgram->setUniformLocationWith1f(glProgram->getUniformLocation("u_alpha"), _displayedOpacity / 255.0);

    GL::blendFunc(_blendFunc.src, _blendFunc.dst);

    if (_dirtyGLPoint)
    {
        glBindBuffer(GL_ARRAY_BUFFER, _vboGLPoint);
        glBufferData(GL_ARRAY_BUFFER, sizeof(V3F_C4B_T2F)*_bufferCapacityGLPoint, _bufferGLPoint, GL_STREAM_DRAW);
        
        _dirtyGLPoint = false;
    }
    
    if (Configuration::getInstance()->supportsShareableVAO())
    {
        GL::bindVAO(_vaoGLPoint);
    }
    else
    {
        glBindBuffer(GL_ARRAY_BUFFER, _vboGLPoint);
        GL::enableVertexAttribs( GL::VERTEX_ATTRIB_FLAG_POS_COLOR_TEX);
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_POSITION, 3, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, vertices));
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_COLOR, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, colors));
        glVertexAttribPointer(GLProgram::VERTEX_ATTRIB_TEX_COORD, 2, GL_FLOAT, GL_FALSE, sizeof(V3F_C4B_T2F), (GLvoid *)offsetof(V3F_C4B_T2F, texCoords));
    }
    
    glDrawArrays(GL_POINTS, 0, _bufferCountGLPoint);
    
    if (Configuration::getInstance()->supportsShareableVAO())
    {
        GL::bindVAO(0);
    }
    
    glBindBuffer(GL_ARRAY_BUFFER, 0);
    
    CC_INCREMENT_GL_DRAWN_BATCHES_AND_VERTICES(1,_bufferCountGLPoint);
    CHECK_GL_ERROR_DEBUG();
    
    if (_drawOnce)
    {
        _bufferCountGLPoint = 0;
        _dirtyGLPoint = true;
    }
}

void DrawNode3D::drawPoint(const Vec3& position, const float pointSize, const Color4F &color)
{
    ensureCapacityGLPoint(1);
    
    V3F_C4B_T2F *point = (V3F_C4B_T2F*)(_bufferGLPoint + _bufferCountGLPoint);
    V3F_C4B_T2F a = {position, Color4B(color), Tex2F(pointSize,0)};
    *point = a;
    
    _bufferCountGLPoint += 1;
    _dirtyGLPoint = true;
}

void DrawNode3D::drawPoints(const Vec3 *position, unsigned int numberOfPoints, const Color4F &color)
{
    drawPoints(position, numberOfPoints, 1.0, color);
}

void DrawNode3D::drawPoints(const Vec3 *position, unsigned int numberOfPoints, const float pointSize, const Color4F &color)
{
    ensureCapacityGLPoint(numberOfPoints);
    
    V3F_C4B_T2F *point = (V3F_C4B_T2F*)(_bufferGLPoint + _bufferCountGLPoint);
    
    for(unsigned int i=0; i < numberOfPoints; i++,point++)
    {
        V3F_C4B_T2F a = {position[i], Color4B(color), Tex2F(pointSize,0)};
        *point = a;
    }
    
    _bufferCountGLPoint += numberOfPoints;
    _dirtyGLPoint = true;
}

void DrawNode3D::drawLine(const Vec3 &origin, const Vec3 &destination, const Color4F &color, const Mat4& transform)
{
    ensureCapacityGLLine(2);
    
    V3F_C4B_T2F *point = (V3F_C4B_T2F*)(_bufferGLLine + _bufferCountGLLine);
    
    V3F_C4B_T2F a = {transform.MultiplyPoint3(origin), Color4B(color), Tex2F(0.0, 0.0)};
    V3F_C4B_T2F b = {transform.MultiplyPoint3(destination), Color4B(color), Tex2F(0.0, 0.0)};
    
    *point = a;
    *(point+1) = b;
    
    _bufferCountGLLine += 2;
    _dirtyGLLine = true;
}

void DrawNode3D::drawPoly(const Vec3 *poli, unsigned int numberOfPoints, bool closePolygon, const Color4F &color, const Mat4& transform)
{
    unsigned int vertex_count;
    if(closePolygon)
    {
        vertex_count = 2 * numberOfPoints;
        ensureCapacityGLLine(vertex_count);
    }
    else
    {
        vertex_count = 2 * (numberOfPoints - 1);
        ensureCapacityGLLine(vertex_count);
    }
    
    V3F_C4B_T2F *point = (V3F_C4B_T2F*)(_bufferGLLine + _bufferCountGLLine);
 
    unsigned int i = 0;
    for(; i<numberOfPoints-1; i++)
    {
        V3F_C4B_T2F a = {transform.MultiplyPoint3(poli[i]), Color4B(color), Tex2F(0.0, 0.0)};
        V3F_C4B_T2F b = {transform.MultiplyPoint3(poli[i+1]), Color4B(color), Tex2F(0.0, 0.0)};
        
        *point = a;
        *(point+1) = b;
        point += 2;
    }
    if(closePolygon)
    {
        V3F_C4B_T2F a = {transform.MultiplyPoint3(poli[i]), Color4B(color), Tex2F(0.0, 0.0)};
        V3F_C4B_T2F b = {transform.MultiplyPoint3(poli[0]), Color4B(color), Tex2F(0.0, 0.0)};
        *point = a;
        *(point+1) = b;
    }
    
    _bufferCountGLLine += vertex_count;
}

void DrawNode3D::drawWireArc( const Vec3& center, const Vec3& normal, const Vec3& from, float angle, float radius, const Color4F &color, const Mat4& transform)
{
    const int kArcPoints = 60;
    static Vec3 s_Points[kArcPoints];
    setDiscSectionPoints (s_Points, kArcPoints, center, normal, from, angle, radius);
    drawPoly(s_Points, kArcPoints, false, color, transform);
}

void DrawNode3D::drawWireDisk (const Vec3 &center, const Vec3 &normal, float radius, const Color4F &color, const Mat4& transform)
{
    Vec3 tangent = Cross (normal, Vec3(0,1,0));
    if (SqrMagnitude(tangent) < 0.001f)
        tangent = Cross (normal, Vec3(1,0,0));
    drawWireArc (center, normal, tangent, 360.0f, radius, color, transform);
}

void DrawNode3D::drawWireSphere(const Vec3 &center, float radius, const Color4F &color, const Mat4& transform)
{
    drawWireArc (center, Vec3(0,1,0), Vec3(1,0,0), 360.0f, radius, color, transform);
    drawWireArc (center, Vec3(0,0,1), Vec3(1,0,0), 360.0f, radius, color, transform);
    drawWireArc (center, Vec3(1,0,0), Vec3(0,1,0), 360.0f, radius, color, transform);
}

void DrawNode3D::drawWireCube (const Vec3& center, const Vec3& siz, bool depthTest, const Color4F &color, const Mat4& transform)
{
    Vec3 halfsize = siz * 0.5f;
    Vec3 p000 = center + Vec3(-halfsize.x, -halfsize.y, -halfsize.z);
    Vec3 p001 = center + Vec3(-halfsize.x, -halfsize.y,  halfsize.z);
    Vec3 p010 = center + Vec3(-halfsize.x,  halfsize.y, -halfsize.z);
    Vec3 p011 = center + Vec3(-halfsize.x,  halfsize.y,  halfsize.z);
    Vec3 p100 = center + Vec3( halfsize.x, -halfsize.y, -halfsize.z);
    Vec3 p101 = center + Vec3( halfsize.x, -halfsize.y,  halfsize.z);
    Vec3 p110 = center + Vec3( halfsize.x,  halfsize.y, -halfsize.z);
    Vec3 p111 = center + Vec3( halfsize.x,  halfsize.y,  halfsize.z);

    #define ADD_CUBE_LINE(aaa,bbb) drawLine(aaa, bbb, color, transform)
    ADD_CUBE_LINE(p000, p001);
    ADD_CUBE_LINE(p001, p011);
    ADD_CUBE_LINE(p011, p010);
    ADD_CUBE_LINE(p010, p000);
    
    ADD_CUBE_LINE(p100, p101);
    ADD_CUBE_LINE(p101, p111);
    ADD_CUBE_LINE(p111, p110);
    ADD_CUBE_LINE(p110, p100);
    
    ADD_CUBE_LINE (p000, p100);
    ADD_CUBE_LINE (p001, p101);
    ADD_CUBE_LINE (p010, p110);
    ADD_CUBE_LINE (p011, p111);
    #undef ADD_CUBE_LINE
}

void DrawNode3D::drawDot(const Vec3 &pos, float radius, const Color4F &color)
{
    unsigned int vertex_count = 2*3;
    ensureCapacity(vertex_count);
    
    V3F_C4B_T2F a = {Vec3(pos.x - radius, pos.y - radius, pos.z), Color4B(color), Tex2F(-1.0, -1.0) };
    V3F_C4B_T2F b = {Vec3(pos.x - radius, pos.y + radius, pos.z), Color4B(color), Tex2F(-1.0,  1.0) };
    V3F_C4B_T2F c = {Vec3(pos.x + radius, pos.y + radius, pos.z), Color4B(color), Tex2F( 1.0,  1.0) };
    V3F_C4B_T2F d = {Vec3(pos.x + radius, pos.y - radius, pos.z), Color4B(color), Tex2F( 1.0, -1.0) };
    
    V3F_C4B_T2F_Triangle *triangles = (V3F_C4B_T2F_Triangle *)(_buffer + _bufferCount);
    V3F_C4B_T2F_Triangle triangle0 = {a, b, c};
    V3F_C4B_T2F_Triangle triangle1 = {a, c, d};
    triangles[0] = triangle0;
    triangles[1] = triangle1;
    
    _bufferCount += vertex_count;
    
    _dirty = true;
}

void DrawNode3D::drawRect(const Vec3 &p1, const Vec3 &p2, const Vec3 &p3, const Vec3& p4, const Color4F &color)
{
    drawLine(p1, p2, color, Mat4::IDENTITY);
    drawLine(p2, p3, color, Mat4::IDENTITY);
    drawLine(p3, p4, color, Mat4::IDENTITY);
    drawLine(p4, p1, color, Mat4::IDENTITY);
}

void DrawNode3D::drawPolygon(const Vec3 *verts, int count, const Color4F &fillColor, float borderWidth, const Color4F &borderColor)
{
    CCASSERT(count >= 0, "invalid count value");
    
    bool outline = (borderColor.a > 0.0f && borderWidth > 0.0f);
    
    auto  triangle_count = outline ? (3*count - 2) : (count - 2);
    auto vertex_count = 3*triangle_count;
    ensureCapacity(vertex_count);
    
    V3F_C4B_T2F_Triangle *triangles = (V3F_C4B_T2F_Triangle *)(_buffer + _bufferCount);
    V3F_C4B_T2F_Triangle *cursor = triangles;
    
    for (int i = 0; i < count-2; i++)
    {
        V3F_C4B_T2F_Triangle tmp = {
            {verts[0], Color4B(fillColor), __t(v3fzero)},
            {verts[i+1], Color4B(fillColor), __t(v3fzero)},
            {verts[i+2], Color4B(fillColor), __t(v3fzero)},
        };
        
        *cursor++ = tmp;
    }
    
    if(outline)
    {
        struct ExtrudeVerts {Vec3 offset, n;};
        struct ExtrudeVerts* extrude = (struct ExtrudeVerts*)malloc(sizeof(struct ExtrudeVerts)*count);
        memset(extrude, 0, sizeof(struct ExtrudeVerts)*count);
        
        for (int i = 0; i < count; i++)
        {
            Vec3 v0 = __v3f(verts[(i-1+count)%count]);
            Vec3 v1 = __v3f(verts[i]);
            Vec3 v2 = __v3f(verts[(i+1)%count]);
            
            Vec3 n1 = v3fnormalize(v3fperp(v3fsub(v1, v0)));
            Vec3 n2 = v3fnormalize(v3fperp(v3fsub(v2, v1)));
            
            Vec3 offset = v3fmult(v3fadd(n1, n2), 1.0f / (v3fdot(n1, n2) + 1.0f));
            struct ExtrudeVerts tmp = {offset, n2};
            extrude[i] = tmp;
        }
        
        for(int i = 0; i < count; i++)
        {
            int j = (i+1)%count;
            Vec3 v0 = __v3f(verts[i]);
            Vec3 v1 = __v3f(verts[j]);
            
            Vec3 n0 = extrude[i].n;
            
            Vec3 offset0 = extrude[i].offset;
            Vec3 offset1 = extrude[j].offset;
            
            Vec3 inner0 = v3fsub(v0, v3fmult(offset0, borderWidth));
            Vec3 inner1 = v3fsub(v1, v3fmult(offset1, borderWidth));
            Vec3 outer0 = v3fadd(v0, v3fmult(offset0, borderWidth));
            Vec3 outer1 = v3fadd(v1, v3fmult(offset1, borderWidth));
            
            V3F_C4B_T2F_Triangle tmp1 = {
                {inner0, Color4B(borderColor), __t(v3fneg(n0))},
                {inner1, Color4B(borderColor), __t(v3fneg(n0))},
                {outer1, Color4B(borderColor), __t(n0)}
            };
            *cursor++ = tmp1;
            
            V3F_C4B_T2F_Triangle tmp2 = {
                {inner0, Color4B(borderColor), __t(v3fneg(n0))},
                {outer0, Color4B(borderColor), __t(n0)},
                {outer1, Color4B(borderColor), __t(n0)}
            };
            *cursor++ = tmp2;
        }
        
        free(extrude);
    }
    
    _bufferCount += vertex_count;
    
    _dirty = true;
}

void DrawNode3D::drawSolidPoly(const Vec3 *poli, unsigned int numberOfPoints, const Color4F &color)
{
    drawPolygon(poli, numberOfPoints, color, 0.0, Color4F(0.0, 0.0, 0.0, 0.0));
}

void DrawNode3D::drawTriangle(const Vec3 &p1, const Vec3 &p2, const Vec3 &p3, const Color4F &color)
{
    unsigned int vertex_count = 3;
    ensureCapacity(vertex_count);

    Color4B col = Color4B(color);
    V3F_C4B_T2F a = {Vec3(p1.x, p1.y, p1.z), col, Tex2F(0.0, 0.0) };
    V3F_C4B_T2F b = {Vec3(p2.x, p2.y, p2.z), col, Tex2F(0.0,  0.0) };
    V3F_C4B_T2F c = {Vec3(p3.x, p3.y, p3.z), col, Tex2F(0.0,  0.0) };

    V3F_C4B_T2F_Triangle *triangles = (V3F_C4B_T2F_Triangle *)(_buffer + _bufferCount);
    V3F_C4B_T2F_Triangle triangle = {a, b, c};
    triangles[0] = triangle;

    _bufferCount += vertex_count;
    _dirty = true;
}

void DrawNode3D::clear()
{
    _bufferCount = 0;
    _dirty = true;
    _bufferCountGLLine = 0;
    _dirtyGLLine = true;
    _bufferCountGLPoint = 0;
    _dirtyGLPoint = true;
    _lineWidth = _defaultLineWidth;
}

const BlendFunc& DrawNode3D::getBlendFunc() const
{
    return _blendFunc;
}

void DrawNode3D::setBlendFunc(const BlendFunc &blendFunc)
{
    _blendFunc = blendFunc;
}

void DrawNode3D::setLineWidth(GLfloat lineWidth)
{
    _lineWidth = lineWidth;
}

GLfloat DrawNode3D::getLineWidth()
{
    return this->_lineWidth;
}


NS_CC_END