/*
 * Autotester script
 *
 * The script tests the controller with a positioner using a set of tests.
 */

var global_axis;
const LEFT = 0;
const RIGHT = 1;
const delay = 100;

function SetDefaultSettings()
{  
    global_axis.command_clear_fram();
    msleep(5000);

    // 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..100000
    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 = 5000;	// 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.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
    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..100000.
    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..100000
         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);
    
    global_axis.command_stop();
}


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

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


function test_move_left()
{
    var  NumOfIterations = 100;

   log("> Checking movement to the left hand.",3);

    state = global_axis.get_status();
    var OldPos = state.CurPosition;
    global_axis.command_left();
    msleep(100);
    for (var i = 0; i <= NumOfIterations; i++)
    {
        state = global_axis.get_status();
        dPos = state.CurPosition - OldPos;
        OldPost = state.CurPosition;
        if (state.CurSpeed > 0)
        {
            log("Error ! Moving direction is left but speed > 0.",1);
            return 1;
        }

        if (dPos > 0)
        {
            log("Error ! Moving direction is left but delta position > 0", 1);
            return 1;
        }
        msleep(100);
    } 
    msleep(200);
    global_axis.command_stop();
    log(">>> OK", 3);
}

function test_move_right()
{
    var  NumOfIterations = 100;

    log("> Checking movement to the right hand.",3);

    state = global_axis.get_status();
    var OldPos = state.CurPosition;
    global_axis.command_right();
    msleep(200);
    for (var i = 0; i <= NumOfIterations; i++)
    {
        state = global_axis.get_status();
        dPos = state.CurPosition - OldPos;
        OldPost = state.CurPosition;
        if (state.CurSpeed < 0)
        {
            log("Error ! Moving direction is right but speed > 0.",1);
            return 1;
        }

        if (dPos < 0)
        {
            log("Error ! Moving direction is right but delta position > 0", 1);
            return 1;
        }
        msleep(100);
    } 
    msleep(200);
    global_axis.command_stop();
    log(">>> OK", 3);
}

function test_check_sstp()
{
	
   var NumOfIterations = 10;
   var ErrorRange = 0.1;
   
   var move_settings = global_axis.get_move_settings();

   log("> Checking sstp command.", 3);
   global_axis.command_left();
   msleep(5000);
   global_axis.command_sstp();
   msleep(200);

   var time = 200;
   engine_settings = global_axis.get_engine_settings();
   
   while ( (global_axis.get_status().MoveSts & MOVE_STATE_MOVING) != 0 ) 
    {        
		
        if (time > (1 + ErrorRange)*1000*move_settings.Speed/move_settings.Decel)
        {
           log(">>> Error ! Engine trying to stop for a long time.", 1);
           return 1;
        }
		
        time += 100;
        msleep(100);        
    }
    
    if ( (engine_settings.EngineFlags & ENGINE_ACCEL_ON) != 0 )
    {
        if (time < (1 - ErrorRange)*1000*move_settings.Speed/move_settings.Decel )
        {
            log(">>> Error ! Engine has stopped very soon.", 1);
            return 1;
        }
    }
     
   msleep(200);

   global_axis.command_right();
   msleep(5000);
   global_axis.command_sstp();
   msleep(200);

   var time = 200;
   var engine_settings = global_axis.get_engine_settings();
   
   while ( (global_axis.get_status().MoveSts & MOVE_STATE_MOVING) != 0 ) 
   {
        if (time > (1+ErrorRange)*1000*move_settings.Speed/move_settings.Decel )
        {
           log(">>> Error ! Engine trying to stop for a long time.", 1);
           return 1;
        }
        
        msleep(100);
        time += 100;
    }
    
    if ( (engine_settings.EngineFlags & ENGINE_ACCEL_ON) != 0 )
    {
        if (time < (1-ErrorRange)*1000*move_settings.Speed/move_settings.Decel )
        {
            log(">>> Error ! Engine has stopped very soon.", 1);
            return 1;
        }
    }
    
    msleep(200);

    log(">>> OK", 3); 
}

function test_check_sstp_when_stopped()
{
    log("> Checking sstp command when motor is stopped.", 3);

    var GETS = global_axis.get_status();
    var SMOV = global_axis.get_move_settings();

    SMOV.Accel = 1000;
    SMOV.Decel = 500;

    global_axis.set_move_settings(SMOV);

    var Position = GETS.CurPosition;
    var uPosition = GETS.uCurPosition;

    for (var i = 0; i < 10; i++)
    {
        global_axis.command_sstp();
        msleep(200);
    }

    GETS = global_axis.get_status();

    if (GETS.CurPosition == Position && GETS.uCurPosition == uPosition)
        log(">>> OK", 3);
    else
    {
        log(">>> Error! Engine moving when must stop.", 1);
        return 1;
    }
}

