#include "sim_native_homing.h"
#include <assert.h>
#include <math.h>
#include <stdlib.h>

int main(void) {
    native_home *h = malloc(sim_native_home_size());
    native_home *other = malloc(sim_native_home_size());
    double p[12] = {0,0,-1,0,0,0,0,0,-200,200,20,300};
    double position[5] = {10,20,30,15,45};
    assert(sim_native_home_init(h,5,.001) == 0);
    assert(sim_native_home_init(other,5,.001) == 0);
    for (int j=0;j<5;j++) assert(sim_native_home_config(h,j,p) == 0);
    assert(sim_native_home_sync(h,position) == 0);
    assert(sim_native_home_start(h,0,70000) == 0);
    assert(sim_native_home_get(h,0,7) == 10);
    assert(sim_native_home_step(h) == 0);
    assert(sim_native_home_get(h,0,6) == 21);
    assert(sim_native_home_get(h,0,4) == 1);
    assert(sim_native_home_get(h,0,5) == 0);
    assert(sim_native_home_get(h,0,7) == 0);
    assert(sim_native_home_get(h,0,8) == 10);
    assert(sim_native_home_get(h,1,7) == 20);
    for(int i=0;i<10;i++) assert(sim_native_home_step(h) == 0);
    assert(sim_native_home_get(h,0,5) == 1);
    assert(sim_native_home_get(other,0,5) == 0);
    p[1]=5;
    assert(sim_native_home_config(h,1,p) == 0);
    assert(sim_native_home_start(h,1,70001) == 0);
    assert(sim_native_home_step(h) == 0);
    assert(sim_native_home_get(h,1,7)>0 && sim_native_home_get(h,1,7)<5);
    for(int i=0;i<2000 && sim_native_home_get(h,0,0);i++) sim_native_home_step(h);
    assert(fabs(sim_native_home_get(h,1,7)-5)<1e-8);
    assert(fabs(sim_native_home_get(h,1,8)-25)<1e-8);
    assert(sim_native_home_start(h,1,70002) == 0);
    sim_native_home_step(h);
    assert(sim_native_home_cancel(h,0) == 0);
    assert(sim_native_home_get(h,1,5) == 0);
    for (int i=0;i<1000 && sim_native_home_get(h,0,0);i++) sim_native_home_step(h);
    assert(sim_native_home_get(h,0,0) == 0);
    free(h); free(other);
    return 0;
}