#include "gugaomotor.h"
#include<QDateTime>
#include<QTime>
#include<conio.h>
#include<QThread>
#include<time.h>
#include<qeventloop.h>

GuGaoMotor::GuGaoMotor(QObject *parent):QObject(parent)
{

}

GuGaoMotor::~GuGaoMotor()
{
    for (int Axle=1;Axle<5;Axle++)
    {
//        GT_ClrSts(Axle);  // 清除各轴的报警和限位
        GT_AxisOff(Axle);  // 伺服使能
    }
     GT_Close();

}

void GuGaoMotor::CommandHandler(QString CmdStr, short CmdResult)
{
    QString m_time=QTime::currentTime().toString("hh:mm:ss:zzz");
    QString str=m_time+" >> "+CmdStr+" ; "+QString::number(int(CmdResult));

//    qDebug()<<str;
//    emit SigCommandHandler(str);
}

int GuGaoMotor::LoadSetting(char *CfgStr)
{
    short sRtn;
    sRtn = GT_Open();
    CommandHandler("GT_Open", sRtn);
    if(sRtn)
        return sRtn;

    sRtn = GT_Reset();
    CommandHandler("GT_Reset", sRtn);

    sRtn = GT_LoadConfig(CfgStr); // 配置运动控制器
    CommandHandler("GT_LoadConfig", sRtn);

    for (int Axle=1;Axle<5;Axle++)
    {
        sRtn = GT_ClrSts(Axle);  // 清除各轴的报警和限位
        CommandHandler("GT_ClrSts", sRtn);

        sRtn = GT_AxisOn(Axle);  // 伺服使能
        CommandHandler("GT_AxisOn", sRtn);
        QThread::sleep(1);
    }
     QThread::sleep(2);
    return 0;
}

int GuGaoMotor::Home(int Axle)
{
    long SEARCH_HOME= -100000; // 定义home回零搜索距离
    long HOME_OFFSET= 2000; // 定义到home捕获位置的偏移量
    short sRtn, capture;   //   ，捕获状态
    TTrapPrm trapPrm;
    long status, pos;
    double prfPos, encPos, axisPrfPos, axisEncPos;

    sRtn = GT_SetCaptureMode(Axle, CAPTURE_HOME);
    CommandHandler("GT_SetCaptureMode", sRtn);

    sRtn = GT_PrfTrap(Axle);
    CommandHandler("GT_PrfTrap", sRtn);

    sRtn = GT_GetTrapPrm(Axle, &trapPrm);
    CommandHandler("GT_GetTrapPrm", sRtn);
    trapPrm.acc = 0.25;
    trapPrm.dec = 0.25;

    sRtn = GT_SetTrapPrm(Axle, &trapPrm);
    CommandHandler("GT_SetTrapPrm", sRtn);

    sRtn = GT_SetVel(Axle, 4);
    CommandHandler("GT_SetVel", sRtn);

    sRtn = GT_SetPos(Axle, SEARCH_HOME);
    CommandHandler("GT_SetPos", sRtn);

    sRtn = GT_Update(1<<(Axle-1));
    CommandHandler("GT_Update", sRtn);
    qDebug()<<QObject::tr("\nWaiting for home signal...");
    do
    {
        sRtn = GT_GetSts(Axle, &status);

        sRtn = GT_GetCaptureStatus(Axle, &capture, &pos);

        sRtn = GT_GetPrfPos(Axle, &prfPos);

        sRtn = GT_GetEncPos(Axle, &encPos);
//        printf("capture=%d prfPos=%.2lf encPos=%.2lf\r", capture, prfPos, encPos);

        if( 0 == ( status & 0x400 ) )
        {
           qDebug()<<QObject::tr("\nno home found...");
//            getch();
            return 1;
        }

    }while( 0 == capture );

//    printf("\ncapture pos = %ld\n", pos);
    qDebug()<<pos<<QObject::tr(";  ")<<prfPos<<QObject::tr(";  ")<<encPos;
    sRtn = GT_SetPos(Axle, pos + HOME_OFFSET);
    CommandHandler("GT_SetPos", sRtn);

    sRtn = GT_Update(1<<(Axle-1));
    CommandHandler("GT_Update", sRtn);
    do
    {

        sRtn = GT_GetSts(Axle, &status);

        sRtn = GT_GetPrfPos(Axle, &prfPos);

        sRtn = GT_GetEncPos(Axle, &encPos);
//        printf("status=0x%-8lx prfPos=%-10.1lf encPos=%-10.1lf\r", status, prfPos, encPos);

    }while( status & 0x400 );

    if( prfPos != pos+HOME_OFFSET )
    {
        printf("\nmove to home pos error\n");
//        getch();
        return 2;
    }
    printf("\nHome finish\n");

    QThread::usleep(200);
    printf("\nPress any key to set pos as 0...\n");
//    getch();

    sRtn = GT_ZeroPos(Axle);
    CommandHandler("GT_ZeroPos", sRtn);

    sRtn = GT_GetPrfPos(Axle, &prfPos);

    sRtn = GT_GetEncPos(Axle, &encPos);

    sRtn = GT_GetAxisPrfPos(Axle, &axisPrfPos);

    sRtn = GT_GetAxisEncPos(Axle, &axisEncPos);
    printf("\nprfPos=%-10.0lf encPos=%-10.0lf axisPrfPos=%-10.0lf axisEncPos=%-10.0lf",
           prfPos, encPos, axisPrfPos, axisEncPos);

//    sRtn = GT_AxisOff(Axle);
//    printf("\nGT_AxisOff()=%d\n", sRtn);
//    getch();
    return 0;
}

