//
// Kiwi Scientific Acceleration
// ArtyJoysideApp
//
// (C) 2017 DJ Greaves - University of Cambridge, Computer Laboratory
//

// There are two demonstrations possible with this set of parts: either the inter-FPGA link is a visible resource that
// is explicitly accessed in the C# code running in various places.  Or else the C# code is written to directly call
// from one dll to another, but the HPR System Integrator automatically adapts the inter-dll protocol to 
// use the inter-FPGA link as a necessity.

//
// This tiny application acts as a data pump, shovelling the data from the Joystick over the Inter-FPGA link. It corresponds
// to the former (explict link) paradigm.
//
//
// The actual application here does almost nothing, except that in the KMeans demo it integrates the joystick velocity signals
// to give the crosshairs cursor position.
//
using System;
using KiwiSystem;


class ArtyJoysideApp
{
  const int no_of_sticks = 2;

  [Kiwi.InputBitPort("rtc50")] static bool rtc50;   // 50 Hz clock or so
  [Kiwi.OutputWordPort("mon32")] static int mon32;  // Debug monitoring
  [Kiwi.OutputWordPort("mon2")] static uint mon2;  // Debug monitoring
  [Kiwi.OutputWordPort("mon3")] static uint mon3;  // Debug monitoring
  [Kiwi.InputBitPort("recal")] static bool recal;    


  static short [] origin_x = new short[no_of_sticks]; 
  static short [] origin_y = new short[no_of_sticks]; 

  static void calibrate_origins()
  {
    for (int s = 0; s<no_of_sticks; s++)
      {
	
	int q = JoyStickUnit.ReadStick(s);
	short xpos_raw = (short)(q & 0xFFF);
	short ypos_raw = (short)((q>>16) & 0xFFF);
	origin_x[s] = xpos_raw;
	origin_y[s] = ypos_raw;
      }
  }


  static bool read_stick_button(int s)
  {
    int q = JoyStickUnit.ReadStick(s);
    return (q < 0);  // When the stick is pushed down, the msb is set.
  }
  
  static void read_stick_nice(int s, out short xx, out short yy)
  {
    int q = JoyStickUnit.ReadStick(s);
    if (q < 0)
      {
	xx = 0;    // When the stick is pushed its value is perturbed so mask here.
	yy = 0;
      }
    else
      {
	short xpos_raw = (short)(q & 0xFFF);
	short ypos_raw = (short)((q>>16) & 0xFFF);
	xx = (short)((xpos_raw - origin_x[s])/16);
	yy = (short)((ypos_raw - origin_y[s])/16);
      }
  }



  static int recalibrate_time = 1000;

  public static void RunServer()
  {
    Kiwi.Pause();

    calibrate_origins();
    mon32 = 100;

    Kiwi.KppMark("CALIBRATED");

    const int width = 512;
    const int height = 512;

    short pos_x = width/2, pos_y = height/2;

    bool failed = false;
    bool old_rtc50 = false;
    bool npush0, oldpush0 = false;
    bool npush1;

    Kiwi.KppMark("MAINLOOP");
    while(true)
      {

	if (recalibrate_time > 0)
	  {
	    if (recalibrate_time == 1) calibrate_origins();
	    recalibrate_time -= 1;
	  }

	npush0 = read_stick_button(0);
	if (npush0 && !oldpush0)
	  {
//	    if (false) adapt_centroids(kmd);
	  }
	oldpush0 = npush0;

	npush1 = read_stick_button(1);

	while (old_rtc50 == rtc50)
	  {
	    Kiwi.Pause();
	  }
	old_rtc50 = rtc50;

	if (recal)
	  {
	    calibrate_origins();
	    continue;
	  }


	Kiwi.Pause(); 

	short stick_xx, stick_yy;
	read_stick_nice(0, out stick_xx, out stick_yy);
	short vel_x = stick_xx;
	short vel_y = stick_yy;

	pos_x = (short)(pos_x - vel_x);   // Correct joystick sign and graphical +ve y being down.
	pos_y = (short)(pos_y - vel_y);
	const int denom = 32;

	Kiwi.Pause();	
	if (pos_x < 0) pos_x = 0; else if (pos_x >= 512*denom) pos_x = 511*denom;
	if (pos_y < 0) pos_y = 0; else if (pos_y >= 512*denom) pos_y = 511*denom;
	Kiwi.Pause();	
 //	mon32 = ((stick_xx) << 16) + (stick_yy);
 //	mon32 = ((pos_x/denom) << 16) + (pos_y/denom);

	uint bflags = (npush1) ? (1u << 31):0;
	bflags += (npush0) ? (1u << 30):0;
	mon2 = bflags;

	uint lo_word = bflags;  // + (uint)((pos_x/denom) << 16) + (uint)(pos_y/denom);

	uint hi_word = (uint)(vel_x << 16) + (uint)(vel_y);

	bool fail = DuplexInterFPGALink.Write64((ulong)lo_word | (((ulong)hi_word)<<32));
	failed |= fail;
	Kiwi.Pause();	
      }
    return;
  }
}

 
public static class JoysideBench
{
  [Kiwi.OutputBitPort("done")] static bool done;  
 
  public static void Main()
  {
    RunHW();
  }


  [Kiwi.HardwareEntryPoint()]
  public static void RunHW()
  {
    Console.WriteLine("Kiwi Joyside App --- Start");
    Kiwi.KppMark("START");
    ArtyJoysideApp.RunServer();
    Kiwi.KppMark("FINISH");
    Console.WriteLine("KMeans Demo Finish");
    done = true;
  }
}


// eof