// cncappDlg.cpp : implementation file
//

#include "stdafx.h"
#include "cncapp.h"
#include "cncappDlg.h"


#include "usbmclib.h"
#include "movgen.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

#ifdef USBMC_STATIC_LIB
#ifdef _DEBUG
# pragma comment(lib, "usbmclibd.lib") //Static debug
#else
# pragma comment(lib, "usbmclib.lib") //Static release
#endif
#else
# pragma comment(lib, "usbmc.lib")	//DLL version
#endif

#pragma comment(lib, "setupapi.lib")

using namespace USBMC;


static int status_watchdog=0;


/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About

class CAboutDlg : public CDialog
{
public:
	CAboutDlg();

// Dialog Data
	//{{AFX_DATA(CAboutDlg)
	enum { IDD = IDD_ABOUTBOX };
	//}}AFX_DATA

	// ClassWizard generated virtual function overrides
	//{{AFX_VIRTUAL(CAboutDlg)
	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support
	//}}AFX_VIRTUAL

// Implementation
protected:
	//{{AFX_MSG(CAboutDlg)
	//}}AFX_MSG
	DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
	//{{AFX_DATA_INIT(CAboutDlg)
	//}}AFX_DATA_INIT
}

void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CAboutDlg)
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
	//{{AFX_MSG_MAP(CAboutDlg)
		// No message handlers
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CCncappDlg dialog

CCncappDlg::CCncappDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CCncappDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CCncappDlg)
		// NOTE: the ClassWizard will add member initialization here
	//}}AFX_DATA_INIT
	// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CCncappDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CCncappDlg)
		// NOTE: the ClassWizard will add DDX and DDV calls here
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CCncappDlg, CDialog)
	//{{AFX_MSG_MAP(CCncappDlg)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_BN_CLICKED(IDC_RESET, OnConnect)
	ON_BN_CLICKED(IDC_STOP, OnStop)
	ON_BN_CLICKED(IDC_ESTOP, OnEstop)
	ON_WM_DESTROY()
	ON_BN_CLICKED(IDC_SPINDLE, OnSpindle)
	ON_BN_CLICKED(IDC_OUT1, OnOut1)
	ON_BN_CLICKED(IDC_OUT2, OnOut2)
	ON_BN_CLICKED(IDC_OUT3, OnOut3)
	ON_BN_CLICKED(IDC_GOTO, OnGoto)
	ON_BN_CLICKED(IDC_GOTOZ, OnGotoz)
	ON_BN_CLICKED(IDC_GOTOLIN, OnGotolin)
	ON_BN_CLICKED(IDC_REFALL, OnRefAll)
	ON_BN_CLICKED(IDC_SOFTLINITS, OnSoftlimits)
	ON_BN_CLICKED(IDC_PROBE, OnProbe)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CCncappDlg message handlers

void StatusMsg(const char*txt)
{
	AfxGetMainWnd()->SetDlgItemText(IDC_STATUS, txt);
}


BOOL CCncappDlg::OnInitDialog()
{
	CDialog::OnInitDialog();

	// Add "About..." menu item to system menu.

	// IDM_ABOUTBOX must be in the system command range.
	ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
	ASSERT(IDM_ABOUTBOX < 0xF000);

	CMenu* pSysMenu = GetSystemMenu(FALSE);
	if (pSysMenu != NULL)
	{
		CString strAboutMenu;
		strAboutMenu.LoadString(IDS_ABOUTBOX);
		if (!strAboutMenu.IsEmpty())
		{
			pSysMenu->AppendMenu(MF_SEPARATOR);
			pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
		}
	}

	// Set the icon for this dialog.  The framework does this automatically
	//  when the application's main window is not a dialog
	SetIcon(m_hIcon, TRUE);			// Set big icon
	SetIcon(m_hIcon, FALSE);		// Set small icon
	
	// TODO: Add extra initialization here

	SetDlgItemInt(IDC_TX,0);
	SetDlgItemInt(IDC_TY,0);
	SetDlgItemInt(IDC_TZ,0);

	SetDlgItemInt(IDC_FEEDRATE, 50000);

	StatusMsg(" Ready.");
	
	return TRUE;  // return TRUE  unless you set the focus to a control
}

void CCncappDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
	if ((nID & 0xFFF0) == IDM_ABOUTBOX)
	{
		CAboutDlg dlgAbout;
		dlgAbout.DoModal();
	}
	else
	{
		CDialog::OnSysCommand(nID, lParam);
	}
}

// If you add a minimize button to your dialog, you will need the code below
//  to draw the icon.  For MFC applications using the document/view model,
//  this is automatically done for you by the framework.

void CCncappDlg::OnPaint() 
{
	if (IsIconic())
	{
		CPaintDC dc(this); // device context for painting

		SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);

		// Center icon in client rectangle
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// Draw the icon
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CDialog::OnPaint();
	}
}

// The system calls this to obtain the cursor to display while the user drags
//  the minimized window.
HCURSOR CCncappDlg::OnQueryDragIcon()
{
	return (HCURSOR) m_hIcon;
}




static int online;

#define SPINDLE_RELAY_PIN 10

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////  Setup motor speed profiles and configure I/O pins ///////////////////////////////

//	Pins:
//  1, 2 - StepX, DirX
//  3, 4 - SteyY, DirY
//  5, 6 - SteyZ, DirZ
//  7, 8 - SteyA, DirA
//  9 - Spindle PWM out
//
//  1 - EStop
//  2 - Index
//  3 - HomeX
//  4 - HomeY

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void MCInit()
{
			USBMC_Config c;

			memset(&c, 0, sizeof c);

			c.ax_cfg[0].spu = 1000;
			c.ax_cfg[0].vel = 100000;
			c.ax_cfg[0].acc = 100000;
			c.ax_cfg[0].home_speed = 10000;
			c.ax_cfg[0].home_offset = 0;
			c.ax_cfg[0].home_flags = HOME_AUTOZERO;
			c.ax_cfg[0].smax = 50000;
			c.ax_cfg[0].smin = -50000;

			c.ax_cfg[1].spu = 1000;
			c.ax_cfg[1].vel = 100000;
			c.ax_cfg[1].acc = 100000;
			c.ax_cfg[1].home_speed = 10000;
			c.ax_cfg[1].home_offset = 0;
			c.ax_cfg[1].home_flags = HOME_AUTOZERO;
			c.ax_cfg[1].smax = 50000;
			c.ax_cfg[1].smin = -50000;

			c.ax_cfg[2].spu = 1000;
			c.ax_cfg[2].vel = 100000;
			c.ax_cfg[2].acc = 100000;
			c.ax_cfg[2].home_speed = 10000;
			c.ax_cfg[2].home_offset = 0;
			c.ax_cfg[2].home_flags = 0;
			c.ax_cfg[2].smax = 50000;
			c.ax_cfg[2].smin = -50000;

			c.ax_cfg[3].spu = 1000;
			c.ax_cfg[3].vel = 100000;
			c.ax_cfg[3].acc = 100000;
			c.ax_cfg[3].home_speed = 10000;
			c.ax_cfg[3].home_offset = 0;
			c.ax_cfg[3].home_flags = 0;
			c.ax_cfg[3].slave = 1;	//A is slave of X
			c.ax_cfg[3].smax = 50000;
			c.ax_cfg[3].smin = -50000;

			c.ax_cfg[4].spu = 1000;
			c.ax_cfg[4].vel = 100000;
			c.ax_cfg[4].acc = 100000;
			c.ax_cfg[5].spu = 1000;
			c.ax_cfg[5].vel = 100000;
			c.ax_cfg[5].acc = 100000;

			c.home_retract_speed = 10;

			c.sp_cfg.spindle_motor = SPINDLE_PWM;	//SPINDLE_NOMOTOR
			c.sp_cfg.spindle_pwmfreq = 10000;
			c.sp_cfg.spindle_pwmmax = 1<<16;
			c.sp_cfg.spindle_pwmmin = 0;


			c.state_flags = 0;	//LIMIT_OVERRIDE;	// HOME_UNLINK_SLAVE
			c.enc_for_mpg = 0;

			USBMC_IOCfg p;

			memset(&p, 0, sizeof p);

			p.stepx.enable = TRUE;
			p.stepx.pin = 1;
			p.dirx.enable = TRUE;
			p.dirx.pin = 2;

			p.stepy.enable = TRUE;
			p.stepy.pin = 3;
			p.diry.enable = TRUE;
			p.diry.pin = 4;

			p.stepz.enable = TRUE;
			p.stepz.pin = 5;
			p.dirz.enable = TRUE;
			p.dirz.pin = 6;

			p.stepa.enable = TRUE;
			p.stepa.pin = 7;
			p.dira.enable = TRUE;
			p.dira.pin = 8;

			p.steps.enable = TRUE;
			p.steps.pin = 9;	//Spindle axis output pin (PWM or StepDir)

			//p.ext1.enable = TRUE;
			//p.ext1.pin = 16;

			p.estop.enable = TRUE;
			p.estop.inv = TRUE;
			p.estop.pin = 1;

			p.index.enable = TRUE;
			p.index.pin = 2;

			p.homex.enable = TRUE;
			p.homex.pin = 3;
			p.homex.inv = TRUE;
			p.homey.enable = TRUE;
			p.homey.pin = 4;
			p.homey.inv = TRUE;

			p.probe.pin = 3;
			p.probe.enable = TRUE;
			p.probe.inv = TRUE;

			for(int i=0;i<16;i++) p.debounce[i] = 1;

			p.debounce[1] = 0;	//no debouncing for index input

			usbmc_Init(&c, &p);

			usbmc_SetBuffSize(400);
}

