#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <libgen.h>
#include "LinuxDARwIn.h"

#define INI_FILE_PATH       "../../../../Data/config.ini"
#define U2D_DEV_NAME        "/dev/ttyUSB0"

using namespace Robot;


void change_current_dir()
{
    char exepath[1024] = {0};
    if(readlink("/proc/self/exe", exepath, sizeof(exepath)) != -1)
        chdir(dirname(exepath));
}

int main(void)
{
    printf( "\n===== Walk Backward Tutorial for DARwIn =====\n\n");

    change_current_dir();

    minIni* ini = new minIni(INI_FILE_PATH);

	//////////////////// Framework Initialize ////////////////////////////
	LinuxCM730 linux_cm730(U2D_DEV_NAME);
	CM730 cm730(&linux_cm730);
	if(MotionManager::GetInstance()->Initialize(&cm730) == false)
	{
		printf("Fail to initialize Motion Manager!\n");
			return 0;
	}
        //Port initialization and opening, dynamixel power on
        
        MotionManager::GetInstance()->LoadINISettings(ini);
        Walking::GetInstance()->LoadINISettings(ini);
        //Load default settings for MotionManager and Walking module

	MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
	MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
        LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
        motion_timer->Start();
        //Create MotionManager object and registers head and walking modules, then timers are initialized.
	/////////////////////////////////////////////////////////////////////

        /////////////////////////Capture Motor Position//////////////////////
	int n = 0;
	int param[JointData::NUMBER_OF_JOINTS * 5];
	int wGoalPosition, wStartPosition, wDistance;

	for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
	{
		wStartPosition = MotionStatus::m_CurrentJoints.GetValue(id);
		wGoalPosition = Walking::GetInstance()->m_Joint.GetValue(id);
		if( wStartPosition > wGoalPosition )
			wDistance = wStartPosition - wGoalPosition;
		else
			wDistance = wGoalPosition - wStartPosition;

		wDistance >>= 2;
		if( wDistance < 8 )
			wDistance = 8;

		param[n++] = id;
		param[n++] = CM730::GetLowByte(wGoalPosition);
		param[n++] = CM730::GetHighByte(wGoalPosition);
		param[n++] = CM730::GetLowByte(wDistance);
		param[n++] = CM730::GetHighByte(wDistance);
	}
	cm730.SyncWrite(MX28::P_GOAL_POSITION_L, 5, JointData::NUMBER_OF_JOINTS - 1, param);
        //Capture initial position of Darwin OP 2's motor position
        
	printf("Press the ENTER key to begin!\n");
	getchar();

        Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
        Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
	MotionManager::GetInstance()->SetEnable(true);
        //Walking and MotionManager enable motors

    while(1)
    {

	    if(Action::GetInstance()->IsRunning() == 0)
            {
		Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
                Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);

                if(Walking::GetInstance()->IsRunning() == false){
                Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0;
                //X_MOVE_AMPLITUDE is a variable in Walking class that controls Darwin OP 2's walking step length, 
                //each unit corresponds to approximately 1 mm. Set X_MOVE_AMPLITUDE to negative numbers to program 
                //Darwin OP 2 to walk backwards. The Walking class calculates corresponding motor positions 
                //internally
                
                Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0;
                //A_MOVE_AMPLITUDE is a variable in Walking class that controls Darwin OP 2's yaw movement. Each 
                //unit correspond approximately to 1 degree. Positive degrees for Darwin OP 2 to turn 
                //counterclockwise, negative degree for clockwise. 
                
                Walking::GetInstance()->Start();
                //Start walking algorithm
                }

		Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0;
		Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0;
                
	     }

    }

    return 0;
}