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;

const Int32 MOTO_SCREW_TX0 = 0x0280;
const Int32 MOTO_SCREW_TX1 = 0x0282;
const Int32 MOTO_SCREW_RX0 = 0x0284;
const Int32 MOTO_SCREW_RX1 = 0x0286;

const Int32 MOTO_LDRIVING_TX1 = 0x0222;
const Int32 MOTO_LDRIVING_TX0 = 0x0220;
const Int32 MOTO_LDRIVING_RX1 = 0x0226;
const Int32 MOTO_LDRIVING_RX0 = 0x0224;

const Int32 MOTO_LLOADING_TX1 = 0x0232;
const Int32 MOTO_LLOADING_TX0 = 0x0230;

const Int32 MOTO_LDANCER1_TX1 = 0x02c2;
const Int32 MOTO_LDANCER1_TX0 = 0x02c0;

const Int32 MOTO_LDANCER2_TX1 = 0x02d2;
const Int32 MOTO_LDANCER2_TX0 = 0x02d0;
const Int32 MOTO_LDANCER2_A1_AMT_OF_WORDS = 0x02d8;

const Int32 MOTO_LPIVOT1_TX1 = 0x0302;
const Int32 MOTO_LPIVOT1_TX0 = 0x0300;
const Int32 MOTO_LPIVOT1_A1_AMT_OF_WORDS = 0x0308;

const Int32 MOTORDRIVER_HIGHZ_COMMAND_MSB = 0xa800;
const Int32 MOTORDRIVER_HIGHZ_COMMAND_LSB = 0x0000;	

const Int32 MOTORDRIVER_RUN_COMMAND_MSB = 0x5000;
const Int32 MOTORDRIVER_RUN_COMMAND_LSB = 0x0000;	

const Int32 MOTORDRIVER_MOVE_COMMAND_MSB = 0x4000;
const Int32 MOTORDRIVER_MOVE_COMMAND_LSB = 0x0000;	

const Int32 MOTORDRIVER_RDCFG_COMMAND_MSB = 0x3800;
const Int32 MOTORDRIVER_RDCFG_COMMAND_LSB = 0x0000;	


StubFpgaReadRegResponse Fpga_Read_Reg(Int32 Fpga, Int32 Addr)
{
    var response = stubManager.Run<StubFpgaReadRegResponse>("StubFpgaReadRegRequest" , Fpga + Addr);
	response.Value = response.Value & 0xffff;
	stubManager.Write("\n\nFPGA Reg. Read (FPGA Base, Addr, Data): (");	
	stubManager.WriteHex(Fpga,4);
	stubManager.Write(", ");
	stubManager.WriteHex(Addr,4);
	stubManager.Write(", ");
	stubManager.WriteHex(response.Value,4);
	stubManager.Write(")");
	response.Value = 0;
	
	return response;
}

int Fpga_Write_Reg(Int32 Fpga, Int32 Addr, Int32 Data)
{
	var respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + Addr, Data);
	return 1;
}

int Motor_HighZ(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
       
	Fpga_Write_Reg(FPGA1, HighAdr, MOTORDRIVER_HIGHZ_COMMAND_MSB);
	Fpga_Write_Reg(FPGA1, LowAdr, MOTORDRIVER_HIGHZ_COMMAND_LSB);
	
	return 1;
}

int Motor_Run(Int32 Fpga, Int32 HighAdr, Int32 LowAdr, Int32 Speed, Int32 Direction)
{
	
	var RunCmdHigh = ( ((MOTORDRIVER_RUN_COMMAND_MSB >> 8) + (Direction & 0x1)) << 8 ) + ( (Speed & 0x0fffff) >> 16);
	var RunCmdLow = (Speed & 0xffff);
	
	Fpga_Write_Reg(FPGA1, HighAdr, RunCmdHigh);
	Fpga_Write_Reg(FPGA1, LowAdr, RunCmdLow);

	return 1;
}

int Motor_Move(Int32 Fpga, Int32 HighAdr, Int32 LowAdr, Int32 NumOfSteps, Int32 Direction)
{
	
	var MoveCmdHigh = ( ((MOTORDRIVER_MOVE_COMMAND_MSB >> 8) + (Direction & 0x1)) << 8 ) + ( (NumOfSteps & 0x3fffff) >> 16);
	var MoveCmdLow = (NumOfSteps & 0xffff);
	
	Fpga_Write_Reg(FPGA1, HighAdr, MoveCmdHigh);
	Fpga_Write_Reg(FPGA1, LowAdr, MoveCmdLow);
	
	return 1;
}