UINT tid=0;
USBMC_Status mcstat;

static BOOL spindle_on=0;
static BOOL EStop=0;

static volatile BOOL need_resync=0;


static WORD dig_outputs=0;	  //state for all outputs
static WORD zero_mask=0;	//to null axis
static WORD mc_flags = 0;	//SOFT_LIMITS;
static BOOL update_out = 0;	//signal to main update procedure


static BOOL is_jogging = 0;
static BOOL is_probing = 0;

void set_spindle_relay(BOOL spindle_on)
{
		//Spindle reley is controlled as any other output

		WORD bmask = 1<<SPINDLE_RELAY_PIN;
		if(spindle_on) dig_outputs |= bmask;
		else dig_outputs &= ~bmask;

		update_out = 1;
}

static char*mode[]={"IDLE", "JOG",	"SEGM", "HOLD", "ESTOP","SAFE", "RDOWN"};
static char*sequ[]={"NONE", "HOME0", "HOME1", "HOME2", "HOME3", "HOME4", "PROBE1", "PROBE2","MPG"};
static char*fault[]={	"NONE",  "UNKN", "HOMESW", "LIMITSW", "SOFTLIMIT", "ESTOP",  "PROBE"};

void print_bin(CString&s, WORD b, int bits)
{
	s="";
	for(int i=0; i<bits; i++){
		s += (b&1? "1":"0");
		b >>= 1;
	}
}

#define GMOVE_BUSY	(ax.state || ay.state || az.state || lin_mov_avail())
#define MOTION_BUSY	(is_jogging || GMOVE_BUSY)

Axis ax, ay, az;	//axis definition for motion generation

void MotionGeneration()
{
		USBMC_BufSeg p;
		p.line=0;
		p.ecmd=0;
		p.epin=0;
		for(int i=0;i<6;i++) p.pos[i]=0.0;

		// Generate segment data and put in the motion buffer while there is a free space in
		// the buffer

		while(usbmc_BufFreeCnt() && GMOVE_BUSY)
		{

					if(ax.state || ay.state || az.state){	//rapid move is active
							ramp_gen_tpos(&ax);
							ramp_gen_tpos(&ay);
							ramp_gen_tpos(&az);
					}else
						lin_move_gen(&ax, &ay, &az);	//linear move is active
			
					p.pos[0] = ax.x;
					p.pos[1] = ay.x;
					p.pos[2] = az.x;

					usbmc_BufAdd(&p);	//add point to the motion buffer
		}

}


/////////////////////////// Main update procedure for USB-MC controller ////////////////////////////