function test_check_stop()
{
    var  NumOfIterations = 10;

    log("> Checking stop command.", 3);
    global_axis.command_left();
    msleep(3000);
    global_axis.command_stop();
    msleep(200);
    state = global_axis.get_status();
    OldPos = state.CurPosition;
    msleep(200);
    for (var i = 0; i <= NumOfIterations; i++)
    {
        state = global_axis.get_status();
        dPos = state.CurPosition - OldPos;
        OldPos = state.CurPosition;
        if (state.CurSpeed != 0)
        {
            log(">>> Error ! Engine has stopped but speed is not zero.",1);
            return 1;
        }

        if (dPos != 0)
        {
            log(">>> Error ! Engine has stopped bug dPos is not zero.", 1);
            return 1;
        }
        msleep(100);
    } 

    global_axis.command_right();
    msleep(3000);
    global_axis.command_stop();
    msleep(200);
    state = global_axis.get_status();
    OldPos = state.CurPosition;
    msleep(200);
    for (var i = 0; i <= NumOfIterations; i++)
    {
        state = global_axis.get_status();
        dPos = state.CurPosition - OldPos;
        OldPos = state.CurPosition;
        if (state.CurSpeed != 0)
        {
            log(">>> Error ! Engine has stopped but speed is not zero.",1);
            return 1;
        }

        if (dPos != 0)
        {
            log(">>> Error ! Engine has stopped bug dPos is not zero.", 1);
            return 1;
        }
        msleep(100);
    } 

    log(">>> OK", 3 );
}

function test_zero(DstPos)
{
// Testing
    log("> Checking zeroing...", 3);
    global_axis.command_move(DstPos); 
    msleep(30);
    while ( (global_axis.get_status().MoveSts & MOVE_STATE_MOVING) != 0 )  {  msleep(100);   }
    if (global_axis.get_status().CurPosition != DstPos)
    {
        log(">> Error while going to " + DstPos, 2);
        return 1;
    }
    global_axis.command_zero();
    var Position = global_axis.get_status().CurPosition;
    if (global_axis.get_status().CurPosition == 0)
    {
        log(">>> OK", 3);
        return 0;
    }
    else
    {
        log(">>> Error !", 1);
        return 1;
    }
    log(">>> OK, 3");
    return 0;
}

function test_change_direction()
{
//     
    log("> Checking direction change.");
    var OldPos;
    var status;
    var dPos;

    global_axis.command_left();
    OldPos = global_axis.get_status().CurPosition;
    msleep(200);
    for(var i = 0; i < 100; i++)
    {
        status = global_axis.get_status();
        msleep(100);
        dPos = status.CurPosition - OldPos;
        OldPos = status.CurPosition;
        if ( (status.CurSpeed > 0) || (dPos > 0) )
        {
            log("Error ! Incorrect direction.",1);
            return 1;
        }        
    }

    global_axis.command_right();
   
    engine_settings = global_axis.get_engine_settings();
    if (engine_settings.EngineFlags & ENGINE_ACCEL_ON)
    {
         move_settings = global_axis.get_move_settings();
         var StoppingTime = move_settings.Speed / move_settings.Decel * 1000;
         for (var i = 0; i < StoppingTime + 100; i += 100 )
              msleep(100);
    }
    


    OldPos = global_axis.get_status().CurPosition;
    msleep(200);
    for(var i = 0; i < 100; i++)
    {
        status = global_axis.get_status();
        dPos = status.CurPosition - OldPos;
        OldPos = status.CurPosition;
        if ( (status.CurSpeed < 0) || (dPos < 0) )
        {
            log("Error ! Incorrect direction.",1);
            return 1;
        }        
        msleep(100);
    }

    global_axis.command_left();
    msleep(200);

    engine_settings = global_axis.get_engine_settings();
    if (engine_settings.EngineFlags & ENGINE_ACCEL_ON)
    {
         move_settings = global_axis.get_move_settings();
         var StoppingTime = move_settings.Speed/move_settings.Decel * 1000;
         for (var i = 0; i < StoppingTime + 100; i += 100)
              msleep(100);
    }


    OldPos = global_axis.get_status().CurPosition;
    msleep(200);
    for(var i = 0; i < 100; i++)
    {
        status = global_axis.get_status();
        dPos = status.CurPosition - OldPos;
        OldPos = status.CurPosition;
        if ( (status.CurSpeed > 0) || (dPos > 0) )
        {
            log("Error ! Incorrect direction.",1);
            return 1;
        }        
    }       

    global_axis.command_stop();
    log(">>> OK", 3);
    return 0;
}