int GuGaoMotor::HomeIndex(int Axle)
{
    long SEARCH_HOME	=-300000;// 定义home回零搜索距离
    long HOME_OFFSET	=-2000;// 定义到home捕获位置的偏移量
    long SEARCH_INDEX	=15000;// 定义index回零搜索距离
    long INDEX_OFFSET   =2000;// 定义到index捕获位置的偏移量
    short sRtn;
    short capture;// 捕获状态
    TTrapPrm trapPrm;// 点位运动参数结构体
    long status;// 轴状态
    long pos;// 捕获位置
// 分别是规划位置，编码器位置，轴的规划位置，轴的编码器位置
    double prfPos, encPos, axisPrfPos, axisEncPos;

    sRtn = GT_SetCaptureMode(Axle, CAPTURE_HOME);// 开启轴的home捕获功能
    CommandHandler("GT_SetCaptureMode", sRtn);

    sRtn = GT_PrfTrap(Axle);// 设置轴为点位运动模式
    CommandHandler("GT_PrfTrap", sRtn);

    sRtn = GT_GetTrapPrm(Axle, &trapPrm);// 读取点位运动参数
    CommandHandler("GT_GetTrapPrm", sRtn);
    trapPrm.acc = 0.25;
    trapPrm.dec = 0.125;

    sRtn = GT_SetTrapPrm(Axle, &trapPrm);// 设置点位运动参数
    CommandHandler("GT_SetTrapPrm", sRtn);
    if(Axle==Axle4)
    {
        sRtn = GT_SetVel(Axle, 2);// 设置目标速度
    }else {
        sRtn = GT_SetVel(Axle, 15);// 设置目标速度
    }
    CommandHandler("GT_SetVel", sRtn);

    sRtn = GT_SetPos(Axle, SEARCH_HOME);	// 设置目标位置
    CommandHandler("GT_SetPos", sRtn);

    sRtn = GT_Update(1<<(Axle-1));// 启动运动，等待home信号触发
    CommandHandler("GT_Update", sRtn);
    printf("\nWaiting for home signal...\n");

    do
    {
        sRtn = GT_GetSts(Axle, &status);// 读取轴状态

        sRtn = GT_GetCaptureStatus(Axle, &capture, &pos);// 读取捕获状态

        sRtn = GT_GetPrfPos(Axle, &prfPos);// 读取规划位置

        sRtn = GT_GetEncPos(Axle, &encPos);	// 读取编码器位置
//        printf("capture=%d prfPos=%-10.1lf encPos=%-10.1lf\r", capture, prfPos, encPos);
// 电机已经停止，说明整个搜索过程中home信号一直没有触发
        if( 0 == ( status & 0x400 ) )
        {
            printf("\nno home found");
            return 1;
        }

    }while( 0 == capture );
    printf("\ncapture pos = %ld\n", pos);
    // 设定目标位置为捕获位置+偏移量
    sRtn = GT_SetPos(Axle, pos + HOME_OFFSET);
    CommandHandler("GT_SetPos", sRtn);
    // 启动运动
    sRtn = GT_Update(1<<(Axle-1));
    CommandHandler("GT_Update", sRtn);

    do
    {

        sRtn = GT_GetSts(Axle, &status);

        sRtn = GT_GetPrfPos(Axle, &prfPos);

        sRtn = GT_GetEncPos(Axle, &encPos);
//        printf("status=0x%-8lx prfPos=%-10.1lf encPos=%-10.1lf\r", status, prfPos, encPos);
    }while( status & 0x400 );

    if( prfPos != pos + HOME_OFFSET)
    {
        printf("\nmove to home pos error");
        return 2;
    }

    printf("\nHome finish\n");
    QThread::usleep(200);
// 启动index捕获
    sRtn = GT_SetCaptureMode(Axle, CAPTURE_INDEX);
    CommandHandler("GT_SetCaptureMode", sRtn);

    sRtn = GT_SetPos(Axle, (long)(prfPos + SEARCH_INDEX));
    CommandHandler("GT_SetPos", sRtn);

    sRtn = GT_Update(1<<(Axle-1));
    CommandHandler("GT_Update", sRtn);

    printf("\nWaiting for index signal...\n");

    do
    {
        sRtn = GT_GetSts(Axle, &status);

        sRtn = GT_GetCaptureStatus(Axle, &capture, &pos);

        sRtn = GT_GetPrfPos(Axle, &prfPos);

        sRtn = GT_GetEncPos(Axle, &encPos);
//        printf("capture=%d prfPos=%-10.1lf encPos=%-10.1lf\r", capture, prfPos, encPos);

        if( 0 == ( status & 0x400 ) )
        {
            printf("\nno index found\n");
//            getch();
            return 1;
        }

    }while( 0 == capture );

    printf("\ncapture pos = %ld\n", pos);

    sRtn = GT_SetPos(Axle, pos+ INDEX_OFFSET);
    CommandHandler("GT_SetPos", sRtn);

    sRtn = GT_Update(1<<(Axle-1));
    CommandHandler("GT_Update", sRtn);

    do
    {
        sRtn = GT_GetSts(Axle, &status);

        sRtn = GT_GetPrfPos(Axle, &prfPos);

        sRtn = GT_GetEncPos(Axle, &encPos);
//        printf("status=0x%-8lx prfPos=%-10.1lf encPos=%-10.1lf\r", status, prfPos, encPos);
    }while( status & 0x400 );

    if( prfPos != pos+ INDEX_OFFSET)
    {
        printf("\nmove to index pos error\n");
//        getch();
        return 2;
    }

    printf("\nHome+Index finish\n");
    printf("\nPress any key to set pos as 0...\n");
//    getch();

    QThread::usleep(200);
    // 位置清零
    sRtn = GT_ZeroPos(Axle);
    printf("GT_ZeroPos", sRtn);

    sRtn = GT_GetPrfPos(Axle, &prfPos);
    sRtn = GT_GetEncPos(Axle, &encPos);
    sRtn = GT_GetAxisPrfPos(Axle, &axisPrfPos);
    sRtn = GT_GetAxisEncPos(Axle, &axisEncPos);
    printf("\nprfPos=%-10.0lf encPos=%-10.0lf axisPrfPos=%-10.0lf axisEncPos=%-10.0lf",
           prfPos, encPos, axisPrfPos, axisEncPos);

    CommandHandler("HomeIndex",0);
//    getch();
    return 0;
}

