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.UI;

const Int32 FPGA1 =  0x60000000;
const Int32 FPGA2 =  0x60000400;
const Int32 FPGA3 =  0x60000800;
const Int32 TEST_REG = 0x3f0; //F1 TEST_REG

//TX 1-MSW 0-LSW
const Int32 MOTO_LDRIVING_1 = 0x0222;
const Int32 MOTO_LDRIVING_0 = 0x0220;
const Int32 MOTO_LLOADING_1 = 0x0232;
const Int32 MOTO_LLOADING_0 = 0x0230;
const Int32 MOTO_LDANCER1_1 = 0x02c2;
const Int32 MOTO_LDANCER1_0 = 0x02c0;
const Int32 MOTO_LDANCER2_1 = 0x02d2;
const Int32 MOTO_LDANCER2_0 = 0x02d0;
const Int32 MOTO_LPIVOT1_1 = 0x302;
const Int32 MOTO_LPIVOT1_0 = 0x300;

int Motor_HighZ(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
       
    //Send "highz" command - 1 byte
	var response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+TEST_REG, 0xa800);
	var respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+HighAdr, 0xa800); // Stop x_HARD_HIZ 0xA8 + NOP
	
	//Send "nop" command - last 2 bytes
	response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+TEST_REG, 0x0000);
	respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+LowAdr, 0x0000);
	
	return 1;
}

int Motor_Run(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
	//Send "run" command - first 2 bytes
	var response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+TEST_REG, 0x5000);
	var respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + HighAdr, 0x5000);//x_RUN 0x50 + Direction 0

	
	//Send "run" command - last 2 bytes
	response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+TEST_REG, 0xffff);
	respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + LowAdr, 0xffff);//Max Speed

	return 1;
}

int Motor_Move(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
	
	 for (int i = 0; i < 8; i++)
    {
    //Send "Move" command - First 2 bytes - clockwise
	var response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + TEST_REG, 0x4000); 
	var respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + HighAdr, 0x4000); //x_MOVE 0x40 + Direction 0
	
	//Send "Move" command - last 2 bytes
	response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + TEST_REG, 0x3200);
	respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + LowAdr, 0x3200); // Position
	
	Thread.Sleep(800);
	
	//Send "Move" command - First 2 bytes - clockwise
	response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + TEST_REG, 0x4100); 
	respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + HighAdr, 0x4100);//x_MOVE 0x40 + Direction 1 
	
	//Send "Move" command - last 2 bytes
	response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + TEST_REG, 0x3200);
	respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + LowAdr, 0x3200); //Position
	
	Thread.Sleep(800);
        
    }

	return 1;
}


public void OnExecute(StubManager stubManager)
{
	Motor_HighZ(FPGA1, MOTO_LDRIVING_1, MOTO_LDRIVING_0);
	Motor_HighZ(FPGA1, MOTO_LLOADING_1, MOTO_LLOADING_0);
	Motor_HighZ(FPGA1, MOTO_LDANCER1_1, MOTO_LDANCER1_0);
	Motor_HighZ(FPGA1, MOTO_LDANCER2_1, MOTO_LDANCER2_0);
	Motor_HighZ(FPGA1, MOTO_LPIVOT1_1, MOTO_LPIVOT1_0);
	
	Motor_Run(FPGA1, MOTO_LDRIVING_1, MOTO_LDRIVING_0);
	Thread.Sleep(6000);	
	Motor_HighZ(FPGA1, MOTO_LDRIVING_1, MOTO_LDRIVING_0);

	Motor_Move(FPGA1, MOTO_LLOADING_1, MOTO_LLOADING_0);
	Motor_HighZ(FPGA1, MOTO_LLOADING_1, MOTO_LLOADING_0);	
	
	Motor_Move(FPGA1, MOTO_LDANCER1_1, MOTO_LDANCER1_0);
	Motor_HighZ(FPGA1, MOTO_LDANCER1_1, MOTO_LDANCER1_0);	
	
	Motor_Move(FPGA1, MOTO_LDANCER2_1, MOTO_LDANCER2_0);
	Motor_HighZ(FPGA1, MOTO_LDANCER2_1, MOTO_LDANCER2_0);

	Motor_Move(FPGA1, MOTO_LPIVOT1_1, MOTO_LPIVOT1_0);
	Motor_HighZ(FPGA1, MOTO_LPIVOT1_1, MOTO_LPIVOT1_0);	

	
}