function test_moveto(DstPos)
{
    log("> Checking move to...", 3);
    global_axis.command_move(DstPos); 
    msleep(200);
    while ( (global_axis.get_status().MoveSts & MOVE_STATE_MOVING) != 0 )  {  msleep(100);   }

    if (global_axis.get_status().CurPosition != DstPos)
    {
        log(">>> Error while going to " + DstPos, 1);
        return 1;
    }

    log(">>> OK", 3);
    return 0;
}

function test_big_speed_and_acceleration(DstPos)
{   
    var ErrorRange = 0.10;
	
    var    move_settings = global_axis.get_move_settings();
    Speed = move_settings.Speed;
    Accel = move_settings.Accel;
    Decel = move_settings.Decel;
   
// Calculating the movement time. Must be modified for the some cases. 
        var __accel_time =  move_settings.Speed/move_settings.Accel;
        var __accel_distant = move_settings.Accel*(__accel_time*__accel_time)/2;
        var __decel_time = move_settings.Speed/move_settings.Decel;
        var __decel_distant = move_settings.Decel*(__decel_time*__decel_time)/2;
        var __const_speed_time = (abs(DstPos) - (__accel_distant + __decel_distant) )/move_settings.Speed;
        var FullTime = __accel_time + __decel_time + __const_speed_time;
    
    log("> Checking big speed and acceleration. Speed = "+ move_settings.Speed 
		+ ", Accel = " + move_settings.Accel + ", Decel = " 
		+ move_settings.Decel + ", TargetPosition = " + DstPos, 3);
    global_axis.command_zero();

// Testing
    var time = 200;
    global_axis.command_move(DstPos);
    msleep(200);
    var status;
    while ( ((status = global_axis.get_status().MoveSts) & MOVE_STATE_MOVING) != 0 ) 
    {            
// Checking movement time.
        if (time > ((1 + ErrorRange)*FullTime*1000))
        {
            log(">>> Error ! Movement to the position takes a long time.", 1);            
            global_axis.command_stop();
            return 1;
        }

// Checking for crossing the target position.
        if (abs(status.CurPosition) > abs(DstPos))
        {
            log (">>> Error ! Crossing target position.", 1);
            global_axis.command_stop();
            return 1;
        }
        msleep(100);   
        time += 100;
    }

// Checking for position achivement. 
    if (global_axis.get_status().CurPosition != DstPos)
    {
        log(">>> Error ! Target position does not achieved.", 1);
        return 1;
    }

        log(">>> OK", 3);
        return 0;
   
}

function test_GoToMaxIntPos()
{
    const ErrorRange = 0.10;
    const Speed         = 100000;

    var TestPosition = Math.pow(2, 31) - 1;	
    var    move_settings = global_axis.get_move_settings();
    move_settings.Speed = Speed;
    global_axis.set_move_settings(move_settings);
    var    status              = global_axis.get_status();   

    var PredictedMovingTime = 0;
    if (get_engine_settings().EngineFlags & ENGINE_ACCEL_ON)
    {
// Calculating the movement time. Must be modified for the some cases. 
        var __accel_time =  move_settings.Speed/move_settings.Accel;
        var __accel_distant = move_settings.Accel*(__accel_time*__accel_time)/2;
        var __decel_time = move_settings.Speed/move_settings.Decel;
        var __decel_distant = move_settings.Decel*(__decel_time*__decel_time)/2;
        var __const_speed_time = (abs(TestPosition - status.CurPosition) - (__accel_distant + __decel_distant) )/move_settings.Speed;
        var PredictedMovingTime = __accel_time + __decel_time + __const_speed_time;
    } else
    {
         var PredictedMovingTime = (TestPosition - status.CurPosition)/move_settings.Speed;
    }

    log("> Checking for going to max integer position ( = " + TestPosition + ").")
    global_axis.command_move(TestPosition);
    msleep(20);

    var RealMovingTime = 20;
    while((status = global_axis.get_status()).MoveSts & MOVE_STATE_MOVING) 
    { 
        if (sgn(status.CurSpeed) != sgn(TestPosition - status.CurPosition))
        {
            log(">>> Error ! Incorrect movement direction.", 1);
            global_axis.command_stop();
            return 1;
        }

        if (RealMovingTime > (ErrorRange + 1)*PredictedMovingTime*1000)   
        {
            log(">>> Error ! Moving to position takes a lot of time.", 1);
            global_axis.command_stop();
            return 1;
        }     

        msleep(20);
        RealMovingTime += 20;
    };

    if ( global_axis.get_status().CurPosition != TestPosition )
    {
        log(">>> Error ! Max integer position could not be achived.",1);
       global_axis.command_stop();
        return 1
    }

    log(">>> OK" ,3);
    global_axis.command_stop();
    return 0;
}

