#include "base.h"
   

BOOLEAN ProcessG75(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)		 G75->End_X=Pos_X;
	else if(b_UCode) G75->End_X=Pos_U+Start_X;
	
	if(b_ZCode)		 G75->End_Z=Pos_Z;
	else if(b_WCode) G75->End_Z=Pos_W+Start_Z;
	
	if(b_RCode)G75->Z_Back=fabs(Pos_R);
	if(b_PCode)G75->Z_Add=fabs(Pos_P*0.001);
	if(b_QCode)G75->X_Add=fabs(Pos_Q*0.001);  
	
	if(G75->X_Add==0 || G75->Z_Add==0)return TRUE;   
	
	if(G75->End_Z>Start_Z)
	    G75->Z_Direct=1;
	else {
	   G75->Z_Direct=0;
	   G75->Z_Add=-G75->Z_Add;
	   G75->Z_Back=-G75->Z_Back;
	}
    if(G75->End_X>Start_X)
       G75->X_Direct=1;
	else {
	   G75->X_Direct=0;
	   G75->X_Add=-G75->X_Add;
	   G75->X_Back=-G75->X_Back;
	} 
	
    while(1){
            while(1){
                  
		          if(G75->X_Direct){
			              if((Get_AbsPos(1)+G75->X_Add+G75->X_Back)<=G75->End_X){
			                    Target_PosX=Get_AbsPos(1)+G75->X_Add+G75->X_Back;
			              }
			              else{
			                    Target_PosX=G75->End_X;
			                    X_End=TRUE;
			              }
			      }        
			      else{
			              if(Get_AbsPos(1)+G75->X_Add+G75->X_Back>=G75->End_X){
			                    Target_PosX=Get_AbsPos(1)+G75->X_Add+G75->X_Back;
			              }
			              else{
			                    Target_PosX=G75->End_X;
			                    X_End=TRUE;
			              }
			      }          
			      Target_PosZ=Get_AbsPos(3);
			      
			      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(X_End)break;
			      
		          Set_SipSpeed(1,g_Current_Fast_Rate*g_Sysparam.X_FastSpeed);
		          Target_PosX=Get_AbsPos(1)-G75->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;
		          }
    
            }//end for while(1){//Z軸到終點
 
            if(Z_End)break;    
            X_End=FALSE;
		    Set_SipSpeed(3,g_Current_Fast_Rate*g_Sysparam.Z_FastSpeed);
		    Target_PosZ=Get_AbsPos(3)-G75->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;
		    }
		    
		    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;
		    }
		    
		    if(G75->Z_Direct){
                     if((Get_AbsPos(3)+G75->Z_Add+G75->Z_Back)<=G75->End_Z){
			              Target_PosZ=Get_AbsPos(3)+G75->Z_Add+G75->Z_Back;
			         }else{
			              Target_PosZ=G75->End_Z;
			              Z_End=TRUE;
			         }
			}else{
			         if((Get_AbsPos(3)+G75->Z_Add+G75->Z_Back)>=G75->End_Z){
			              Target_PosZ=Get_AbsPos(3)+G75->Z_Add+G75->Z_Back;
			         }else{
			              Target_PosZ=G75->End_Z;
			              Z_End=TRUE;
			         }
			}         
		    Set_SipSpeed(3,g_Current_Fast_Rate*g_Sysparam.Z_FastSpeed);
		    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;
    }

    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;
    }
	return FALSE;
}