#define THRUST  1000

Bool blast_off, plane_hit;

CMass   m1, //Bottom of rocket
        m2, //Top of rocket
        m3; //Plane
CSpring s;

#define ROCKET_HEIGHT   40
#define GROUND_Y        (GR_HEIGHT - 3 * FONT_HEIGHT)


     <1>/* Graphics Not Rendered in HTML */



     <2>/* Graphics Not Rendered in HTML */



    <3>/* Graphics Not Rendered in HTML */
 















/* Graphics Not Rendered in HTML */

CDC      *dc2;
CMathODE *ode;

#define STATE_NOZZLE_ANGLE              0
#define STATE_NOZZLE_ANGLE_VELOCITY     1
#define STATE_NUM                       2

CD3 target;
F64 my_debug, antispin_coefficient;

U0 DrawIt(CTask *task, CDC *dc)
{
    I64 i, x, y, cx = GR_WIDTH / 2, cy = GROUND_Y;
    F64 theta = Arg(m2.x - m1.x, m2.y - m1.y), nozzle_angle = ode->state[STATE_NOZZLE_ANGLE];

    if (blast_off)
    {
        x = m1.x - 10 * Cos(theta + nozzle_angle);
        y = m1.y - 10 * Sin(theta + nozzle_angle);
        for (i = 0; i < 6; i++)
        {
            if ((i ^ winmgr.updates) & 1)
                dc->color = YELLOW;
            else
                dc->color = RED;
            GrLine(dc, cx + (m1.x + i * Cos(theta - pi  /2)), cy - (m1.y + i * Sin(theta - pi / 2)), cx + x, cy - y);
            GrLine(dc, cx + (m1.x + i * Cos(theta + pi / 2)), cy - (m1.y + i * Sin(theta + pi / 2)), cx + x, cy - y);
        }

        for (i = 0; i < 10; i++)
        {
            switch (RandU16 & 3)
            {
                case 0: dc2->color = WHITE;     break;
                case 1: dc2->color = LTGRAY;    break;
                case 2: dc2->color = DKGRAY;    break;
                case 3: dc2->color = BLACK;     break;
            }
            GrPlot(dc2, cx + (x + RandU16 % 12 - 6), cy - (y + RandU16 % 12 - 6));
        }
    }

    if (plane_hit)
        Sprite3(dc, cx + m3.x, cy - m3.y, 0, <2>);
    else
        Sprite3(dc, cx + m3.x, cy - m3.y, 0, <1>);

    if (blast_off && !plane_hit)
    {
        dc->color           = ROP_COLLISION;
        dc->bkcolor         = LTCYAN;
        dc->collision_count = 0;
        Sprite3ZB(dc, cx + (m1.x + m2.x) / 2, cy - (m1.y + m2.y) / 2, 0, <3>, -theta);
        if (dc->collision_count > 100)
        {
            Noise(1000, 62, 81);
            plane_hit = TRUE;
        }
        else
            Sound(22);
    }
    else if (!plane_hit)
        Sound;

    dc->color = ROP_EQU;
    Sprite3(dc, 0, GROUND_Y, 0, <4>);
    Sprite3ZB(dc, cx + (m1.x + m2.x) / 2, cy - (m1.y + m2.y) / 2, 0, <3>, -theta);

    dc->color = RED;
    GrCircle(dc, cx + target.x, cy - target.y, 5);

    dc->color = BLUE;
    GrCircle(dc, cx + m3.x, cy - m3.y, 5);

    dc->color = BLACK;
    GrPrint(dc, 0, FONT_HEIGHT, "%12.6f", my_debug);
}