VOID CALLBACK UpdateProc( HWND hwnd, UINT uMsg, UINT_PTR idEvent, DWORD dwTime)
{

	if(online && usbmc_GetStatus(&mcstat)){

		status_watchdog = 0;	//reset watchdog counter

		if(mcstat.state==M_ESTOP) {
			EStop = 1;
			spindle_on = 0;
			set_spindle_relay(0);
			usbmc_CmdSpindle(0);
		}

		if(mcstat.homed & S_FLG_AXISHOMED){	//Finish homing one axis
			int axnum = (mcstat.homed & 7);
			//do something if needed
			if(axnum==0) ShowWindow(GetDlgItem(hwnd, IDC_XHOMED), SW_SHOWNORMAL);
			else if(axnum==1) ShowWindow(GetDlgItem(hwnd, IDC_YHOMED), SW_SHOWNORMAL);
			else if(axnum==2) ShowWindow(GetDlgItem(hwnd, IDC_ZHOMED), SW_SHOWNORMAL);
		}

		if(is_probing){
			int res = usbmc_GetProbeResult();
			if(res == PROBE_NOHIT) {
				StatusMsg(" Probe move finished with NO hit!");
				is_probing = 0;
			}
			else if(res == PROBE_HIT){
				CString s;
				double pos[6];
				usbmc_GetProbePos(pos);
				s.Format(" Probe hit: %f, %f, %f, %f", pos[0], pos[1], pos[2], pos[3]);
				StatusMsg(s);
				is_probing = 0;
			}
		}

		CString s; s.Format("Mode: %s", mode[mcstat.state]);
		SetDlgItemText(hwnd, IDC_STATE, s);

		s.Format("Seq: %s", sequ[mcstat.cur_seq]);
		SetDlgItemText(hwnd, IDC_SEQUENCE, s);

		s.Format("Fault: %s", fault[mcstat.fault]);
		SetDlgItemText(hwnd, IDC_ERR, s);

		print_bin(s, mcstat.inputs, 14);
		SetDlgItemText(hwnd, IDC_INPUTS, s);

		print_bin(s, mcstat.outputs, 16);
		SetDlgItemText(hwnd, IDC_OUTPUTS, s);
		
		SetDlgItemInt(hwnd, IDC_X,  mcstat.axis_ticks[0], 1);
		SetDlgItemInt(hwnd, IDC_Y,  mcstat.axis_ticks[1], 1);
		SetDlgItemInt(hwnd, IDC_Z,  mcstat.axis_ticks[2], 1);
		SetDlgItemInt(hwnd, IDC_A,  mcstat.axis_ticks[3], 1);

		SetDlgItemInt(hwnd, IDC_ANALOG, mcstat.analogv, 0);

		SetDlgItemInt(hwnd, IDC_SPINDLERPM, (int)(mcstat.spindle_rpm+0.5),0);

		if((need_resync || (mcstat.flags&S_FLG_SWRESYNC)) && (mcstat.state==M_IDLE)) {
			need_resync = 0;
			usbmc_PosResync();
			ax.x = mcstat.axis_ticks[0];
			ay.x = mcstat.axis_ticks[1];
			az.x = mcstat.axis_ticks[2];
			//AfxMessageBox("Resynced!");
		}
	}

		if(online){
			if(++status_watchdog>500/25){		//Check watchdog
				//Error status not received for 500 ms
				usbmc_Close();
				online = 0;
				EStop = 1;
				SetDlgItemText(hwnd, IDC_STATE, "Mode: not connected.");
				StatusMsg(" Connection lost!");
				SetDlgItemText(hwnd, IDC_RESET, "Connect");
			}	
		}
		else status_watchdog=0;

	
		if(online)
			//if(zero_mask || (last_outputs != dig_outputs)){	//optimized, cmd only if there is a change
			if(update_out ){
				usbmc_CmdSetOutputs(dig_outputs, mc_flags, zero_mask);	//Set all outputs
				//last_outputs = dig_outputs;
				zero_mask = 0;
				update_out = 0;
			}
		

		if(EStop) return;
		if(mcstat.state == M_JOG) return;

		
		MotionGeneration();

		usbmc_BufUpdate();	//Regulary send current data in the buffer to the controller

		//If move was short and finished before enough data is generated to auto-trigger buffer executioin start,
		// then purge all that is currently left in the buffer

		if(mcstat.state==M_HOLD && !GMOVE_BUSY){
			usbmc_CmdPurge(0);
		}
}


////////////////////////////////////////////////////////////////

