#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_dbg,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_cnt=0;
    Sprite3ZB(dc,cx+(m1.x+m2.x)/2,cy-(m1.y+m2.y)/2,0,<3>,-theta);
    if (dc->collision_cnt>100) {
      Noise(1000,62,81);
      plane_hit=TRUE;
    } else
      Snd(22);
  } else if (!plane_hit)
    Snd;

  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_dbg);
}

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_dbg=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 vars 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;
  SndTaskEndCB;
}

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;
  QueIns(&m1,ode->last_mass);
  QueIns(&m2,ode->last_mass);
  QueIns(&m3,ode->last_mass);
  QueIns(&s,ode->last_spring);

  QueIns(ode,Fs->last_ode);

  Fs->draw_it=&DrawIt;

  try {
    GetKey;
    blast_off=TRUE;
    while (TRUE) {
      switch (GetChar(,FALSE)) {
        case '\n':
          Init;
          GetKey;
          blast_off=TRUE;
          break;
        case CH_SHIFT_ESC:
        case CH_ESC:
          goto rs_done;
      }
    }
rs_done:
  } catch
    PutExcept;
  QueRem(ode);
  ODEDel(ode);
  DocClear;
  SettingsPop;
  DCFill;
  DCDel(dc2);
  MenuPop;
}

RocketScience;