#include "ImportURDFSetup.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"


#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "Bullet3Common/b3FileUtils.h"

#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "MyURDFImporter.h"

static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
static bool enableConstraints = true;//false;
#include "URDF2Bullet.h"

#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"

#include "urdf_samples.h"

//#include "BulletCollision/CollisionShapes/btCylinderShape.h"
//#define USE_BARREL_VERTICES
//#include "OpenGLWindow/ShapeData.h"

#include <iostream>
#include <fstream>
using namespace urdf;


#include "../CommonInterfaces/CommonMultiBodyBase.h"

#include "MyMultiBodyCreator.h"


class ImportUrdfSetup : public CommonMultiBodyBase
{
    char m_fileName[1024];
    
    struct ImportUrdfInternalData* m_data;
	bool m_useMultiBody;
    
public:
    ImportUrdfSetup(struct GUIHelperInterface* helper, int option);
    virtual ~ImportUrdfSetup();

	virtual void initPhysics();
	virtual void stepSimulation(float deltaTime);
    
    void setFileName(const char* urdfFileName);
};


btAlignedObjectArray<std::string> gFileNameArray;


#define MAX_NUM_MOTORS 1024

struct ImportUrdfInternalData
{
    ImportUrdfInternalData()
    :m_numMotors(0)
    {
    }
    
    btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
    btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
    int m_numMotors;
};


ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option)
	:CommonMultiBodyBase(helper)
{
	if (option==1)
	{
		m_useMultiBody = true;
	} else
	{
		m_useMultiBody = false;
	}

	static int count = 0;
    gFileNameArray.clear();
    gFileNameArray.push_back("r2d2.urdf");

    m_data = new ImportUrdfInternalData;
    
    //load additional urdf file names from file
    
    FILE* f = fopen("urdf_files.txt","r");
    if (f)
    {
        int result;
        //warning: we don't avoid string buffer overflow in this basic example in fscanf
        char fileName[1024];
        do
        {
            result = fscanf(f,"%s",fileName);
            if (result==1)
            {
                gFileNameArray.push_back(fileName);
            }
        } while (result==1);
        
        fclose(f);
    }
    
    int numFileNames = gFileNameArray.size();

    if (count>=numFileNames)
	{
		count=0;
	}
    sprintf(m_fileName,gFileNameArray[count++].c_str());
}

ImportUrdfSetup::~ImportUrdfSetup()
{
    delete m_data;
}

static btVector4 colors[4] =
{
	btVector4(1,0,0,1),
	btVector4(0,1,0,1),
	btVector4(0,1,1,1),
	btVector4(1,1,0,1),
};


btVector3 selectColor()
{

	static int curColor = 0;
	btVector4 color = colors[curColor];
	curColor++;
	curColor&=3;
	return color;
}

void ImportUrdfSetup::setFileName(const char* urdfFileName)
{
    memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
}





void printTree1(my_shared_ptr<const Link> link,int level = 0)
{
    level+=2;
    int count = 0;
    for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
    {
        if (*child)
        {
            for(int j=0;j<level;j++) std::cout << "  "; //indent
            std::cout << "child(" << (count++)+1 << "):  " << (*child)->name  << std::endl;
            // first grandchild
            printTree1(*child,level);
        }
        else
        {
            for(int j=0;j<level;j++) std::cout << " "; //indent
            std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
        }
    }

}


struct URDF_LinkInformation
{
    const Link* m_thisLink;
	int m_linkIndex;
	//int m_parentIndex;

    btTransform m_localInertialFrame;
    //btTransform m_localVisualFrame;
	btTransform m_bodyWorldTransform;
	btVector3 m_localInertiaDiagonal;
	btScalar m_mass;

	btCollisionShape* m_collisionShape;
    btRigidBody* m_bulletRigidBody;

	URDF_LinkInformation()
		:m_thisLink(0),
		m_linkIndex(-2),
		//m_parentIndex(-2),
		m_collisionShape(0),
		m_bulletRigidBody(0)
	{

	}
	virtual ~URDF_LinkInformation()
	{
        printf("~\n");
	}
};

struct URDF_JointInformation
{

};


struct URDF2BulletMappings
{
    btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
	btAlignedObjectArray<btScalar>			m_linkMasses;
	
	bool m_createMultiBody;
	int m_totalNumJoints;
	btMultiBody*	m_bulletMultiBody;

    btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
	URDF2BulletMappings()
		:m_createMultiBody(false),
		m_totalNumJoints(0),
		m_bulletMultiBody(0)
	{
	}

};