void CCncappDlg::OnConnect() 
{
	if(online){

		spindle_on=0;
		set_spindle_relay(0);
		usbmc_CmdSpindle(0);
		
		online = 0;
		KillTimer(tid);

		usbmc_Disconnect();
		usbmc_Close();

		SetDlgItemText(IDC_STATE, "Mode: not connected");
		SetDlgItemText(IDC_RESET, "Connect");
		StatusMsg(" Disconnected.");
	}
	else if(usbmc_Open()){	
		//AfxMessageBox(CString("Motion controller found! FW: ")+USBMC_FirmwareVersion);
		StatusMsg(CString(" Connected. USB-MC firmware version: ")+usbmc_GetFirmwareVersion());
		online=1;
		EStop=0;
	
		tid = SetTimer(0, 25, (TIMERPROC)UpdateProc);
		need_resync = 1;
		MCInit();

		SetDlgItemText(IDC_RESET, "Disconnect");
	}
	else
		AfxMessageBox("Motion controller NOT found!");
}



void  stop_motion()
{
		usbmc_BufClear();
		ax.state = IDLE;
		ay.state = IDLE;
		az.state = IDLE;
		stop_lin_move();
		is_probing = 0;
}


void CCncappDlg::OnStop() 
{
	stop_motion();

	if(online) usbmc_CmdStop(0);		//STOP_ESTOP

	spindle_on=0;
	EStop = 0;
	set_spindle_relay(0);
	usbmc_CmdSpindle(0);

	need_resync=true;
}



////////////////// Handle cursor keys for JOG moves /////////////////////////////

BOOL CCncappDlg::PreTranslateMessage(MSG* pMsg) 
{
	// TODO: Add your specialized code here and/or call the base class

	//no Joging ESTOP mode or if move is already active

	if(EStop || !online || GMOVE_BUSY || mcstat.state==M_SEGM || mcstat.state==M_HOLD) 
		return CDialog::PreTranslateMessage(pMsg);

	static int xjog=0, yjog=0,zjog=0;
	int ret = 0;

	if(pMsg->message == WM_KEYDOWN ){

		DWORD key_code = pMsg->wParam;
		int shift = GetAsyncKeyState(VK_SHIFT);
		DWORD speed = shift? 100000:10000;
		BOOL dir = (key_code == VK_LEFT) || (key_code == VK_DOWN) || (key_code == VK_NEXT);
		int prev = pMsg->lParam & (1<<30);	//check previous key state to avoid repeat

		if(key_code == VK_LEFT || key_code == VK_RIGHT){
			if(!prev)  { usbmc_CmdJogOn(0, dir, speed); xjog=1; }
			ret = 1;
		}
		else if(key_code == VK_UP || key_code == VK_DOWN){
			if(!prev) { usbmc_CmdJogOn(1, dir, speed); yjog=1; }
			ret = 1;
		}
		else if(key_code == VK_PRIOR || key_code == VK_NEXT){
			if(!prev) { usbmc_CmdJogOn(2, dir, speed); zjog=1; }
			ret = 1;
		}

		is_jogging=xjog || yjog || zjog;
		if(ret) return 1;

	}
	else if(pMsg->message == WM_KEYUP){

		DWORD key_code = pMsg->wParam;

		if(key_code == VK_LEFT || key_code == VK_RIGHT){
			usbmc_CmdJogOff(0); xjog = 0;
			ret = 1;
		}
		else if(key_code == VK_UP || key_code == VK_DOWN){
			usbmc_CmdJogOff(1); yjog = 0;
			ret =  1;
		}
		else if(key_code == VK_PRIOR || key_code == VK_NEXT){
			usbmc_CmdJogOff(2); zjog = 0;
			ret = 1;
		}

		is_jogging=xjog || yjog || zjog;
		if(ret) return 1;
	}
	
	return CDialog::PreTranslateMessage(pMsg);
}




void CCncappDlg::OnEstop() 
{
	stop_motion();

	if(online) usbmc_CmdStop(STOP_ESTOP);

	spindle_on=0;
	set_spindle_relay(0);
	EStop = 1;
	usbmc_CmdSpindle(0);

	need_resync=true;
}



/*
void SpindleON(BOOL on)
{
		//Spindle reley is controlled as any other output

		WORD bmask = 1<<SPINDLE_RELEY_PIN;
		if(on) dig_outputs |= bmask;
		else dig_outputs &= ~bmask;

		if(online){
			usbmc_CmdSetOutputs(dig_outputs);	//Turn On/Off spindle reley
			usbmc_CmdSpindle(on);	//only needed for PWM/Step&Dir modes
		}

		spindle_on = on;
}
*/

