/*
 * Closed loop tuning test
 *
 */

var global_axis;

global_axis = new_axis(get_next_serial(0));
global_axis.command_stop();

const DEBUG = 1;
const MICROSTEPS = 256;
const SKIP_ENCODER_COUNT = 7;
const STORE_COUNT_MICROSTEPS = 19;
const TRUST_INDEX = 15;     // Always TRUST_INDEX < STORE_COUNT_MICROSTEPS

/*
 * Save and overwrite settings
 */
var SFBS = global_axis.get_feedback_settings(); // Save information about encoder (IPS)
SFBS.FeedbackType = FEEDBACK_NONE;              // Overwrite feedback type, because in profile feedback is encoder

var SENG = global_axis.get_engine_settings();   // Save information about engine (nominal current, steps per revolution)

var SENT = global_axis.get_entype_settings();   // Save information about engine type

var SEDS = global_axis.get_edges_settings();    // Save information about edges

/*
 * Clear FRAM for synchronization
 * real full step with full step of firmware
 */
log("Clearing FRAM", 1);
global_axis.command_clear_fram();
msleep(4000);

/*
 * Restore controller settings
 */
global_axis.set_feedback_settings(SFBS);
msleep(100);

global_axis.set_engine_settings(SENG);
msleep(100);

global_axis.set_entype_settings(SENT);
msleep(100);

global_axis.set_edges_settings(SEDS);
msleep(100);

/*
 * Prepare and apply move settings
 */
var SMOV = global_axis.get_move_settings();
SMOV.Speed = 0;
SMOV.uSpeed = 16;
global_axis.set_move_settings(SMOV);
msleep(100);

/*
 * Going to the full step and waiting 3 seconds for equilibration
 */
global_axis.command_move(0, 0);
msleep(3000);

/*
 * Arrays for measurements and structure for GPOS
 */
var MicroStepsToRight = [];
var MicroStepsToLeft = [];
var EncToRight = [];
var EncToLeft = [];
var GPOS;

/*
 * Start moving
 */
global_axis.command_right();

/*
 * Skipping a some first counts for the stable experiment
 */
for (var i = 0; i < SKIP_ENCODER_COUNT; i++)
{
  GPOS = global_axis.get_position();
  var EncPos = GPOS.EncPosition;

  while (EncPos == GPOS.EncPosition)
  {
    msleep(40);
    GPOS = global_axis.get_position();
  }

  EncPos = GPOS.EncPosition;
}

/*
 * Start measurements
 */
for (var i = 0; i < STORE_COUNT_MICROSTEPS; i++)
{
  GPOS = global_axis.get_position();
  var EncPos = GPOS.EncPosition;

  while (EncPos == GPOS.EncPosition)
  {
    msleep(40);
    GPOS = global_axis.get_position();
  }

  EncPos = GPOS.EncPosition;

  MicroStepsToRight[i] = GPOS.Position * MICROSTEPS + GPOS.uPosition;
  EncToRight[i] = GPOS.EncPosition;
}

/*
 * Stop moving
 */
global_axis.command_stop();

/*
 *  Start moving and measurements
 */
global_axis.command_left();

for (var i = 0; i < STORE_COUNT_MICROSTEPS; i++)
{
  GPOS = global_axis.get_position();
  var EncPos = GPOS.EncPosition;

  while (EncPos == GPOS.EncPosition)
  {
    msleep(40);
    GPOS = global_axis.get_position();
  }

  EncPos = GPOS.EncPosition;

  MicroStepsToLeft[STORE_COUNT_MICROSTEPS - 1 - i] = GPOS.Position * MICROSTEPS + GPOS.uPosition;
  EncToLeft[STORE_COUNT_MICROSTEPS - 1 - i] = GPOS.EncPosition;
}

/*
 * Stop moving
 */
global_axis.command_stop();

/*
 * Check all values
 */
for (var i = 0; i < STORE_COUNT_MICROSTEPS; i++)
{
  var diffMicrosteps = MicroStepsToRight[i] - MicroStepsToLeft[i];
  var diffConts = EncToRight[i] - EncToLeft[i];

  if (DEBUG)
  {
    log(MicroStepsToRight[i] + " - " + MicroStepsToLeft[i] + " = " + diffMicrosteps + " microstep(s)\t" +
      EncToRight[i] + " - " + EncToLeft[i] + " = " + diffConts + " count(s)", 3);
  }

  if (diffConts != 1)
  {
    log("Script error! Try again", 1);
  }
}

var RightB = (MicroStepsToRight[TRUST_INDEX] * EncToRight[0] - MicroStepsToRight[0] * EncToRight[TRUST_INDEX]) / (EncToRight[0] - EncToRight[TRUST_INDEX]);
var LeftB = (MicroStepsToLeft[TRUST_INDEX] * EncToLeft[0] - MicroStepsToLeft[0] * EncToLeft[TRUST_INDEX]) / (EncToLeft[0] - EncToLeft[TRUST_INDEX]);

log("Right counts show B = " + RightB, 2);
log("Left counts show B = " + LeftB, 2);
log("Aver B = " + ((RightB + LeftB) / 2), 3);