void ImportUrdfSetup::initPhysics()
{

	int upAxis = 2;
	m_guiHelper->setUpAxis(2);

	this->createEmptyDynamicsWorld();
	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
    btIDebugDraw::DBG_DrawConstraints
    +btIDebugDraw::DBG_DrawContactPoints
    +btIDebugDraw::DBG_DrawAabb
        );//+btIDebugDraw::DBG_DrawConstraintLimits);

	
	btVector3 gravity(0,0,0);
	gravity[upAxis]=-9.8;

	m_dynamicsWorld->setGravity(gravity);
    //int argc=0;
	char relativeFileName[1024];
	
	b3FileUtils fu;
	printf("m_fileName=%s\n", m_fileName);
	bool fileFound = fu.findFile(m_fileName, relativeFileName, 1024);



	std::string xml_string;
	char pathPrefix[1024];
	pathPrefix[0] = 0;
	
    if (!fileFound){
        std::cerr << "URDF file not found, using a dummy test URDF" << std::endl;
        xml_string = std::string(urdf_char);

    } else
    {
		
		int maxPathLen = 1024;
		fu.extractPath(relativeFileName,pathPrefix,maxPathLen);


        std::fstream xml_file(relativeFileName, std::fstream::in);
        while ( xml_file.good() )
        {
            std::string line;
            std::getline( xml_file, line);
            xml_string += (line + "\n");
        }
        xml_file.close();
    }

    my_shared_ptr<ModelInterface> robot = parseURDF(xml_string);
    if (!robot){
        std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
        return ;
    }
    std::cout << "robot name is: " << robot->getName() << std::endl;

    // get info from parser
    std::cout << "---------- Successfully Parsed XML ---------------" << std::endl;
    // get root link
    my_shared_ptr<const Link> root_link=robot->getRoot();
    if (!root_link) return ;

    std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;

    // print entire tree
    printTree1(root_link);
    printf("now using new interface\n");
    std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;
    
    //now print the tree using the new interface
    MyURDFImporter u2b(robot,m_guiHelper);
    printTree(u2b, 0);
    
    btTransform identityTrans;
	identityTrans.setIdentity();
	
	int numJoints = (*robot).m_numJoints;

	
    bool useUrdfInterfaceClass = true;
    
    {
        URDF2BulletMappings mappings;
      
        btMultiBody* mb = 0;
        
		
        
            
        //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
        int rootLinkIndex = u2b.getRootLinkIndex();
        printf("urdf root link index = %d\n",rootLinkIndex);
		MyMultiBodyCreator creation(m_guiHelper);

        ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,pathPrefix);
        mb = creation.getBulletMultiBody();

        if (m_useMultiBody)
        {
            mb->setHasSelfCollision(false);
            mb->finalizeMultiDof();
            m_dynamicsWorld->addMultiBody(mb);
            
                
            //create motors for each joint
                
            for (int i=0;i<mb->getNumLinks();i++)
            {
                int mbLinkIndex = i;
                if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute)
                {
                    if (m_data->m_numMotors<MAX_NUM_MOTORS)
                    {
                        int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
                            
                        std::string jointName = u2b.getJointName(urdfLinkIndex);
                        char motorName[1024];
                        sprintf(motorName,"%s q'", jointName.c_str());
                        btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
                        *motorVel = 0.f;
                        SliderParams slider(motorName,motorVel);
                        slider.m_minVal=-4;
                        slider.m_maxVal=4;
                        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                        float maxMotorImpulse = 0.1f;
                        btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
                        m_data->m_jointMotors[m_data->m_numMotors]=motor;
                        m_dynamicsWorld->addMultiBodyConstraint(motor);
                        m_data->m_numMotors++;
                    }
                }
                    
            }
        }
    }
	
	//the btMultiBody support is work-in-progress :-)

	
	printf("numJoints/DOFS = %d\n", numJoints);

	bool createGround=true;
	if (createGround)
	{
        btVector3 groundHalfExtents(20,20,20);
        groundHalfExtents[upAxis]=1.f;
        btBoxShape* box = new btBoxShape(groundHalfExtents);
        box->initializePolyhedralFeatures();

        m_guiHelper->createCollisionShapeGraphicsObject(box);
        btTransform start; start.setIdentity();
        btVector3 groundOrigin(0,0,0);
        groundOrigin[upAxis]=-2;//.5;
        start.setOrigin(groundOrigin);
        btRigidBody* body =  createRigidBody(0,start,box);
        //m_dynamicsWorld->removeRigidBody(body);
       // m_dynamicsWorld->addRigidBody(body,2,1);
        btVector3 color(0.5,0.5,0.5);
        m_guiHelper->createRigidBodyGraphicsObject(body,color);
    }

	///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
	m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
}

void ImportUrdfSetup::stepSimulation(float deltaTime)
{
	if (m_dynamicsWorld)
	{
        for (int i=0;i<m_data->m_numMotors;i++)
        {
            m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
        }
        
		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
		m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
	}
}

class ExampleInterface*    ImportURDFCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option)
{

	return new ImportUrdfSetup(helper, option);
}