/*
 * Probabilistic tests
 *
 * Description of the script: 
 *  The script runs a set of repeatable tests a certain number of times and is expected to fail.
 * 
 * Note:
 *  This is a rather difficult script to learn, since it uses a large number of commands and structures.
 * 
 * Important: 
 *  To run the script, upload it to the mDrive Direct Control software 
 */

var global_axis;
var status;

const LEFT = 0;
const RIGHT = 1;

const FIRST_BRD     = -20;
const SECOND_BRD    = 20;

const delay = 100;

function SetDefaultSettings()
{  
    // SFBS settings
    SFBS = global_axis.get_feedback_settings();
    SFBS.IPS = 4000;
    SFBS.FeedbackType = FEEDBACK_NONE;
    SFBS.FeedbackFlags = 0;
    global_axis.set_feedback_settings(SFBS);
    msleep(100);

    // SHOM settings
    var SHOM = global_axis.get_home_settings();
    SHOM.FastHome = 1000;
    SHOM.uFastHome = 0;
    SHOM.SlowHome = 20;
    SHOM.uSlowHome = 0;
    SHOM.HomeDelta = 500;
    SHOM.uHomeDelta = 0;
    SHOM.HomeFlags = HOME_DIR_SECOND | HOME_STOP_FIRST_LIM | HOME_STOP_SECOND_REV;
    global_axis.set_home_settings(SHOM);
    msleep(100);

    // SMOV settings
    var SMOV = global_axis.get_move_settings();
    SMOV.Speed = 1000;
    SMOV.uSpeed = 0;
    SMOV.Accel = 1000;
    SMOV.Decel = 2000;
    SMOV.AntiplaySpeed = 50;
    SMOV.uAntiplaySpeed = 0;
    global_axis.set_move_settings(SMOV);
    msleep(100);

    // SENG settings
    var SENG = global_axis.get_engine_settings();
    SENG.NomVoltage = 1200;	// Rated voltage. Controller will keep the voltage drop on motor below this value if ENGINE_LIMIT_VOLT flag is set(Used with DC only). Range: 1..65535
    SENG.NomCurrent = 500;	// Rated current. Controller will keep current consumed by motor below this value if ENGINE_LIMIT_CURR flag is set. Range: 1..65535
    SENG.NomSpeed = 5000;	// Nominal speed (in whole steps / s or rpm for DC and stepper motor as a master encoder). Controller will keep motor shaft RPM below this value if ENGINE_LIMIT_RPM flag is set. Range: 1..1000000
    SENG.uNomSpeed = 0;        // Rated RPM in 1/256 microsteps(Used with steper motor only). Range: 0..255
    //SENG.EngineFlags = ENGINE_ACCEL_ON | ENGINE_LIMIT_VOLT | ENGINE_LIMIT_CURR | ENGINE_LIMIT_RPM;	// Set of flags specify motor shaft movement algorithm and list of limitations
    SENG.EngineFlags = ENGINE_ACCEL_ON | ENGINE_LIMIT_VOLT | ENGINE_LIMIT_CURR;	// Set of flags specify motor shaft movement algorithm and list of limitations
    SENG.Antiplay = 50;	// Number of pulses for backlash (play) compensation procedure. Used if ENGINE_ANTIPLAY flag is set. Range: 1..65535
    SENG.MicrostepMode = MICROSTEP_MODE_FRAC_256;	// Settings of microstep mode(Used with steper motor only)
    SENG.StepsPerRev = 200;	// Number of full steps per revolution(Used with steper motor only). Range: 1..65535
    global_axis.set_engine_settings(SENG);
    msleep(100);

    // SENT settings
    var SENT = global_axis.get_entype_settings();
    SENT.EngineType = ENGINE_TYPE_STEP;	// Engine type
    SENT.DriverType = DRIVER_TYPE_INTEGRATE;	// Driver type
    global_axis.set_entype_settings(SENT);
    msleep(100);

    // SPWR settings
    var SPWR = global_axis.get_power_settings();
    SPWR.HoldCurrent = 60;	// Current in holding regime, percent of nominal. Range: 0..100
    SPWR.CurrReductDelay = 1500;	// Time in ms from going to STOP state to reducting current. Range: 0..65535
    SPWR.PowerOffDelay = 3600;	// Time in s from going to STOP state to turning power off. Range: 0..65535
    SPWR.CurrentSetTime = 600;	// Time in ms to reach nominal current. Range: 0..65535
    SPWR.PowerFlags = POWER_REDUCT_ENABLED;	// Flags with parameters of power control
    global_axis.set_power_settings(SPWR);
    msleep(100);

    // SSEC settings
    var SSEC = global_axis.get_secure_settings();
    SSEC.LowUpwrOff = 800;	// Lower voltage limit to turn off the motor (10 mV)
    SSEC.CriticalIpwr = 3000;	// Motor maximum current (mA)
    SSEC.CriticalUpwr = 4000;	// Motor maximum voltage (10 mV)
    SSEC.CriticalT = 800;	// Maximum temperature (0.1 C)
    SSEC.CriticalIusb = 450;	// Maximum USB current
    SSEC.CriticalUusb = 520;	// Maximum USB voltage (10 mV)
    SSEC.MinimumUusb = 420;	// Minimum USB voltage (10 mV)
//    SSEC.Flags = ALARM_ON_DRIVER_OVERHEATING | LOW_UPWR_PROTECTION | H_BRIDGE_ALERT | ALARM_ON_BORDERS_SWAP_MISSET;	// Critical parameter flags
    SSEC.Flags = ALARM_ON_DRIVER_OVERHEATING |  H_BRIDGE_ALERT | ALARM_ON_BORDERS_SWAP_MISSET;
    global_axis.set_secure_settings(SSEC);
    msleep(100);

    // SEDS settings
    var SEDS = global_axis.get_edges_settings();
    SEDS.BorderFlags = BORDER_STOP_LEFT | BORDER_STOP_RIGHT;	// Border flags, specify types of borders and motor behaviour on borders
    SEDS.EnderFlags = ENDER_SW1_ACTIVE_LOW | ENDER_SW2_ACTIVE_LOW;	// Ender flags, specify electrical behaviour of limit switches like order and pulled positions
    SEDS.LeftBorder = -1000;	// Left border position, used if BORDER_IS_ENCODER flag is set. Range: -2147483647..2147483647
    SEDS.uLeftBorder = 0;	// Left border position in 1/256 microsteps(used with stepper motor only). Range: -255..255
    SEDS.RightBorder = 1000;	// Right border position, used if BORDER_IS_ENCODER flag is set. Range: -2147483647..2147483647
    SEDS.uRightBorder = 0;	// Right border position in 1/256 microsteps. Range: -255..255(used with stepper motor only)
    global_axis.set_edges_settings(SEDS);
    msleep(100);

    // SPID settings
    var SPID = global_axis.get_pid_settings();
    SPID.KpI = 4000;	// Proportional gain for current PID routine
    SPID.KiI = 20000;	// Integral gain for current PID routine
    SPID.KdI = 0;	// Differential gain for current PID routine
    SPID.KpU = 300;	// Proportional gain for voltage PID routine
    SPID.KiU = 1000;	// Integral gain for voltage PID routine
    SPID.KdU = 0;	// Differential gain for voltage PID routine
    SPID.KpSpeed = 200;	// Proportional gain for speed PID routine
    SPID.KiSpeed = 3000;	// Integral gain for speed PID routine
    SPID.KdSpeed = 0;	// Differential gain for speed PID routine
    SPID.KpPos = 30000;	// Proportional gain for position PID routine
    SPID.KiPos = 3000;	// Integral gain for position PID routine
    SPID.KdPos = 0;	// Differential gain for position PID routine
    SPID.kpe1 = 0;	// Proportional gain for ext1 PID routine
    SPID.kie1 = 0;	// Integral gain for ext1 PID routine
    SPID.kde1 = 0;	// Differential gain for ext1 PID routine
    SPID.kpe2 = 0;	// Proportional gain for ext2 PID routine
    SPID.kie2 = 0;	// Integral gain for ext2 PID routine
    SPID.kde2 = 0;	// Differential gain for ext2 PID routine
    SPID.kpe3 = 0;	// Proportional gain for ext3 PID routine
    SPID.kie3 = 0;	// Integral gain for ext3 PID routine
    SPID.kde3 = 0;	// Differential gain for ext3 PID routine
    global_axis.set_pid_settings(SPID);
    msleep(100);

    // SSNI settings
    var SSNI = global_axis.get_sync_in_settings();
    SSNI.SyncInFlags = 0;	// Input synchronization flags
    SSNI.ClutterTime = 2000; // Contacts clutch suppression in mks
    SSNI.Position = 0;	// Desired position or shift (whole steps)
    SSNI.uPosition = 0;	// The fractional part of a position or shift in microsteps (-255 .. 255)(is only used with stepper motor)
    SSNI.Speed = 500;	// Target speed(for stepper motor: steps / c, for DC: rpm). Range: 0..1000000.
    SSNI.uSpeed = 0;	// Target speed in microsteps/s. Using with stepper motor only. Range: 0..255.
    global_axis.set_sync_in_settings(SSNI);
    msleep(100);

    // SSNO settings
    var SSNO = global_axis.get_sync_out_settings();
    SSNO.SyncOutFlags = SYNCOUT_ONSTART | SYNCOUT_ONSTOP;	// Output synchronization flags
    SSNO.SyncOutPulseSteps = 100;	// Output synchronization pulse duration or input synchronization pulse dead time. Range: 0..65535
    SSNO.SyncOutPeriod = 2000;	// This value specifies number of encoder pulses between two output synchronization pulses when TTL_SYNCOUT_ONPERIOD is set. Range: 0..65535
    global_axis.set_sync_out_settings(SSNO);
    msleep(100);

    // SEIO settings
    var SEIO = global_axis.get_extio_settings();
    SEIO.EXTIOSetupFlags = EXTIO_SETUP_OUTPUT;	// Configuration flags of the external I-O
    SEIO.EXTIOModeFlags = EXTIO_SETUP_MODE_OUT_OFF;	// Flags mode settings external I-O
    global_axis.set_extio_settings(SEIO);
    msleep(100);

    // SBRK settings
    var SBRK = global_axis.get_brake_settings();
    SBRK.t1 = 300;	// Time in ms between turn on motor power and turn off brake. Range: 0..65535
    SBRK.t2 = 500;	// Time in ms between turn off brake and moving readiness. All moving commands will execute after this interval. Range: 0..65535
    SBRK.t3 = 300;	// Time in ms between motor stop and turn on brake. Range: 0..65535
    SBRK.t4 = 400;	// Time in ms between turn on brake and turn off motor power. Range: 0..65535
    SBRK.BrakeFlags = BRAKE_ENG_PWROFF;	// Flags
    global_axis.set_brake_settings(SBRK);
    msleep(100);

    // SCTL settings
    var SCTL = global_axis.get_control_settings();
    for(var i=0; i < 10; i++) // Set default settings first
    {
         SCTL.MaxSpeed[i] = 0;	// Array of speeds (full step) using with joystick and button control. Range: 0..1000000
         SCTL.uMaxSpeed[i] = 0;	// Array of speeds (1/256 microstep) using with joystick and button control. Range: 0..255
         SCTL.Timeout[i] = 1000;	// timeout[i] is time in ms, after that max_speed[i] is applying. It is using with buttons control only. Range: 0..65535
    } // Fix some settings later
    SCTL.MaxSpeed[0] =  1;
    SCTL.MaxSpeed[1] =  10;
    SCTL.MaxSpeed[2] =  100;
    SCTL.MaxSpeed[3] =  1000;
    SCTL.MaxSpeed[4] =  10000;
    SCTL.Timeout[0] = 200;
    SCTL.Timeout[1] = 500;
    SCTL.Timeout[2] = 800;
    SCTL.Flags = CONTROL_MODE_OFF;	// Flags
    global_axis.set_control_settings(SCTL);
    msleep(100);

   
    // SJOY settings
    var SJOY = global_axis.get_joystick_settings();
    SJOY.JoyLowEnd = 0;	// Joystick lower end position
    SJOY.JoyCenter = 5000;	// Joystick center position
    SJOY.JoyHighEnd = 10000;	// Joystick higher end position
    SJOY.ExpFactor =100;	// Exponential nonlinearity factor
    SJOY.DeadZone = 50;	// Joystick dead zone
    global_axis.set_joystick_settings(SJOY);
    msleep(100);

    // SCTP settings
    var SCTP = global_axis.get_ctp_settings();
    SCTP.CTPMinError = 8;	// Minimum contrast steps from step motor encoder position, wich set STATE_CTP_ERROR flag. Measured in steps step motor. Range: 0..255
    SCTP.CTPFlags = CTP_ALARM_ON_ERROR;	// Flags
    global_axis.set_ctp_settings(SCTP);
    msleep(100);

    // SURT settings
    var SURT = global_axis.get_uart_settings();
    SURT.Speed = 115200;
    SURT.UARTSetupFlags = UART_PARITY_BIT_EVEN;
    global_axis.set_uart_settings(SURT);
    msleep(100);
}