function test_GoToMaxIntPos2()
{
    log("Checking for overflow and incorrect movement direction while going to max integer position ",3);

    global_axis.command_zero();
    var TestPosition = 2147483647;	

    global_axis.command_move(100,0);
    msleep(20);
    while(global_axis.get_status().MoveSts & MOVE_STATE_MOVING) {msleep(100)}
    global_axis.command_zero();
    global_axis.command_move(TestPosition,0);
    
    msleep(1000);
    
    var status = global_axis.get_status();

    if (sgn(status.CurSpeed) != sgn(TestPosition - status.CurPosition))
    {
        log(">>> Error ! Incorrect movement direction.", 1);
        global_axis.command_stop();
        return 1; 
    } else
    {
       log(">>> OK", 3);
       global_axis.command_stop(); 
       return 0;
    }
}

function test_antiplay(DstPos)
{
    var AntiplayLeft;
    var AntiplayRight;
    var engine_settings = global_axis.get_engine_settings();
    var move_settings = global_axis.get_move_settings();
    var Antiplay = engine_settings.Antiplay;
	
    global_axis.command_zero();

    log("> Cheking for antiplay. Target Position: " + DstPos 
    + " Antiplay: " + Antiplay + " AntiplaySpeed: " 
    + move_settings.AntiplaySpeed, 3);

	

// Testing
    var Direction = sgn(DstPos - global_axis.get_status().CurPosition);
    global_axis.command_move(DstPos);
    msleep(100);
    var status;

    while( ((status = global_axis.get_status()).MoveSts & MOVE_STATE_MOVING) != 0 ) 
    { 
//        log("Antiplay: " + Antiplay +" status.CurSpeed: " + status.CurSpeed + " status.CurPosition: " + status.CurPosition);

        if (Antiplay > 0)
        {
            if( (status.CurSpeed > 0) && ( status.CurPosition > DstPos ))
            {
                log(">>> Error ! Crossing target position while moving right.", 1);
                return 1;
            }

           if ( (status.MoveSts & MOVE_STATE_ANTIPLAY) && (status.CurSpeed > 0) && (status.CurPosition > DstPos) )
          {
              log (">>> Error ! Target position has not been crossed but MOVE_STATE_ANTIPLAY flag has set.", 1);
              return 1; 
          }

           if ( (Direction < 0) && (status.CurSpeed > 0) && (status.CurPosition < DstPos) && !(status.MoveSts & MOVE_STATE_ANTIPLAY) )
          {
              log (">>> Error ! Target position has been crossed but MOVE_STATE_ANTIPLAY flag has not set. ", 1);
              return 1; 
          }
        }

        if ( Antiplay < 0 )
        {
            if( (status.CurSpeed < 0) && ( status.CurPosition < DstPos ) )
            {
                log(">>> Error ! Crossing target position while moving left.", 1);
                return 1;
            }

            if ( (status.MoveSts & MOVE_STATE_ANTIPLAY) && (status.CurSpeed < 0) && (status.CurPosition < DstPos) )
            {
                 log(">>> Error ! Target position has not been crossed but MOVE_STATE_ANTIPLAY flag has set.", 1);
                 return 1;
             }
         }

           if ( (Direction > 0 ) && (status.CurSpeed < 0) && (status.CurPosition > DstPos) && !(status.MoveSts & MOVE_STATE_ANTIPLAY) )
          {
              log (">>> Error ! Target position has been crossed but MOVE_STATE_ANTIPLAY flag has not set. ", 1);
              return 1; 
          }

          if ( (Direction > 0) && (status.CurPosition > DstPos ) && (status.MoveSts & MOVE_STATE_ANTIPLAY) )
          {
              AntiplayRight = 1;
          }

          if ( (Direction < 0) && (status.CurPosition < DstPos ) && (status.MoveSts & MOVE_STATE_ANTIPLAY) )
          {
              AntiplayLeft = 1;
          }
        msleep(100);
    }

    if  ( (Direction > 0) && !AntiplayRight && (Antiplay < 0) )
    {
        log ("Error ! Antiplay not working: target position has not been crossed.",1);
        return 0;
    }

    if ( (Direction < 0) && !AntiplayLeft && (Antiplay > 0) )
    {
        log("Error ! Antiplay not working: target position has not been crossed.", 1);
        return 0;
    }


    msleep(100);
    log(">>> OK", 3);
    return 0;
}

