#include "base.h"

BOOLEAN ProcessG74(void)
{
    
	FP32 Start_X,Start_Z;
	INT8U ret;
	BOOLEAN X_End=FALSE,Z_End=FALSE;

    Start_X=Get_AbsPos(1);
	Start_Z=Get_AbsPos(3);
	
	if(b_XCode)		 G74->End_X=Pos_X;
	else if(b_UCode) G74->End_X=Pos_U+Start_X;
	
	if(b_ZCode)		 G74->End_Z=Pos_Z;
	else if(b_WCode) G74->End_Z=Pos_W+Start_Z;
	
	if(b_RCode)G74->X_Back=fabs(Pos_R);
	if(b_PCode)G74->X_Add=fabs(Pos_P*0.001);
	if(b_QCode)G74->Z_Add=fabs(Pos_Q*0.001);  
	
	if(G74->X_Add==0 || G74->Z_Add==0)return TRUE;   
	
	if(G74->End_Z>Start_Z)
	    G74->Z_Direct=1;
	else {
	   G74->Z_Direct=0;
	   G74->Z_Add=-G74->Z_Add;
	   G74->Z_Back=-G74->Z_Back;
	}
    if(G74->End_X>Start_X)
       G74->X_Direct=1;
	else {
	   G74->X_Direct=0;
	   G74->X_Add=-G74->X_Add;
	   G74->X_Back=-G74->X_Back;
	} 
	
    while(1){
            while(1){
                  
		          if(G74->Z_Direct){
			               if((Get_AbsPos(3)+G74->Z_Add+G74->Z_Back)<=G74->End_Z){
			                      Target_PosZ=Get_AbsPos(3)+G74->Z_Add+G74->Z_Back;
			               }
			               else{
			                      Target_PosZ=G74->End_Z;
			                      Z_End=TRUE;
			               }
			      }
			      else{
			               if((Get_AbsPos(3)+G74->Z_Add+G74->Z_Back)>=G74->End_Z){
			                      Target_PosZ=Get_AbsPos(3)+G74->Z_Add+G74->Z_Back;
			               }
			               else{
			                      Target_PosZ=G74->End_Z;
			                      Z_End=TRUE;
			               }
			      }  
			      Target_PosX=Get_AbsPos(1);
			      
			      g_Current_ASpeed=g_Current_PSpeed*g_Current_Inp_Rate;
	              if(g_Current_ASpeed>g_Sysparam.Max_FastSpeed)
		          g_Current_ASpeed=g_Sysparam.Max_FastSpeed;
			      Set_InpSpeed(g_Current_ASpeed);
			      bRunInp=TRUE;
			      while(1)
			      {
				       G01_AbsPos(Target_PosX,Target_PosZ);
				       g_MoveMode=1;
				       ret=WaitMotionEnd();
				       if(ret==255)return TRUE;
				       if(ret==0)break;
			      }
			      if(Z_End)break;
			      
		          Set_SipSpeed(3,g_Current_Fast_Rate*g_Sysparam.Z_FastSpeed);
		          Target_PosZ=Get_AbsPos(3)-G74->Z_Back;
		          bRunInp=FALSE;
		          while(1){
			          Move_AbsPos(3,Target_PosZ);
			          g_MoveMode=1;
			          ret=WaitMotionEnd();
			          if(ret==255)return TRUE;
			          if(ret==0)break;
		          }
    
            }//end for while(1){//Z軸到終點
 
            if(X_End)break;    
            Z_End=FALSE;
		    Set_SipSpeed(1,g_Current_Fast_Rate*g_Sysparam.X_FastSpeed);
		    Target_PosX=Get_AbsPos(1)-G74->X_Back;
		    bRunInp=FALSE;
		    while(1){
			    Move_AbsPos(1,Target_PosX);
			    g_MoveMode=1;
			    ret=WaitMotionEnd();
			    if(ret==255)return TRUE;
			    if(ret==0)break;
		    }
		    
		    Set_SipSpeed(3,g_Current_Fast_Rate*g_Sysparam.Z_FastSpeed);
		    Target_PosZ=Start_Z;
		    bRunInp=FALSE;
		    while(1){
			    Move_AbsPos(3,Target_PosZ);
			    g_MoveMode=1;
			    ret=WaitMotionEnd();
			    if(ret==255)return TRUE;
			    if(ret==0)break;
		    }
		    
		    if(G74->X_Direct){
                        if((Get_AbsPos(1)+G74->X_Add+G74->X_Back)<=G74->End_X){
			                   Target_PosX=Get_AbsPos(1)+G74->X_Add+G74->X_Back;
			            }else{
			                   Target_PosX=G74->End_X;
			                   X_End=TRUE;
			            }
			}
			else{
			            if((Get_AbsPos(1)+G74->X_Add+G74->X_Back)>=G74->End_X){
			                   Target_PosX=Get_AbsPos(1)+G74->X_Add+G74->X_Back;
			            }else{
			                   Target_PosX=G74->End_X;
			                   X_End=TRUE;
			            }
			}
		    Set_SipSpeed(1,g_Current_Fast_Rate*g_Sysparam.Z_FastSpeed);
		    bRunInp=FALSE;
		    while(1){
			    Move_AbsPos(1,Target_PosX);
			    g_MoveMode=1;
			    ret=WaitMotionEnd();
			    if(ret==255)return TRUE;
			    if(ret==0)break;
		    }
    } 

    Set_SipSpeed(3,g_Current_Fast_Rate*g_Sysparam.Z_FastSpeed);
    Target_PosZ=Start_Z;
    bRunInp=FALSE;
    while(1){
		 Move_AbsPos(3,Target_PosZ);
		 g_MoveMode=1;
	     ret=WaitMotionEnd();
		 if(ret==255)return TRUE;
		 if(ret==0)break;
    }   

    Set_SipSpeed(1,g_Current_Fast_Rate*g_Sysparam.X_FastSpeed);
    Target_PosX=Start_X;
    bRunInp=FALSE;
    while(1){
		 Move_AbsPos(1,Target_PosX);
		 g_MoveMode=1;
	     ret=WaitMotionEnd();
		 if(ret==255)return TRUE;
		 if(ret==0)break;
    }   
	return FALSE;//正確返回
}