function abs(x)
{
    return ( x == 0 ) ? 0 : ((x > 0) ? x : -x);
}

function sgn(x)
{
    return ( x == 0 ) ? 0 : ( x > 0 ) ? 1 : -1 ;
}



function test_uncomplete_command_status()
{
    const max_shift=3000;
    const min_shift=-3000;
 
    // var shift = Math.round(Math.random() * (max_shift - min_shift) + min_shift);
    var shift = 200;

    // Send shift position command to controller.
    global_axis.command_movr(shift);
    msleep(20);

    // Waiting for movement finish. 
    while ( (status = global_axis.get_status()).MoveSts & MOVE_STATE_MOVING )
    {
       msleep(20);
    }        
    msleep(20);

    // Check command status and return 0 if already is OK and 1 if we have a problem.
    if ( (status = global_axis.get_status()).MvCmdSts & MVCMD_RUNNING )    
        return 1;
    else
        return 0;
}


function test_revers_while_movement()
{
    // Start motion
    global_axis.command_right();
    msleep(200);

    // Change reverse settings while controller is moving
    engine_settings = global_axis.get_engine_settings();
    engine_settings.EngineFlags ^= ENGINE_REVERSE;
    global_axis.set_engine_settings(engine_settings);   


    // Stop the controller
    global_axis.command_stop();    
    msleep(2000);

    // Check the command status
    if (global_axis.get_status().MvCmdSts & MVCMD_RUNNING)
    {
        log(">>> Error, MVCMD_RUNNING flag is set but it shouldn't." ,1);
        // Reset for escape from bad state
        global_axis.command_reset();
        msleep(5000);
        return 1;
    }

    // Try to move again
    status = global_axis.get_status();
    var Pos = status.CurPosition * 256 + status.uCurPosition;
    global_axis.command_right();
    msleep(2000);
    global_axis.command_stop();
    status = global_axis.get_status();
    newPos = status.CurPosition*256 + status.uCurPosition;
    if (Pos == newPos)
    {
        log(">>> Error, controller couldn't move.",1);
        // Reset for escape from bad state
        global_axis.command_reset();
        msleep(5000);
        return 1;
    }

    // Return from function
    return 0;
}