function test_MoveTo_ChangeStepMode()
{
    var DstPos = 4001;
    var uDstPos = 159;

    log("> Checking move to with change step mode", 3);

    engine_settings = global_axis.get_engine_settings();
    engine_settings.MicrostepMode = MICROSTEP_MODE_FRAC_256;
    msleep(200);
    global_axis.command_move(DstPos, uDstPos);
    msleep(3000);
    engine_settings = global_axis.get_engine_settings();
    engine_settings.MicrostepMode = MICROSTEP_MODE_FULL;
    global_axis.set_engine_settings(engine_settings);
    msleep(100);
    while((global_axis.get_status().MoveSts & MOVE_STATE_MOVING) != 0) { msleep(100); }
    msleep(3000);
    engine_settings = global_axis.get_engine_settings();
    engine_settings.MicrostepMode = MICROSTEP_MODE_FRAC_256;
    global_axis.set_engine_settings(engine_settings);
    msleep(100);
    if (global_axis.get_status().uCurPosition != 0)
    {
         log(">>> Error ! Incorrect position recalculation while changing step mode.", 1);
         return 1;
    }
    log(">>> OK", 3);
}

function test_ShiftTo_ChangeStepMode()
{
    var StartPos = 0;
    var uStartPos = 139
    log("> Checking shift to with change step mode", 3);
    global_axis.command_move(StartPos, uStartPos);
    msleep(200);
    while( (global_axis.get_status().MoveSts & MOVE_STATE_MOVING) !=  0 ) { msleep(100) ; }
    engine_settings = global_axis.get_engine_settings();
    engine_settings.MicrostepMode = MICROSTEP_MODE_FULL;
    global_axis.set_engine_settings(engine_settings);
    msleep(200);
    global_axis.command_movr(100,0);
    msleep(200);
    while( (global_axis.get_status().MoveSts & MOVE_STATE_MOVING) !=  0 ) { msleep(100) ; }
    if (global_axis.get_status().uCurPosition != 0)
    {
        log(">>> Error ! Incorrect position recalculation while changing step mode.", 1);
        return 1;
    }
    log(">>> OK", 3);
}

function test_mean_speed(direction)
{
    /*
     *    This function have to called from section without acceleration.
     */

    var accuracy  = 0.05;
    var Delay        = 10000;

    log("> Checking mean speed", 3);
 
    if (global_axis.get_engine_settings().EngineFlags & ENGINE_ACCEL_ON)
    {
        log(">> Usage error. This function must be called without acceleration",2);
        return 3;
    }

    var StepDivisionFactor = global_axis.get_engine_settings().MicrostepMode;
    var StartPos  = global_axis.get_status().CurPosition  + global_axis.get_status().uCurPosition / Math.pow(2, StepDivisionFactor - 1);
    var Speed = global_axis.get_move_settings().Speed + global_axis.get_move_settings().uSpeed / Math.pow(2, StepDivisionFactor - 1);
    var PredictedDistant = 0;

    switch (direction)
    {
        case RIGHT:
            global_axis.command_right();
            break;
        case LEFT:
            global_axis.command_left();
            break;
    }    
    msleep(10000);
    global_axis.command_stop();  

    var Distant = abs(global_axis.get_status().CurPosition + global_axis.get_status().uCurPosition / Math.pow(2, StepDivisionFactor - 1) - StartPos);
    PredictedDistant = Speed * Delay/1000;
    if (abs(Distant - PredictedDistant) > PredictedDistant * accuracy ) 
    {
        log(">>> Error while checking mean speed", 1);
        global_axis.command_stop();
        return 1;
    }
    
  
    log(">>> OK", 3);
}

function test_mean_acceleration(direction)
{
    /*
     *    This function have to called from section with acceleration/deceleration.
     */

    var accuracy  = 0.05;
    var Delay = 10000;

    log("> Checking mean acceleration", 3);
 
    if (!(global_axis.get_engine_settings().EngineFlags & ENGINE_ACCEL_ON))
    {
        log(">> Usage error. This function must be called without acceleration flag", 2);
        return 3;
    }
	
    if (global_axis.get_move_settings().Accel*Delay/1000 > global_axis.get_move_settings().Speed)
    {
        log (">> Usage error. Speed must be above then Acceleration multiplyed with time_of_test.", 2);
        return 3;
    }

    var StepDivisionFactor = global_axis.get_engine_settings().MicrostepMode;
    var StartSpeed  = global_axis.get_status().CurSpeed  + global_axis.get_status().uCurSpeed / Math.pow(2, StepDivisionFactor - 1);
    var Acceleration = global_axis.get_move_settings().Accel;
    var PredictedDistant = 0;

    switch (direction)
    {
        case RIGHT:
            global_axis.command_right();
            break;
        case LEFT:
            global_axis.command_left();
            break;
     }    
    msleep(Delay);  

    var DeltaSpeed = abs(global_axis.get_status().CurSpeed + global_axis.get_status().uCurSpeed / Math.pow(2, StepDivisionFactor - 1));

    PredictedSpeed = Acceleration * Delay/1000;
    if (abs(DeltaSpeed - PredictedSpeed) > PredictedSpeed * accuracy ) 
    {
        log(">>> Error while checking mean acceleration" + " : " + PredictedSpeed + " : " + DeltaSpeed, 1);
        global_axis.command_stop();
        return 1;
    }

    global_axis.command_stop();  
    log(">>> OK", 3);
}