int GuGaoMotor::SetCraPrm(TCrdPrm CrdPrm)  //建立二维坐标系
{
    double OriginX,OriginY;

    memset(&CrdPrm,0,sizeof(CrdPrm));// 将结构体变量初始化为 '0'

    CrdPrm.dimension=2; // 坐标系为二维坐标系
    CrdPrm.synVelMax=500; // 最大合成速度： 500pulse/ms
    CrdPrm.synAccMax=2; // 最大加速度： 1pulse/ms^2
    CrdPrm.evenTime = 50; // 最小匀速时间： 50ms
    CrdPrm.profile[0] = 1; // 规划器1对应到X轴
    CrdPrm.profile[1] = 2; // 规划器2对应到Y轴
    CrdPrm.setOriginFlag = 1; // 表示需要指定坐标系的原点坐标的规划位置

    GT_GetPrfPos(1,&OriginX);
    GT_GetPrfPos(2,&OriginY);
    CrdPrm.originPos[0] = OriginX; // 坐标系的原点坐标的规划位置为（OriginX, OriginY)
    CrdPrm.originPos[1] = OriginY;

    GT_SetCrdPrm(1,&CrdPrm);// 建立1号坐标系，设置坐标系参数
    return 0;
}

int GuGaoMotor:: MoveRelative(long position,double speed,int Axle)
{
    short sRtn;
    TTrapPrm trap;
    long sts;
    double prfPos;

    sRtn = GT_PrfTrap(Axle);
    CommandHandler("GT_PrfTrap", sRtn);

    sRtn = GT_GetTrapPrm(Axle, &trap);
    CommandHandler("GT_GetTrapPrm", sRtn);
    trap.acc = 0.5;
    trap.dec = 0.5;
    trap.smoothTime = 25;

    sRtn = GT_SetTrapPrm(Axle, &trap);
    CommandHandler("GT_SetTrapPrm", sRtn);

    sRtn = GT_GetPrfPos(Axle, &prfPos);

    sRtn = GT_SetPos(Axle, prfPos + position);
    CommandHandler("GT_SetPos", sRtn);

    sRtn = GT_SetVel(Axle, speed);
    CommandHandler("GT_SetVel", sRtn);

    sRtn = GT_Update(1<<(Axle-1));
    CommandHandler("GT_Update", sRtn);
    do
    {
        sRtn = GT_GetSts(Axle, &sts);

        sRtn = GT_GetPrfPos(Axle, &prfPos);
//        printf("sts=0x%-10lxprfPos=%-10.1lf\r", sts, prfPos);
    }while(sts&0x400);

    CommandHandler("MoveRelative",0);
//    getch();
    return 0;
}

