using System;
using System.Text;
using System.Linq;
using System.Drawing;
using System.Diagnostics;
using System.Windows.Forms;
using System.Threading;
using System.Threading.Tasks;
using System.Collections.Generic;
using Tango.PMR.Stubs;
using Tango.Stubs;




//-------------------DEFINE Motors------------------
//const Int32	LDANCER_Motor	=	16;		 
//const Int32	LDRIVING_Motor	=	18;		 
//const Int32	LLOADING_Motor	=	19;	
//const Int32	LDANCER_Motor	=	16;		 
//const Int32	LDRIVING_Motor	=	18;		 
//const Int32	LLOADING_Motor	=	19;	

const Int32	LDANCER1_Motor			=	16;		 
const Int32	LDANCER2_Motor			=	17;		 
const Int32	LDRIVING_Motor			=	18;		 
const Int32	LLOADING_Motor			=	19;	
			
const Int32	RDANCER_Motor			=	21;	
//const Int32	RDRIVING_Motor			=	22;	
//const Int32	MAGNET_Driver			=	23;				//replace RLOADARM
const Int32	RLOADING_Motor			=	24;	

//-------------------DEFINE Motors Parameters------------------

const Int32 F1_MOTO_LLOADING_TX1 = 0x60000232;
const Int32 F1_MOTO_LLOADING_TX0 = 0x60000230;
const Int32 F1_MOTO_RLOADING_TX1 = 0x60000202;
const Int32 F1_MOTO_RLOADING_TX0 = 0x60000200;


//-------------------DEFINE Limit Switch---------------
const	Int32	F1_LS_02_Direct		=0x60000030;
const	Int32	F1_LS_03_Direct		=0x60000040;
const	Int32 	F1_LS_RLOADMOTOR_UP	= 0x0040 ;
const	Int32	F1_LS_RLOADMOTOR_DN	= 0x0080 ;

const	Int32 	F1_LS_LLOADMOTOR_UP	= 0x0100 ;
const	Int32	F1_LS_LLOADMOTOR_DN	= 0x0200 ;
const	Int32	F1_LS_LDANCER1_UP 	= 0x1000;
const	Int32	F1_LS_LDANCER1_DN 	= 0x2000;
const	Int32	F1_LS_LDANCER2_UP 	= 0x0400;
const	Int32	F1_LS_LDANCER2_DN 	= 0x0800;
const	Int32	F1_LS_LLOADRAM_LF	= 0x0080;
const	Int32	F1_LS_LLOADRAM_RT	= 0x0040;
const	Int32	F1_LS_LSPARE2		= 0x0004;		//up	
//const	Int32 	F1_LS_LLOADMOTOR_UP	= 0x0100 ;
const	Int32	F1_LS_RDANCER_DN 	= 0x0200;
const	Int32	F1_LS_RLOADRAM_DN	= 0x0020;
const	Int32	F1_LS_LSPARE1		= 0x0008;		//dn

//include "..\..\Defines\Tango_Defines_Basic.cs"
//include "Tango_Defines_LimitSwitch.cs"
//include "Tango_Defines_Motors.cs"
//include "Tango_Defines_SSI.cs"
//include "C:\To_Backup\Mulberry\Mulberry_current\Customers\Twine\Tango\LAB_Tests\Main_PCB\ATP_Test\Scripts\Defines\Tango_Defines_Basic.cs"
//include "C:\To_Backup\Mulberry\Mulberry_current\Customers\Twine\Tango\LAB_Tests\Main_PCB\ATP_Test\Scripts\Defines\Tango_Defines_SSI.cs"