U0 MyDerivative(CMathODE *, F64, F64 *state, F64 *DstateDt)
{
    F64 d, discriminant, v, a,
        theta = Arg(m2.state->x - m1.state->x, m2.state->y - m1.state->y), 
        DthetaDt, collision_estimate_t, target_heading, target_angle_error, 
        desired_nozzle_angle;
    CD3 p, p_target, p_body;

    //Unit vect pointing to top of rocket from bottom.
    D3Sub(&p_body, &m2.state->x, &m1.state->x);
    D3Unit(&p_body);

    //DthetaDt lets us prevent too much spin.
    DthetaDt = antispin_coefficient * (m2.state->DyDt * p_body.x - m2.state->DxDt * p_body.y -
                                   m1.state->DyDt * p_body.x + m1.state->DxDt * p_body.y) / ROCKET_HEIGHT;

    //p_target is vect from top of rocket to plane.
    D3Sub(&p_target, &m3.state->x, &m2.state->x);

    //d=0.5at^2+vt
    d = D3Norm(&p_target);

    D3Copy(&p, &p_target);
    D3Unit(&p);
    v = (m2.state->DxDt * p.x + m2.state->DyDt * p.y) - (m3.state->DxDt * p.x + m3.state->DyDt * p.y);

    a = THRUST / (m1.mass + m2.mass);

    discriminant = v * v + 4 * 0.5 * a * d;
    if (discriminant > 0)
        collision_estimate_t = (-v + Sqrt(discriminant)) / a;
    else
        collision_estimate_t = 0;
    my_debug = collision_estimate_t;

    //Aim for projected pos of plane at time of impact.
    D3Copy(&p, &m3.state->DxDt);
    D3MulEqu(&p, collision_estimate_t);
    D3AddEqu(&p_target, &p);

    D3Copy(&target, &p_target);
    D3AddEqu(&target, &m2.state->x);

    target_heading = Arg(p_target.x, p_target.y);
    target_angle_error = Wrap(theta - target_heading); //Force to range [-pi, pi)
    desired_nozzle_angle = Clamp(50.0 * DthetaDt + 750 * target_angle_error, -pi / 8, pi / 8);

    //For realism we limit the speed the nozzle angle can change.
    DstateDt[STATE_NOZZLE_ANGLE]          = state[STATE_NOZZLE_ANGLE_VELOCITY];
    DstateDt[STATE_NOZZLE_ANGLE_VELOCITY] = Clamp(10000 * (desired_nozzle_angle - state[STATE_NOZZLE_ANGLE]), -1000, 1000) -
                                            10.0 * state[STATE_NOZZLE_ANGLE_VELOCITY]; //Damping

    if (blast_off)
    {
        m1.DstateDt->DxDt += THRUST * Cos(theta + state[STATE_NOZZLE_ANGLE]);
        m1.DstateDt->DyDt += THRUST * Sin(theta + state[STATE_NOZZLE_ANGLE]);

        m1.DstateDt->DyDt -= 25; //Gravity
        m2.DstateDt->DyDt -= 25;
    }

    //For more realism reduce the mass of the rocket because of fuel.
    //You might also factor-in fuel slosh in the tank.

    //To do this,  you would have to set-up state variables for mass and
    //do A=F/m manually instead of relyin on ODECallDerivative() to divide
    //by mass.
}

U0 Init()
{
    DocClear;
    "$BG,LTCYAN$%h*c", ToI64(GROUND_Y / FONT_HEIGHT), '\n';

    blast_off = FALSE;
    plane_hit = FALSE;

    do antispin_coefficient = PopUpRangeF64Exp(0.1, 10.001, Sqrt(10), "%9.4f", "Anti-spin Coefficient\n\n");
    while (!(0.1 <= antispin_coefficient < 10.001));

    //We don't clear que links.
    MemSet(&m1.start, 0, offset(CMass.end) - offset(CMass.start));
    m1.y    = 0;
    m1.mass = 1.0;

    MemSet(&m2.start, 0, offset(CMass.end) - offset(CMass.start));
    m2.y    = ROCKET_HEIGHT;
    m2.mass = 1.0;

    MemSet(&m3.start, 0, offset(CMass.end) - offset(CMass.start));
    m3.y    = 400;
    m3.x    = -300;
    m3.DxDt = 50;
    m3.mass = 1.0;

    MemSet(&s.start, 0, offset(CSpring.end) - offset(CSpring.start));
    s.end1      = &m1;
    s.end2      = &m2;
    s.rest_len  = ROCKET_HEIGHT;
    s.const     = 10000;

    ode->state[STATE_NOZZLE_ANGLE]          = 0;
    ode->state[STATE_NOZZLE_ANGLE_VELOCITY] = 0;

    DCFill;
}

U0 TaskEndCB()
{
    DCFill;
    SoundTaskEndCB;
}

U0 RocketScience()
{
    SettingsPush; //See SettingsPush
    Fs->text_attr = YELLOW << 4 + BLUE;
    MenuPush(   "File {"
                "  Abort(,CH_SHIFT_ESC);"
                "  Exit(,CH_ESC);"
                "}"
                "Play {"
                "  Restart(,'\n');"
                "  Launch(,CH_SPACE);"
                "}"
                );

    AutoComplete;
    WinBorder;
    WinMax;
    DocCursor;
    DocClear;
    dc2 = DCAlias;
    Fs->task_end_cb = &TaskEndCB;

    ode = ODENew(STATE_NUM, 1e - 6, ODEF_HAS_MASSES);
    ode->derive             = &MyDerivative;
    ode->drag_v2            = 0.002;
    ode->drag_v3            = 0.00001;
    ode->acceleration_limit = 5e3;

    //  ode->t_scale=0.1; //Uncomment this to go in slow motion.

    Init;
    QueueInsert(&m1, ode->last_mass);
    QueueInsert(&m2, ode->last_mass);
    QueueInsert(&m3, ode->last_mass);
    QueueInsert(&s, ode->last_spring);

    QueueInsert(ode, Fs->last_ode);

    Fs->draw_it = &DrawIt;

    try
    {
        KeyGet;
        blast_off = TRUE;
        while (TRUE)
        {
            switch (CharGet(, FALSE))
            {
                case '\n':
                    Init;
                    KeyGet;
                    blast_off = TRUE;
                    break;

                case CH_SHIFT_ESC:
                case CH_ESC:
                    goto rs_done;
            }
        }
rs_done:
    }
    catch
        PutExcept;
    QueueRemove(ode);
    ODEDel(ode);
    DocClear;
    SettingsPop;
    DCFill;
    DCDel(dc2);
    MenuPop;
}

RocketScience;