void FlowRouting::fillinpitsandflats( Topo *q, int i, int j )
{
    double min;

    min = q->topo[i][j];

    if (q->topo[q->iup[i]][j] < min)
        min = q->topo[q->iup[i]][j];
    if (q->topo[q->idown[i]][j] < min)
        min = q->topo[q->idown[i]][j];
    if (q->topo[i][q->jup[j]] < min)
        min = q->topo[i][q->jup[j]];
    if (q->topo[i][q->jdown[j]] < min)
        min = q->topo[i][q->jdown[j]];
    if (q->topo[q->iup[i]][q->jup[j]] < min)
        min = q->topo[q->iup[i]][q->jup[j]];
    if (q->topo[q->idown[i]][q->jup[j]] < min)
        min = q->topo[q->idown[i]][q->jup[j]];
    if (q->topo[q->idown[i]][q->jdown[j]] < min)
        min = q->topo[q->idown[i]][q->jdown[j]];
    if (q->topo[q->iup[i]][q->jdown[j]] < min)
        min = q->topo[q->iup[i]][q->jdown[j]];
    if ((q->topo[i][j] <= min) && (i>1)&&(j>1) &&
            (i < q->nx) && (j < q->ny))
    {
        q->topo[i][j] = min + fillincrement;
        fillinpitsandflats(q, i, j);
        fillinpitsandflats(q, q->iup[i], j);
        fillinpitsandflats(q, q->idown[i], j);
        fillinpitsandflats(q, i, q->jup[j]);
        fillinpitsandflats(q, i, q->jdown[j]);
        fillinpitsandflats(q, q->iup[i], q->jup[j]);
        fillinpitsandflats(q, q->idown[i], q->jup[j]);
        fillinpitsandflats(q, q->idown[i], q->jdown[j]);
        fillinpitsandflats(q, q->iup[i], q->jdown[j]);
    }
}
// Have to rethink this algo, on real data will cause stack overflow.
void Ctest_stream_power_model::fillinpitsandflats(int i, int j)
{    float min;

     min=topo[i][j];
     if (topo[iup[i]][j]<min) min=topo[iup[i]][j];
     if (topo[idown[i]][j]<min) min=topo[idown[i]][j];
     if (topo[i][jup[j]]<min) min=topo[i][jup[j]];
     if (topo[i][jdown[j]]<min) min=topo[i][jdown[j]];
     if (topo[iup[i]][jup[j]]<min) min=topo[iup[i]][jup[j]];
     if (topo[idown[i]][jup[j]]<min) min=topo[idown[i]][jup[j]];
     if (topo[idown[i]][jdown[j]]<min) min=topo[idown[i]][jdown[j]];
     if (topo[iup[i]][jdown[j]]<min) min=topo[iup[i]][jdown[j]];
     if ((topo[i][j]<=min)&&(i>1)&&(j>1)&&(i<lattice_size_x)&&(j<lattice_size_y))
      {topo[i][j]=min+fillincrement;
       fillinpitsandflats(i,j);
       fillinpitsandflats(iup[i],j);
       fillinpitsandflats(idown[i],j);
       fillinpitsandflats(i,jup[j]);
       fillinpitsandflats(i,jdown[j]);
       fillinpitsandflats(iup[i],jup[j]);
       fillinpitsandflats(idown[i],jup[j]);
       fillinpitsandflats(idown[i],jdown[j]);
       fillinpitsandflats(iup[i],jdown[j]);}
}
void Ctest_stream_power_model::streampower()
{   
	 FILE *fp1;
     float deltah,time,max;
     int printinterval,idum,i,j,t,step;

     fp1=fopen(fname,"w");
     idum=-678;
     //U=1;                /* m/kyr */
     //K=0.05;             /* kyr^-1 */
     printinterval=duration;
     deltax=200.0;       /* m */
     thresh=0.58*deltax; /* 30 deg */
     timestep=1;         /* kyr */
     //duration=100;
     setupgridneighbors();
     topo=matrix(1,lattice_size_x,1,lattice_size_y);
     topo2=matrix(1,lattice_size_x,1,lattice_size_y);
     topoold=matrix(1,lattice_size_x,1,lattice_size_y);
     slope=matrix(1,lattice_size_x,1,lattice_size_y);
     flow=matrix(1,lattice_size_x,1,lattice_size_y);
     flow1=matrix(1,lattice_size_x,1,lattice_size_y);
     flow2=matrix(1,lattice_size_x,1,lattice_size_y);
     flow3=matrix(1,lattice_size_x,1,lattice_size_y);
     flow4=matrix(1,lattice_size_x,1,lattice_size_y);
     flow5=matrix(1,lattice_size_x,1,lattice_size_y);
     flow6=matrix(1,lattice_size_x,1,lattice_size_y);
     flow7=matrix(1,lattice_size_x,1,lattice_size_y);
     flow8=matrix(1,lattice_size_x,1,lattice_size_y);
     topovec=vector(1,lattice_size_x*lattice_size_y);
     topovecind=ivector(1,lattice_size_x*lattice_size_y);
	 // was <=
     for (i=1;i<lattice_size_x;i++)
	 {
		// was <=
		for (j=1;j<lattice_size_y;j++)
		{

			topo[i][j] = InputGrid->asFloat(i, j);
			topo[i][j]=0.5*gasdev(&idum);
			topoold[i][j]=topo[i][j];
			flow[i][j]=1;
			
		}
	}
     /*construct diffusional landscape for initial flow routing */

     for (step=1;step<=10;step++)
      {hillslopediffusioninit();
       for (i=2;i<=lattice_size_x-1;i++)
        for (j=2;j<=lattice_size_y-1;j++)
         {topo[i][j]+=0.1;
          topoold[i][j]+=0.1;}}
     time=0;
     while (time<duration && Process_Get_Okay(true))
      {
		/*perform landsliding*/
       for (j=1;j<=lattice_size_y;j++)
        for (i=1;i<=lattice_size_x;i++)
         topovec[(j-1)*lattice_size_x+i]=topo[i][j];
       indexx(lattice_size_x*lattice_size_y,topovec,topovecind);
       t=0;
       while (t<lattice_size_x*lattice_size_y)
        {t++;
         i=(topovecind[t])%lattice_size_x;
         if (i==0) i=lattice_size_x;
         j=(topovecind[t])/lattice_size_x+1;
         if (i==lattice_size_x) j--;
         avalanche(i,j);}
       for (j=1;j<=lattice_size_y;j++)
        for (i=1;i<=lattice_size_x;i++)
         topoold[i][j]=topo[i][j];
       for (j=1;j<=lattice_size_y;j++)
        for (i=1;i<=lattice_size_x;i++)
         fillinpitsandflats(i,j);
       for (j=1;j<=lattice_size_y;j++)
        for (i=1;i<=lattice_size_x;i++)
         {flow[i][j]=1;
          topovec[(j-1)*lattice_size_x+i]=topo[i][j];}
       indexx(lattice_size_x*lattice_size_y,topovec,topovecind);
       t=lattice_size_x*lattice_size_y+1;
       while (t>1)
        {t--;
         i=(topovecind[t])%lattice_size_x;
         if (i==0) i=lattice_size_x;
         j=(topovecind[t])/lattice_size_x+1;
         if (i==lattice_size_x) j--;
         mfdflowroute(i,j);}
       for (i=2;i<=lattice_size_x-1;i++)
        for (j=2;j<=lattice_size_y-1;j++)
         {topo[i][j]+=U*timestep;
          topoold[i][j]+=U*timestep;}
       /*perform upwind erosion*/
       max=0;
	   // Potential parallel
       for (i=2;i<=lattice_size_x-1;i++)
        for (j=2;j<=lattice_size_y-1;j++)
         {calculatealongchannelslope(i,j);
          deltah=timestep*K*sqrt(flow[i][j])*deltax*slope[i][j];
          topo[i][j]-=deltah;
          if (topo[i][j]<0) topo[i][j]=0;
          if (K*sqrt(flow[i][j])*deltax>max) max=K*sqrt(flow[i][j])*deltax;}
       time+=timestep;
       if (max>0.3*deltax/timestep)
        {time-=timestep;
         timestep/=2.0;
         for (i=2;i<=lattice_size_x-1;i++)
          for (j=2;j<=lattice_size_y-1;j++)
           topo[i][j]=topoold[i][j]-U*timestep;}
        else
        {if (max<0.03*deltax/timestep) timestep*=1.2;
         for (j=1;j<=lattice_size_y;j++)
          for (i=1;i<=lattice_size_x;i++)
           topoold[i][j]=topo[i][j];}
       if ((int)time == printinterval )
       {
		   printinterval+=printinterval;
		   // This was <=
	       for (i=1;i<lattice_size_x;i++)
		   {
			   // This was <=
               for (j=1;j<lattice_size_y;j++)
               {
				   // NOTE: Output grid size is incorrect, needs to account for 1 cell boundary
				   OutputGrid->Set_Value(i, j, topo[i][j]);
			       //fprintf(fp1,"%f\n",topo[i][j]);
		       }
		   }
	   }
	 }
}