// Global simulation time
clock t_sim = 0;

const int N_CARS = 4;
const double MY_INF = 1e800; // Should overflow to INF. Poormen's float infinity

double a_ref[N_CARS] = {0, 0, 0, 0};
double a_ref_n[N_CARS] = {0, 0, 0, 0};

clock a[N_CARS] = {0, 0, 0, 0};
clock v[N_CARS] = {0, 0, 0, 0};
clock x[N_CARS] = {0, -5, -10, -15};

double a_n[N_CARS] = {0, 0, 0, 0};
double v_n[N_CARS] = {0, 0, 0, 0};
double d_n[N_CARS] = {0, -5, -10, -15};

clock a_p[N_CARS] = {0, 0, 0, 0};
clock v_p[N_CARS] = {0, 0, 0, 0};
clock x_p[N_CARS] = {0, -5, -10, -15};

// Step used to update the CACC and driver's outputs
const double T = 0.01;

// CACC's parameters
const double alfa1 = 0.5;
const double alfa2 = 0.5;
const double alfa3 = -0.3;
const double alfa4 = -0.1;
const double alfa5 = -0.04;

// Desired distance
const double d = 11;

// Used to sync all the CACC modules, so that they act as a single
// CACC system located in the network edge, like in the original
// INTO- CPS model
broadcast chan cacc;

// Attack paramaters
const double t_attack = 30;
double A_lf = 0.08;


// Check for collisions
bool rear_ended[N_CARS] = {0, 0, 0, 0};
bool crashed[N_CARS] = {0, 0, 0, 0};

process Simulation() {
clock t = 0;

//int argA_lf = 0;

void test_collision()
{
    for (i : int[1,N_CARS-1])
    {
        if (x_p[i-1] - x_p[i] < 4.0)
        {
            rear_ended[i] = true;
            crashed[i] = true;
            crashed[i - 1] = true;
        }
    }
}
state
    STEP { t <= T },
    START;
commit
    START;
init
    START;
trans
    STEP -> STEP { guard t < T; },
    STEP -> STEP { guard t >= T; sync cacc!; assign t := 0,
test_collision(); },
    START -> STEP { sync cacc!; assign A_lf := -0.084, t := 0; };
}