void CCncappDlg::OnSpindle()	//Spindle ON/OFF
{
	if(EStop) {
		AfxMessageBox("Cannot start Spindle in ESTOP mode!");
		return;	//no spindle in ESTOP mode
	}

	if(online){
		spindle_on = !spindle_on;
		set_spindle_relay(spindle_on);
		usbmc_CmdSpindle(spindle_on, spindle_on?32768:0);	//only needed for PWM&StepDir output
	}
}


//When window is destroyed on exit

void CCncappDlg::OnDestroy() 
{

	if(online){
		online = 0;
		KillTimer(tid);

		usbmc_Disconnect();
		usbmc_Close();
	}

	CDialog::OnDestroy();	
}


void CCncappDlg::OnOut1() 
{
	dig_outputs ^= 1<<11;
	update_out = 1;

}	

void CCncappDlg::OnOut2() 
{
	dig_outputs ^= 1<<12;
	update_out = 1;
}

void CCncappDlg::OnOut3() 
{
	dig_outputs ^= 1<<13;
	update_out = 1;
}


#define MAXV	100000
#define ACC		100000




void CCncappDlg::OnGoto() 
{
	if(!online) return;

	if(MOTION_BUSY){
		AfxMessageBox("Wait for current move to finish.");
		return;
	}

	start_move(&ax, (int)GetDlgItemInt(IDC_TX), ACC, MAXV);
	start_move(&ay, (int)GetDlgItemInt(IDC_TY), ACC, MAXV);
	start_move(&az, (int)GetDlgItemInt(IDC_TZ), ACC, MAXV);
	
}

void CCncappDlg::OnGotoz() 
{
	if(!online) return;

	if(MOTION_BUSY){
		AfxMessageBox("Wait for current move to finish.");
		return;
	}

	start_move(&ax, 0, ACC, MAXV);
	start_move(&ay, 0, ACC, MAXV);
	start_move(&az, 0, ACC, MAXV);
	
}

void CCncappDlg::OnGotolin() 
{
	if(!online) return;

	if(MOTION_BUSY){
		AfxMessageBox("Wait for current move to finish.");
		return;
	}

	ax.acc = ACC;
	ay.acc = ACC;
	az.acc = ACC;
	ax.max_vel = MAXV;
	ay.max_vel = MAXV;
	az.max_vel = MAXV;

	ax.target_pos = (int)GetDlgItemInt(IDC_TX);
	ay.target_pos = (int)GetDlgItemInt(IDC_TY);
	az.target_pos = (int)GetDlgItemInt(IDC_TZ);

	start_lin_move(&ax, &ay, &az, GetDlgItemInt(IDC_FEEDRATE));
}

void CCncappDlg::OnRefAll() 
{
	if(!online) return;

	if(MOTION_BUSY){
		AfxMessageBox("Wait for current move to finish.");
		return;
	}

	//refferencing w/o switches, just zero all axes
	//usbmc_CmdSetOutputs(dig_outputs, 0, 15);	//TODO: optimize
	zero_mask = 15;
	update_out = 1;

/*
	//real referencing
	usbmc_CmdHomeAxis(0);
	usbmc_CmdHomeAxis(1);
	usbmc_CmdHomeAxis(2);
*/

}

void CCncappDlg::OnSoftlimits() 
{
	// TODO: Add your control notification handler code here
	mc_flags ^= SOFT_LIMITS;
	update_out = 1;

	SetDlgItemText(IDC_SOFTLIMITS, (mc_flags & SOFT_LIMITS)? "SoftL On":"SoftL Off");
}

void CCncappDlg::OnProbe() 
{
	// TODO: Add your control notification handler code here
		if(!online) return;

	if(MOTION_BUSY){
		AfxMessageBox("Wait for current move to finish.");
		return;
	}

	ax.target_pos = (int)GetDlgItemInt(IDC_TX);
	ay.target_pos = (int)GetDlgItemInt(IDC_TY);
	az.target_pos = (int)GetDlgItemInt(IDC_TZ);

	double p1[6] = {ax.x, ay.x,az.x,0,0,0};
	double p2[6] = {ax.target_pos, ay.target_pos, az.target_pos,0,0,0};

	usbmc_CmdProbe(p1, p2, GetDlgItemInt(IDC_FEEDRATE)/1000.);

	is_probing = 1;
}