void OnExecute(StubManager stubManager)
{
		for (int i=0;i<1;i++)
			{

		Move_Motor(LDANCER1_Motor,false,F1_LS_02_Direct,F1_LS_LDANCER1_UP,450);

		Move_Motor(LDANCER2_Motor,false,F1_LS_02_Direct,F1_LS_LSPARE2,450);

		Motor_SetParam(F1_MOTO_LLOADING_TX1,F1_MOTO_LLOADING_TX0,0x0a,0x0a000000);							//set kval ran to 0x2c- 0.9A
		Thread.Sleep(200);
		Move_Motor(LLOADING_Motor,true,F1_LS_02_Direct,F1_LS_LLOADMOTOR_UP,300);
		Motor_SetParam(F1_MOTO_LLOADING_TX1,F1_MOTO_LLOADING_TX0,0x0a,0x07000000);							//set kval ran to 0x2c- 0.6A
		Thread.Sleep(200);

		Move_Motor(RDANCER_Motor,true,F1_LS_03_Direct,F1_LS_RLOADRAM_DN,10);

		Motor_SetParam(F1_MOTO_RLOADING_TX1,F1_MOTO_RLOADING_TX0,0x0a,0x0a000000);							//set kval ran to 0x14,0.65A
		Thread.Sleep(200);
		Move_Motor(RLOADING_Motor,false,F1_LS_03_Direct,F1_LS_RLOADMOTOR_UP,200);
		Motor_SetParam(F1_MOTO_RLOADING_TX1,F1_MOTO_RLOADING_TX0,0x0a,0x07000000);							//set kval ran to 0x14,0.65A
		Thread.Sleep(200);
		

		
	Thread.Sleep(2000);

		Motor_SetParam(F1_MOTO_RLOADING_TX1,F1_MOTO_RLOADING_TX0,0x0a,0x0a000000);							//set kval ran to 0x14,0.65A
		Thread.Sleep(200);
		Move_Motor(RLOADING_Motor,true,F1_LS_03_Direct,F1_LS_RLOADMOTOR_DN,200);
		Motor_SetParam(F1_MOTO_RLOADING_TX1,F1_MOTO_RLOADING_TX0,0x0a,0x07000000);							//set kval ran to 0x14,0.65A
		Thread.Sleep(200);

		Move_Motor(RDANCER_Motor,false,F1_LS_03_Direct,F1_LS_RDANCER_DN,10);

		Motor_SetParam(F1_MOTO_LLOADING_TX1,F1_MOTO_LLOADING_TX0,0x0a,0x0a000000);							//set kval ran to 0x2c- 0.9A
		Thread.Sleep(200);
		Move_Motor(LLOADING_Motor,false,F1_LS_02_Direct,F1_LS_LLOADMOTOR_DN,300);
		Motor_SetParam(F1_MOTO_LLOADING_TX1,F1_MOTO_LLOADING_TX0,0x0a,0x0a000000);							//set kval ran to 0x2c- 0.9A
		Thread.Sleep(200);
		
		Move_Motor(LDANCER2_Motor,true,F1_LS_02_Direct,F1_LS_LSPARE1,450);

		Move_Motor(LDANCER1_Motor,true,F1_LS_02_Direct,F1_LS_LDANCER1_DN,450);

	Thread.Sleep(2000);
		}
		

		
}

 void Move_Motor(UInt32 MotorID,  bool Direction, UInt32 Register , UInt32 Bits, Double speed)
{
int counter ;


	stubManager.Run<StubMotorRunResponse>("StubMotorRunRequest" ,MotorID,Direction,speed);				//move Lloading up
	counter =0;
	while ( counter < 1500)
	{
		counter = counter + 1;
		var response = stubManager.Run<StubFpgaReadRegResponse>("StubFpgaReadRegRequest" ,Register);	//if LS_LLOADMOTOR_UP is up then move down
		if ((response.Value & Bits) != Bits  )
			counter=1650;
		
	Thread.Sleep(20);

	}
		stubManager.Run<StubMotorStopResponse>("StubMotorStopRequest" ,MotorID, 1);					//hold motor

	
}
 
 int Motor_SetParam(Int32 HighAdr, Int32 LowAdr, Int32 ParaAddr, uint ParaData1)
{
	
	Int32 Temp = 0;
	Temp = (ParaAddr << 8 ) + ( (Int32) ParaData1 >> 24);

	stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,HighAdr, (Temp & 0xffff));
	Temp = ((Int32)ParaData1 >> 8) & 0xffff;
	stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,LowAdr, (Temp));
		
	return 1;
}