int GuGaoMotor::MoveABS(long position, double speed, int Axle)
{
    short sRtn;
    TTrapPrm trap;
    long sts;
    double prfPos;

    sRtn = GT_PrfTrap(Axle);
    CommandHandler("GT_PrfTrap", sRtn);

    sRtn = GT_GetTrapPrm(Axle, &trap);
    CommandHandler("GT_GetTrapPrm", sRtn);
    trap.acc = 1;
    trap.dec = 1;
    trap.smoothTime = 25;

    sRtn = GT_SetTrapPrm(Axle, &trap);
    CommandHandler("GT_SetTrapPrm", sRtn);

    sRtn = GT_SetPos(Axle, position);
    CommandHandler("GT_SetPos", sRtn);

    sRtn = GT_SetVel(Axle, speed);
    CommandHandler("GT_SetVel", sRtn);

    sRtn = GT_Update(1<<(Axle-1));
    CommandHandler("GT_Update", sRtn);
    do
    {
        sRtn = GT_GetSts(Axle, &sts);
        sRtn = GT_GetPrfPos(Axle, &prfPos);
//        printf("sts=0x%-10lxprfPos=%-10.1lf\r", sts, prfPos);
    }while(sts&0x400);

    CommandHandler("MoveRelative",0);
    getch();
    return 0;
}

int GuGaoMotor::MoveJop(double speed,QPushButton *btn ,int Axle)
{
    short sRtn;
    TJogPrm jog;
    long sts;
    double prfPos, prfVel;

       // 将AXIS轴设为Jog模式
        sRtn = GT_PrfJog(Axle);
//        commandhandler("GT_PrfTrap", sRtn);
        // 读取Jog运动参数
        sRtn = GT_GetJogPrm(Axle, &jog);
//        commandhandler("GT_GetJogPrm", sRtn);
        jog.acc = 0.0625;
        jog.dec = 0.0625;
        // 设置Jog运动参数
        sRtn = GT_SetJogPrm(Axle, &jog);
//        commandhandler("GT_SetJogPrm", sRtn);
        // 设置AXIS轴的目标速度
        sRtn = GT_SetVel(Axle, speed);
//        commandhandler("GT_SetVel", sRtn);
        // 启动AXIS轴的运动
        sRtn = GT_Update(1<<(Axle-1));
//        commandhandler("GT_Update", sRtn);


//        while(!kbhit())
        while(!kbhit()&&btn->isDown())
        {
            // 读取AXIS轴的状态
            sRtn = GT_GetSts(Axle, &sts);
            // 读取AXIS轴的规划位置
            sRtn = GT_GetPrfPos(Axle, &prfPos);
            // 读取AXIS轴的规划速度
            sRtn = GT_GetPrfVel(Axle, &prfVel);
            //            printf("sts=0x%-10lxprfPos=%-10.1lfprfVel=%-10.1lf\r", sts, prfPos, prfVel);

        }
        return 0;

}