function test_mean_deceleration(direction)
{
    /*
     *    This function have to called from section with acceleration/deceleration.
     */

    var accuracy  = 0.05;
    var Delay = 10000;

    log("> Checking mean deceleration", 3);
 
    if (!(global_axis.get_engine_settings().EngineFlags & ENGINE_ACCEL_ON))
    {
        log(">> Usage error. This function must be called without acceleration flag", 2);
        return 3;
    }
	
    if (global_axis.get_move_settings().Decel*Delay/1000 > global_axis.get_move_settings().Speed)
    {
        log (">> Usage error. Speed must be above then deceleration multiplyed with time of test.", 2);
        return 3;
    }

    switch (direction)
    {
        case RIGHT:
            global_axis.command_right();
            break;
        case LEFT:
            global_axis.command_left();
            break;
     }    
    msleep(global_axis.get_move_settings().Speed / global_axis.get_move_settings().Accel * 1000 + 1000);

    var StepDivisionFactor = global_axis.get_engine_settings().MicrostepMode;
    var StartSpeed  = abs(global_axis.get_status().CurSpeed  + global_axis.get_status().uCurSpeed / Math.pow(2, StepDivisionFactor - 1));
    var Deceleration = global_axis.get_move_settings().Decel;
    var PredictedDistant = 0;

    global_axis.command_sstp();
    msleep(Delay);

    var DeltaSpeed = abs(global_axis.get_status().CurSpeed + global_axis.get_status().uCurSpeed / Math.pow(2, StepDivisionFactor - 1));

    PredictedSpeed = StartSpeed - Deceleration * Delay/1000;
    if (abs(DeltaSpeed - PredictedSpeed) > PredictedSpeed * accuracy ) 
    {
        log(">>> Error while checking mean acceleration", 1);
        global_axis.command_stop();
        return 1;
    }
    
    global_axis.command_stop();   
    log(">>> OK", 3);
}

function test_incorrect_position_with_different_microstep_modes(microstep_mode)
{
        log("> Checing incorrect position with microstep division: 1/" + (1<<(microstep_mode-1)),3);

        // Set microstep division < 256 for test 
        engine_settings = global_axis.get_engine_settings();
        engine_settings.MicrostepMode = microstep_mode;
        global_axis.set_engine_settings(engine_settings);

        var test_pos = -(1 << (microstep_mode-1))/2; // Go to half of full step (to the negative side). 
        global_axis.command_move(0, test_pos);

        // Waiting for finish movement.
        while ((status = global_axis.get_status()).MoveSts & MOVE_STATE_MOVING)        {msleep(100);}
        
        msleep(500)
        // Get the actually achived position.
        status = global_axis.get_status();
        actual_pos = (status.CurPosition << (microstep_mode-1)) + status.uCurPosition;
        if (test_pos != actual_pos)
        {
            log(">>> Error. Incorrect microstep position while going to the negative side",1);
            log("microstep division: 1/" + (1<<(microstep_mode-1)) + ", test_pos (uSteps): " + test_pos + ", actual_pos (uSteps): " + actual_pos, 3);
            return 1;
        } else
        {
            log(">>> OK", 3);   
            return 0;
        }
}

function test_poweroff_while_movement()
{
    log("> Test poweroff while controller is moving",3);

    // Start movement
    global_axis.command_right();
    msleep(200);

    // Turn power off
    global_axis.command_power_off();
    msleep(200);

    // Check MVCMD_RUNNING flag
    if (global_axis.get_status().MvCmdSts & MVCMD_RUNNING)
    {
        log(">>> Error ! MVCMD_RUNNING is set but it shouldn't", 1);
        global_axis.command_stop();
        return 1;
    }

    // Check movement
    var status = global_axis.get_status();
    var Pos = status.CurPosition*256 + status.uCurPosition;
    msleep(2000);
    var status = global_axis.get_status();
    var newPos = status.CurPosition*256 + status.uCurPosition;
    if (Pos != newPos)
    {
        log(">>> Error ! Controller is moving but it shouldn't",1);
        global_axis.command_stop();
        return 1;
    }

    log(">>> OK", 3);
    return 0;
}