process CarDynamic(double start_time, double& a_ref, clock& a, clock& v, clock& x, double attack_time, clock& a_t, clock& v_t, clock& x_t, bool& crashed) {
// The filter's output acceleration
clock o = 0;

// Time since the car has started moving
clock t_mov = 0;

//const double m = 2000.0
const double k = 25.0;

state
    WAIT { t_sim <= start_time	 &&
o' == 0			 &&
a' == 0			 &&
v' == 0			 &&
x' == 0			 &&
a_t' == 0		 &&
v_t' == 0		 &&
x_t' == 0		 &&
t_mov' == 0 },
    DYNAMICS { t_sim <= attack_time &&
o'   == (a_ref - o)/0.2 &&
a'   == k*o + (a_ref - o)/0.2 - k*a &&
v'   == a &&
x'   == v &&
a_t' == k*o + (a_ref - o)/0.2 - k*a_t &&
v_t' == a_t &&
x_t' == v_t ; 10 },
    DYNAMICS_ATTACK { o'   == (a_ref - o)/0.2 &&
a'   == k*o + (a_ref - o)/0.2 - k*a + 0.2*M_PI*A_lf*cos(0.2*M_PI*t_sim)&&
v'   == a &&
x'   == v &&
a_t' == k*o + (a_ref - o)/0.2 - k*a_t &&
v_t' == a_t &&
x_t' == v_t ; 10 },
    CRASH { o' == 0			 &&
a' == 0			 &&
v' == 0			 &&
x' == 0			 &&
a_t' == 0		 &&
v_t' == 0		 &&
x_t' == 0		 &&
t_mov' == 0 };
init
    WAIT;
trans
    DYNAMICS_ATTACK -> CRASH { guard crashed; },
    DYNAMICS -> CRASH { guard crashed; },
    DYNAMICS -> DYNAMICS_ATTACK { guard t_sim >= attack_time; assign v := v + 5*M_1_PI*A_lf,
x := x + 5*M_1_PI*A_lf*t_sim; },
    WAIT -> DYNAMICS { guard t_sim >= start_time; };
}

process LeaderDriver(double &a_ref) {
// Used to time the driver's sin accel
clock t;
// Used to time the driver's step accel
clock t1;

// Parameters
const double A = 0.8;
double f = 0.5;//*1e-3;
const double phase = 0;
const double offset = -0.01;
double acceleration_value = 2.0;//*1e-6;
double deceleration_value = -1.5;
const double slope = 0.5;

// Which mode has been selected by the driver
int mode = -1;

// Step function's parameters
const double sprint_p = 5;
const double T_sprint = sprint_p + 30;
state
    CONST_A { t_sim <= 10 },
    REGIME { t <= T },
    INIT,
    SLOPE { t_sim <= 8 && t <= T },
    ACCEL { t1 <= 5 },
    FINAL_STEPS { t1 <= 25 &&
t <= T },
    DECEL { t1 <= 5 },
    FINAL_STEPS2 { t1 <= 25 &&
t <= T };
branchpoint
    B0;
urgent
    INIT;
init
    INIT;
trans
    FINAL_STEPS2 -> ACCEL { guard t1 >= 25; assign t1 := 0,
a_ref := acceleration_value; },
    DECEL -> FINAL_STEPS2 { guard t1 >= 5; assign t1 := 0, t := 0; },
    FINAL_STEPS -> FINAL_STEPS { guard t1 <= 25; assign a_ref := 0.03*sin(1.5 * t_sim) + 0.005,
t := 0; },
    FINAL_STEPS -> DECEL { guard t1 >= 25; assign a_ref := deceleration_value,
t1 := 0; },
    ACCEL -> FINAL_STEPS { guard t1 >= 5; assign t := 0, t1 := 0; },
    B0 -> ACCEL { assign a_ref := acceleration_value,
mode := 2,
t1 := 0; probability 1; },
    SLOPE -> REGIME { guard t_sim >= 8; assign a_ref := A*sin(f*t_sim + phase) + offset,
t := 0; },
    SLOPE -> SLOPE { guard t_sim <= 8; assign a_ref := slope*t_sim,
t := 0; },
    B0 -> SLOPE { assign a_ref := 0,
mode := 1; probability 1; },
    B0 -> CONST_A { assign a_ref := acceleration_value,
mode := 0; probability 1; },
    INIT -u-> B0 { select avi: int[17, 20],
dvi: int[-17, -15],
fi: int[50, 63]; assign f := fi / 100.0,
acceleration_value = avi / 10.0,
deceleration_value = dvi / 10.0; },
    REGIME -> REGIME { assign a_ref := A*sin(f*t_sim + phase) + offset,
t := 0; },
    CONST_A -> REGIME { guard t_sim >= 10; assign a_ref := A*sin(f*t_sim + phase) + offset,
t := 0; },
    FINAL_STEPS2 -> FINAL_STEPS2 { guard t1 <= 25; assign a_ref := 0.03*sin(1.5 * t_sim) + 0.005,
t := 0; };
}

process CACC(double& a_ref, double& a_leader, double& v_leader,double& a_front, double& v_front, double& a_ego, double& v_ego, double& d_to_front) {
clock t = 0;
state
    STEP;
init
    STEP;
trans
    STEP -> STEP { sync cacc?; assign t := 0,
a_ref := 	
	 alfa1*a_front + alfa2*a_leader +
	 alfa3*(v_ego - v_front) + alfa4*(v_ego-v_leader)
	 + alfa5*(d - d_to_front); };
}

process DownLinkMedium(const double lambda, clock& a_in, clock& v_in, clock& x_in, clock& x_front_in, double& a_out, double& v_out, double& d_out) {
clock t;

int arg_P = 1;

double memory_a;
double memory_v;
double memory_x;
double memory_d;

int n = 0;
double avg_delay = 0;
state
    SAMPLE,
    HOLD { ; lambda };
commit
    SAMPLE;
init
    SAMPLE;
trans
    HOLD -> SAMPLE { assign a_out := memory_a,
v_out := memory_v,
d_out := memory_d,
n := n + 1,
avg_delay := avg_delay + (t - avg_delay)/n; },
    SAMPLE -> HOLD { select argP : int[1, 255]; assign memory_a := a_in,
memory_v := v_in,
memory_x := x_in,
memory_d := x_front_in - memory_x - 4,
t := 0; };
}

process UpLinkMedium(const double lambda, double& in, double& out) {
double memory;

clock t;
int argP = 1;

state
    HOLD { ; lambda },
    SAMPLE;
commit
    SAMPLE;
init
    SAMPLE;
trans
    HOLD -> SAMPLE { assign out := memory; },
    SAMPLE -> HOLD { select argp : int [1, 255]; assign memory := in,
t := 0; };
}
// Place template instantiations here.

net0_d = DownLinkMedium(1/0.03, a[0], v[0], x[0], x[0], a_n[0], v_n[0], d_n[0]);
net1_d = DownLinkMedium(1/0.03, a[1], v[1], x[1], x[0], a_n[1], v_n[1], d_n[1]);
net2_d = DownLinkMedium(1/0.03, a[2], v[2], x[2], x[1], a_n[2], v_n[2], d_n[2]);
net3_d = DownLinkMedium(1/0.03, a[3], v[3], x[3], x[2], a_n[3], v_n[3], d_n[3]);

net1_u = UpLinkMedium(1/0.03, a_ref[1], a_ref_n[1]);
net2_u = UpLinkMedium(1/0.03, a_ref[2], a_ref_n[2]);
net3_u = UpLinkMedium(1/0.03, a_ref[3], a_ref_n[3]);


driver = LeaderDriver(a_ref[0]);
leader = CarDynamic(0, a_ref[0], a[0], v[0], x[0], MY_INF,  a_p[0], v_p[0], x_p[0], crashed[0]);

car1 = CarDynamic(0, a_ref_n[1], a[1], v[1], x[1], 30, a_p[1], v_p[1], x_p[1], crashed[1]);
car2 = CarDynamic(0, a_ref_n[2], a[2], v[2], x[2], MY_INF, a_p[2], v_p[2], x_p[2], crashed[2]);
car3 = CarDynamic(0, a_ref_n[3], a[3], v[3], x[3], MY_INF, a_p[3], v_p[3], x_p[3], crashed[3]);

cacc1 = CACC(a_ref[1], a_n[0], v_n[0], a_n[0], v_n[0], a_n[1], v_n[1], d_n[1]);
cacc2 = CACC(a_ref[2], a_n[0], v_n[0], a_n[1], v_n[1], a_n[2], v_n[2], d_n[2]);
cacc3 = CACC(a_ref[3], a_n[0], v_n[0], a_n[2], v_n[2], a_n[3], v_n[3], d_n[3]);

sim = Simulation();


// List one or more processes to be composed into a system.
//system sim, driver, leader, cacc1, car1, cacc2, car2, cacc3, car3;
system sim, driver, leader, cacc1, car1, cacc2, car2, cacc3, car3, net0_d, net1_d, net2_d, net3_d, net1_u, net2_u, net3_u;