function test_command_running_flag()
{
  global_axis.command_move(FIRST_BRD, 0);   // Move towards one border

  status = global_axis.get_status();
  if (status.MvCmdSts & MVCMD_RUNNING)
    command_wait_for_stop(delay);
  else
  {
    log(">>> Error, MVCMD_RUNNING flag is unset but it must set." ,1);
    msleep(5000);
	return 1;
  }

  global_axis.command_move(SECOND_BRD, 0);  // Move towards another border

  status = global_axis.get_status();
  if (status.MvCmdSts & MVCMD_RUNNING)
    command_wait_for_stop(delay);
  else
  {
    log(">>> Error, MVCMD_RUNNING flag is unset but it must set." ,1);
    msleep(5000);
	return 1;
  }

  return 0;
}

axis_serial = get_next_serial(0);
global_axis = new_axis(axis_serial);


global_axis.command_stop();
global_axis.command_zero();
SetDefaultSettings();


// Make test_uncomplete_command_status() test REPEATS counts and show error-message and iteration number if we have a problem.
const test_uncomplete_command_status_REPEATS = 1000000;

for (var counter = 0; counter <= test_uncomplete_command_status_REPEATS; counter++)
{
    if ( test_uncomplete_command_status() )
    {
        log(">>> Error ! Movement has already stopped but command status are still running.", 1);
        log("Iteration: " + counter, 3);
        break;
    }
}

const test_command_running_flag_REPEATS = 100;

for (var counter = 0; counter <= test_command_running_flag_REPEATS; counter++)
{
  if (test_command_running_flag())
  {
    log(">>> Error ! Command running is not indicated", 1);
    log("Iteration: " + counter, 3);
    break;
  }
}

// test_revers_while_movement have to be at the end of the script
const test_revers_while_movement_REPEATS = 10;

log("> Test for change revers while controller is moving.",3);

for (var counter = 0; counter <= test_revers_while_movement_REPEATS; counter++)
{
    if ( test_revers_while_movement() )
    {
        log(">>> Error ! Change revers while movement cause the bug !", 1);
        log("Iteration: " + counter, 3);
        break;
    }
}