function test_borders()
{
    log("> Test shift on borders and home position. Bug #4488", 3);

    command_move(0);
    command_wait_for_stop(delay);

    var EDGE = get_edges_settings();  // Programm boards
    var EDGE_temp = get_edges_settings();

    EDGE.BorderFlags  = BORDER_IS_ENCODER | BORDER_STOP_LEFT | BORDER_STOP_RIGHT;
    EDGE.LeftBorder   = -1000;
    EDGE.RightBorder  = 1000;
    set_edges_settings(EDGE);

    command_zero();

    log("> Left board...", 3);
    command_left();
    command_wait_for_stop(delay);
    GETS = get_status();
    end_p = GETS.CurPosition;   // Left board position, must be -1000

    command_movr(-100, 0);  // Try move left outsize the boards
    msleep(1000);

    command_movr(200, 0);
    command_wait_for_stop(delay);
    GETS = get_status();

    if (GETS.CurPosition != end_p + 200)
    {
      log(">>> Shifting error. GETS.CurPosition = " + GETS.CurPosition + ", must be -800", 1);
    }
    else
    {
      log(">>> OK", 3);
    }

    log("> Right board...", 3);
    command_right();
    command_wait_for_stop(delay);
    GETS = get_status();
    end_p = GETS.CurPosition;   // Right board position, must be -1000

    command_movr(100,0);  // Try move right outsize the boards
    msleep(1000);

    command_movr(-200, 0);
    command_wait_for_stop(delay);
    GETS = get_status();

    if (GETS.CurPosition != end_p - 200)
    {
      log(">>> Shifting error. GETS.CurPosition = " + GETS.CurPosition + ", must be 800", 1);
    }
    else
    {
      log(">>> OK", 3);
    }

    set_edges_settings(EDGE_temp)
}

function test_shift_change_motor()
{
  var inf = global_axis.get_device_information()
  if (inf.ProductDescription != "XISM-USB")
  {
    log("> Checking shift on after change motor", 3);

    var chmt = new Object();
    chmt.Motor = 0;
    global_axis.command_change_motor(chmt);

    var move_settings = global_axis.get_move_settings();

    var first_pos = global_axis.get_status().CurPosition;

    global_axis.command_movr(1000);
    msleep(1000 * 1000 / move_settings.Speed);
    msleep(500);

    var state = global_axis.get_status();
    if (state.CurPosition != 1000 + first_pos)
    {
      log(">>> Error: wrong position after shift on ", 1);
      return 1;
    }

    chmt.Motor = 1;
    global_axis.command_change_motor(chmt);

    first_pos = global_axis.get_status().CurPosition;
    global_axis.command_movr(1000);
    msleep(1000 * 1000 / move_settings.Speed);
    msleep(500);

    state = global_axis.get_status();
    if (state.CurPosition != 1000 + first_pos)
    {
      log(">>> Error: wrong position after shift on", 1);
      return 1;
    }

    log(">>> OK",3);
  }
  else
  {
     return 0;
  }
}