int GuGaoMotor::MoveLine(Point m_Point)
{
    short sRtn;
    short run;    // 读取插补运动状态
    long segment; // 读取当前已经完成的插补段数
//    long space;
    sRtn=GT_CrdClear(m_Point.crd,m_Point.Fifo);// 即将把数据存入坐标系1的FIFO0中，所以要首先清除此缓存区中的数据
    sRtn=GT_LnXY(m_Point.crd,m_Point.Pos.Xpos,m_Point.Pos.Ypos,m_Point.Speed
                 ,m_Point.ASpeed,m_Point.EndSpeed ,m_Point.Fifo );
    sRtn=GT_CrdStart(m_Point.crd,m_Point.Fifo);// 启动坐标系1的FIFO0的插补运动
    sRtn=GT_CrdStatus(m_Point.crd,&run,&segment,m_Point.Fifo );// 等待运动完成
    do
    {
        sRtn=GT_CrdStatus(m_Point.crd,&run,&segment,m_Point.Fifo );

    }while(run==1);

    return 1;
}

int GuGaoMotor::MoveArcR(Point m_Point)
{
    short sRtn;
    short run;     // 读取插补运动状态
    long segment;  // 读取当前已经完成的插补段数

    sRtn=GT_CrdClear(m_Point.crd,m_Point.Fifo);// 即将把数据存入坐标系1的FIFO0中，所以要首先清除此缓存区中的数据
    sRtn=GT_ArcXYR(m_Point.crd,m_Point.Pos.Xpos,m_Point.Pos.Ypos,m_Point.Radius,
                   m_Point.CircleDir,m_Point.Speed,m_Point.ASpeed,m_Point.EndSpeed
                   ,m_Point.Fifo);

    sRtn=GT_CrdStart(m_Point.crd,m_Point.Fifo);// 启动坐标系1的FIFO0的插补运动
    sRtn=GT_CrdStatus(m_Point.crd,&run,&segment,m_Point.Fifo );// 等待运动完成
    do
    {
        sRtn=GT_CrdStatus(m_Point.crd,&run,&segment,m_Point.Fifo );

    }while(run==1);

    return 1;
}

int GuGaoMotor::MoveArcC(Point m_Point,double X_center,double Y_center)
{
    short sRtn;
    short run;    // 读取插补运动状态
    long segment; // 读取当前已经完成的插补段数

     sRtn=GT_CrdClear(m_Point.crd,m_Point.Fifo);// 即将把数据存入坐标系1的FIFO0中，所以要首先清除此缓存区中的数据
     sRtn=GT_ArcXYC(m_Point.crd,m_Point.Pos.Xpos,m_Point.Pos.Ypos, X_center,Y_center,
                    m_Point.CircleDir,m_Point.Speed,m_Point.ASpeed,m_Point.EndSpeed
                    ,m_Point.Fifo);

     sRtn=GT_CrdStart(m_Point.crd,m_Point.Fifo);// 启动坐标系1的FIFO0的插补运动
     sRtn=GT_CrdStatus(m_Point.crd,&run,&segment,m_Point.Fifo );// 等待运动完成
     do
     {
         sRtn=GT_CrdStatus(m_Point.crd,&run,&segment,m_Point.Fifo );

     }while(run==1);
     return 0;
}

Position GuGaoMotor::GetAxlePos()
{
    Position m_position;
    short sRtn;
    sRtn = GT_GetPrfPos(1, &m_position.Xpos);
    sRtn = GT_GetPrfPos(2, &m_position.Ypos);
    sRtn = GT_GetPrfPos(3, &m_position.Zpos);
    sRtn = GT_GetPrfPos(4, &m_position.Upos);

    return m_position;
}