int Motor_Move_Loop(Int32 Fpga, Int32 HighAdr, Int32 LowAdr, Int32 NumOfSteps, Int32 Direction, Int32 NumOfLoops) 
{

	for (int i = 0; i < NumOfLoops; i++)
	{
		Motor_Move(Fpga, HighAdr, LowAdr, NumOfSteps, Direction);
		Thread.Sleep(1000);
		Motor_Move(Fpga, HighAdr, LowAdr, NumOfSteps, Direction^0x1);
		Thread.Sleep(1000);
	}
	return 1;

}

int Motor_ReadCfg(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
	
	Fpga_Write_Reg(Fpga, HighAdr, MOTORDRIVER_RDCFG_COMMAND_MSB);
	Fpga_Write_Reg(Fpga, LowAdr, MOTORDRIVER_RDCFG_COMMAND_LSB);
	Fpga_Read_Reg(Fpga, HighAdr+4);
	Fpga_Read_Reg(Fpga, LowAdr+4);
		
	return 1;
}


public void OnExecute(StubManager stubManager)
{
	Motor_HighZ(FPGA1, MOTO_SCREW_TX1, MOTO_SCREW_TX0);
	//Motor_HighZ(FPGA1, MOTO_LLOADING_TX1, MOTO_LLOADING_TX0);
	//Motor_HighZ(FPGA1, MOTO_LDANCER1_TX1, MOTO_LDANCER1_TX0);
	//Motor_HighZ(FPGA1, MOTO_LDANCER2_TX1, MOTO_LDANCER2_TX0);
	//Motor_HighZ(FPGA1, MOTO_LPIVOT1_TX1, MOTO_LPIVOT1_TX0);
	
	Motor_ReadCfg(FPGA1, MOTO_SCREW_TX1, MOTO_SCREW_TX0);
	//Motor_ReadCfg(FPGA1, MOTO_LLOADING_TX1, MOTO_LLOADING_TX0);
	//Motor_ReadCfg(FPGA1, MOTO_LDANCER1_TX1, MOTO_LDANCER1_TX0);
	//Motor_ReadCfg(FPGA1, MOTO_LDANCER2_TX1, MOTO_LDANCER2_TX0);
	//Motor_ReadCfg(FPGA1, MOTO_LPIVOT1_TX1, MOTO_LPIVOT1_TX0);
	
	
	Motor_Run(FPGA1, MOTO_SCREW_TX1, MOTO_SCREW_TX0, 800, 0x1);
	Thread.Sleep(10000);	
	Motor_HighZ(FPGA1, MOTO_SCREW_TX1, MOTO_SCREW_TX0);
/*
	Motor_Move_Loop(FPGA1, MOTO_LLOADING_TX1, MOTO_LLOADING_TX0, 0x3200, 0x1, 4);
	Motor_HighZ(FPGA1, MOTO_LLOADING_TX1, MOTO_LLOADING_TX0);	

	Motor_Move_Loop(FPGA1, MOTO_LDANCER1_TX1, MOTO_LDANCER1_TX0, 0x3200, 0x1, 4);
	Motor_HighZ(FPGA1, MOTO_LDANCER1_TX1, MOTO_LDANCER1_TX0);	
	
//	Motor_Move_Loop(FPGA1, MOTO_LDANCER2_TX1, MOTO_LDANCER2_TX0, 0x3200, 0x1, 4);
//	Motor_HighZ(FPGA1, MOTO_LDANCER2_TX1, MOTO_LDANCER2_TX0);

	Motor_Move_Loop(FPGA1, MOTO_LPIVOT1_TX1, MOTO_LPIVOT1_TX0, 0x3200, 0x1, 4);
	Motor_HighZ(FPGA1, MOTO_LPIVOT1_TX1, MOTO_LPIVOT1_TX0);	

*/	
}

/* Script outputs motor driver chip config register
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0226, #002E)

FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0224, #8800)

FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0236, #002E)

FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0234, #8800)

FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #02C6, #002E)

FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #02C4, #8800)

FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #02D6, #002E)

FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #02D4, #8800)

FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0306, #002E)

FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0304, #8800)

*/