function DoCommonTests()
{
// This test must be started first.
    test_GoToMaxIntPos2();
    msleep(1000);

// Testing movement
    test_move_left();
    msleep(1000);

    test_move_right();
    msleep(1000);

    test_check_sstp();
    msleep(1000);

    test_check_stop();
    msleep(1000);

    test_check_sstp_when_stopped();
    msleep(1000);

// Testing zero
    test_zero(1000); // TargetPosition = 1000
    msleep(1000);

    test_change_direction();
    global_axis.command_zero();
    global_axis.command_stop();
    msleep(1000);

    test_moveto(1000);
    global_axis.command_zero();
    global_axis.command_stop();
    msleep(1000);

    test_moveto(-1000);
    global_axis.command_zero();
    global_axis.command_left();
    msleep(1000);

    test_moveto(1000);
    global_axis.command_zero(); 
    global_axis.command_right();
    msleep(1000);

    test_moveto(1000);
    global_axis.command_zero();
    global_axis.command_left();
    msleep(1000);

    test_moveto(-1000);
    global_axis.command_zero();
    global_axis.command_right();
    msleep(1000);

    test_moveto(-1000);
    msleep(1000);

    test_borders();
    msleep(1000);

// Testing go to position 2^31
    var move_settings = global_axis.get_move_settings();
    var old_move_settings = move_settings;
    move_settings.Speed  = 100000;
    move_settings.Accel   = 65535;
    move_settings.Decel   = 65535;
    global_axis.set_move_settings(move_settings);
    test_GoToMaxIntPos();
    global_axis.command_zero();
    global_axis.set_move_settings(old_move_settings);
    msleep(1000);

// Testing step mode changing
   test_MoveTo_ChangeStepMode();
   msleep(1000);

   test_ShiftTo_ChangeStepMode();
   msleep(1000);

// Testing errors in actual position while going to the negative side with different microstep division parameters.
    // Get engine_settings
    engine_settings = global_axis.get_engine_settings();
    // Save original engine settings
    var old_engine_settings = engine_settings;
    for (var microstep_mode = 2; microstep_mode <= 9; microstep_mode++)
    {
         test_incorrect_position_with_different_microstep_modes(microstep_mode);
    }
    // Return original engine settings
    global_axis.set_engine_settings(old_engine_settings);

// Testing antiplay 
    var engine_settings = global_axis.get_engine_settings();
    engine_settings.EngineFlags |= ENGINE_ANTIPLAY;
    global_axis.set_engine_settings(engine_settings);
     
    engine_settings = global_axis.get_engine_settings();
    engine_settings.Antiplay = 200;
    global_axis.set_engine_settings(engine_settings); 
    test_antiplay(1000);
    msleep(1000);

    engine_settings = global_axis.get_engine_settings();
    engine_settings.Antiplay = 200;
    global_axis.set_engine_settings(engine_settings);
    test_antiplay(-1000);
    msleep(1000);
    
    engine_settings = global_axis.get_engine_settings();
    engine_settings.Antiplay = -200;
    global_axis.set_engine_settings(engine_settings);
    test_antiplay(1000);
    msleep(1000);

    engine_settings = global_axis.get_engine_settings();
    engine_settings.Antiplay = -200;
    global_axis.set_engine_settings(engine_settings);
    test_antiplay(-1000);
    msleep(1000);

    var engine_settings = global_axis.get_engine_settings();
    engine_settings.EngineFlags &= ~ENGINE_ANTIPLAY;
    global_axis.set_engine_settings(engine_settings);
    

// Testing motion with big and small speed, acceleration, decleration.
//  Updating move settings
    var    move_settings = global_axis.get_move_settings();
    move_settings.Accel = 65535;
    move_settings.Decel = 65535;
    move_settings.Speed = 100000;
    global_axis.set_move_settings(move_settings);
    test_big_speed_and_acceleration(500000); // Speed = 100000, Accel = 65535, Decel = 65535, TargetPosition = 500000
    msleep(1000);
//  Updating move settings
    var    move_settings = global_axis.get_move_settings();
    move_settings.Accel = 65535;
    move_settings.Decel = 350;
    move_settings.Speed = 100000;
    global_axis.set_move_settings(move_settings);
    test_big_speed_and_acceleration(5000); // Speed = 100000, Accel = 65535, Decel = 1, TargetPosition = 5000
    msleep(1000);
//  Updating move settings
    var    move_settings = global_axis.get_move_settings();
    move_settings.Accel = 350;
    move_settings.Decel = 65535;
    move_settings.Speed = 100000;
    global_axis.set_move_settings(move_settings);
    test_big_speed_and_acceleration(50); // Speed = 100000, Accel = 1, Decel = 65535, TargetPosition = 50
    msleep(1000);
//  Updating move settings
    var    move_settings = global_axis.get_move_settings();
    move_settings.Accel = 65535;
    move_settings.Decel = 65535;
    move_settings.Speed = 1;
    global_axis.set_move_settings(move_settings);
    test_big_speed_and_acceleration(5); // Speed = 1, Accel = 65535, Decel = 65535, TargetPosition = 5
    msleep(1000);
	
//  Updating move settings
    var    move_settings = global_axis.get_move_settings();
    move_settings.Accel = 65535;
    move_settings.Decel = 65535;
    move_settings.Speed = 1000;
    global_axis.set_move_settings(move_settings);
    test_shift_change_motor()
}


log("######### Automatic controller tests ##########",3);
axis_serial = get_next_serial(0);
global_axis = new_axis(axis_serial);

global_axis.command_stop();
global_axis.command_zero();
// global_axis.command_clear_fram(); //uncomment when it will be realized in xilab
SetDefaultSettings();
DoCommonTests();

move_settings = global_axis.get_move_settings();
move_settings.Speed = 20000;
move_settings.Accel = 1000;
move_settings.Decel = 1000;
global_axis.set_move_settings(move_settings);
test_mean_acceleration(LEFT);
test_mean_acceleration(RIGHT);
test_mean_deceleration(LEFT);
test_mean_deceleration(RIGHT);

global_axis.command_stop();
global_axis.command_zero();
// global_axis.command_clear_fram(); //uncomment when it will be realized in xilab
SetDefaultSettings();
engine_settings = global_axis.get_engine_settings();
engine_settings.EngineFlags &= ~ENGINE_ACCEL_ON;
global_axis.set_engine_settings(engine_settings);
DoCommonTests();

test_mean_speed(LEFT);
test_mean_speed(RIGHT);
test_poweroff_while_movement();