#include "Animal.h"

const double SQRT2 = sqrt(2.0); // more efficient than calculating every time

ofstream out ("output1.txt");
bool write_out = WRITE_OUTPUT1;
ofstream out2 ("output2.txt");
bool write_out2 = WRITE_OUTPUT2;

int Animal::AnimalCounter = 0;

StochasticLib1 sto1animal(SEED);

/*------------------------------------------------------------------------------
FUNCTION:   Animal::Animal
ARGUMENTS:  x,y co-ordinates for new animal, year, prob of being male
RETURNS:    none
NOTES:      Animal class default Constructor - used only for initial population
------------------------------------------------------------------------------*/
Animal::Animal(int x1, int y1, int yr, double Pfemale)
{
id = AnimalCounter; AnimalCounter++; //Unique identifier for each animal
// set up all animals to be adults
age = sto1animal.IRandom(2,5); //maybe do something more elegant here?
x = x1; y = y1; natal_x = 0; natal_y = 0;
natal_year = yr;
if (yr < 0) { // intial population - pmale indicates sex of animal
  if (Pfemale > 0.5) sex = 1; else sex = 0;
  status = 3; success = 1; // set up as a successful breeder who remains in situ
}
else { // Pfemale indicates proportion of females
  sex = sto1animal.Bernoulli(Pfemale);
  status = 2; success = 0;
}
// default movement parameters
pr = 5; prmethod = 2; dp = gb = 1.0; memorysize = 1;
// default goal parameters
goaltype = 0; // indicates no goal set
goalx = 0; goaly = 0; 

}

/*------------------------------------------------------------------------------
FUNCTION:   Animal::Animal
ARGUMENTS:  Animal* = Pointer to parent
RETURNS:    none
NOTES:      Animal Copy Constructor
------------------------------------------------------------------------------*/
Animal::Animal(int yr, double Pfemale, const Animal* b)
{
id = AnimalCounter; AnimalCounter++; //Unique identifier for each animal.
age = 0;
x = b->x; y = b->y; natal_x = b->x; natal_y = b->y;
natal_year = yr;
status = 0; success = 0;
sex = sto1animal.Bernoulli(Pfemale);
// movement parameters
pr = b->pr; prmethod = b->prmethod; dp = b->dp; gb = b->gb; memorysize = b->memorysize;
// goal parameters
goaltype = b->goaltype; goalx = b->goalx; goaly = b->goaly;
}

/*------------------------------------------------------------------------------
FUNCTION:		Animal::various functions to set or update animal attributes
ARGUMENTS:
RETURNS:
NOTES:
------------------------------------------------------------------------------*/

void Animal::set_status(int s) { status = s; }
void Animal::set_success(int s) { success = s; }
void Animal::set_location(int lx, int ly) { x = lx; y = ly; }
void Animal::age_incr(void) { age++; }

void Animal::set_goal(int g, int lx, int ly) {
if (g >= 0 && g <= 2) {
  goaltype = g;
  if (g > 0) goalx = lx; goaly = ly;
}
else goaltype = 0;
}

void Animal::set_movement(int p, int m, double d, double g, int ms, int nd) {
if (p < 1) pr = 1; else pr = p;
if (m < 1 || m > 3) prmethod = 2; else prmethod = m;
if (d < 1.0) dp = 1.0; else dp = d;
if (g < 1.0) gb = 1.0; else gb = g;
if (ms < 1) memorysize = 1;
else {
  if (memorysize > 14) // causes an exception error, so make this the max allowed
    memorysize = 14;
  else memorysize = ms;
}
if (nd < 1) nodatacost = 1000; else nodatacost = nd;
}
void Animal::set_dp(double d) { if (d < 1.0) dp = 1.0; else dp = d; }

/*------------------------------------------------------------------------------
FUNCTION:		Animal::various functions to get animal attributes
ARGUMENTS:
RETURNS:
NOTES:
------------------------------------------------------------------------------*/

int Animal::get_id() { return id; }
int Animal::get_age() { return age; }
int Animal::get_natalyear() { return natal_year; }
int Animal::get_sex() { return sex; }
int Animal::get_status() { return status; }
int Animal::get_success() { return success; }
int Animal::get_pr() { return pr; }
int Animal::get_prmethod() { return prmethod; }
double Animal::get_dp() { return dp; }
double Animal::get_gb() { return gb; }
int Animal::get_memsize() { return memorysize; }
locn Animal::get_location() { locn l; l.x = x; l.y = y; return l; }
locn Animal::get_natalloc() { locn l; l.x = natal_x; l.y = natal_y; return l; }
locn Animal::get_goal() { locn l; l.x = goalx; l.y = goaly; return l; }
int Animal::get_goaltype() { return goaltype; }

void Animal::relmove(int dx, int dy) { // make a relative move
locn current;
current.x = x; current.y = y;
if (memory.size() == memorysize) {
  memory.pop(); // remove oldest memory element
}
memory.push(current); // record previous location in memory
if (write_out) out<<"queue length is "<<memory.size()<<endl;
x += dx; y += dy;
}

/*------------------------------------------------------------------------------
FUNCTION:		Animal::nbrmove
ARGUMENTS:  pointer to landscape, step number
RETURNS:    distance and cost of move
NOTES:      Move to a neighbouring cell according to the SMS algorithm
------------------------------------------------------------------------------*/

movedata Animal::nbrmove(Landscape* pLand, int step)
{

array3x3d nbr; // to hold weights/costs/probs of moving to neighbouring cells
array3x3d goal; // to hold weights for moving towards a goal location
array3x3d hab; // to hold weights for habitat (includes percep range)
int x2,y2; // x index from 0=W to 2=E, y index from 0=N to 2=S
int oob; // out-of-bounds indicator;
Square* sq;
Square* newsq;
double sum_nbrs = 0.0;
movedata move;

int xmax = pLand->get_bounds().x;
int ymax = pLand->get_bounds().y;

if (write_out) {
  out<<endl<<"ind = "<<get_id()<<" pr = "<<get_pr()<<" step = "<<step;
  if (step == 0) out<<" start at "<<x<<" "<<y;
  out<<endl;
}

sq = pLand->locate_square(x,y);
if (sq == 0) {
// x,y is a NODATA square - this should not occur here
// return a negative distance to indicate an error
  move.dist = -1.0; move.cost = 0.0;
  return move;
}

//get weights for directional persistence....
nbr = get_sim_dir();
if (write_out) {
  out<<endl<<"directional persistence weights:"<<endl;
	for (y2 = 2; y2 > -1; y2--) {
    for (x2 = 0; x2 < 3; x2++) out<<nbr.cell[x2][y2]<<" ";
	  out<<endl;
	}
}
if (write_out2)
  out2<<endl<<"ind = "<<get_id()<<" pr = "<<get_pr()<<" step = "<<step<<endl<<endl;

//get weights for goal bias....
goal = get_goal_bias();
if (write_out) {
  out<<"goal bias weights:"<<endl;
	for (y2 = 2; y2 > -1; y2--) {
    for (x2 = 0; x2 < 3; x2++) out<<goal.cell[x2][y2]<<" ";
	  out<<endl;
	}
}
//if (write_out2)
//  out2<<endl<<"ind = "<<get_id()<<" pr = "<<get_pr()<<" step = "<<step<<endl<<endl;

// get habitat-dependent weights (mean effective costs, given perceptual range)
// first check if costs have already been calculated

hab = sq->get_effcosts();
//if (write_out) {
//  out<<"stored effective costs:"<<endl;
//	for (y2 = 2; y2 > -1; y2--) {
//    for (x2 = 0; x2 < 3; x2++) out<<hab.cell[x2][y2]<<" ";
//	  out<<endl;
//	}
//}
if (hab.cell[0][0]<0.0) { // they have not been calculated
  hab = get_hab_matrix(pLand,xmax,ymax);
  sq->set_effcosts(hab); // store the costs
}
else { // they have been calculated
  if (write_out) {
    out<<"*** using previous effective costs ***"<<endl;
  }
}
if (write_out) {
  out<<"mean effective costs:"<<endl;
	for (y2 = 2; y2 > -1; y2--) {
    for (x2 = 0; x2 < 3; x2++) {
      out<<hab.cell[x2][y2]<<" ";
    }
		out<<endl;
  }
	out<<"weighted effective costs:"<<endl;
}

// determine weighted effective cost for the 8 neighbours
// multiply directional persistence, goal bias and habitat habitat-dependent weights
for (y2 = 2; y2 > -1; y2--) {
  for (x2 = 0; x2 < 3; x2++) {
    if(x2==1 && y2==1) nbr.cell[x2][y2]=0.0;
    else {
      if(x2==1 || y2==1) //not diagonal
        nbr.cell[x2][y2] = nbr.cell[x2][y2]*goal.cell[x2][y2]*hab.cell[x2][y2];
      else // diagonal
        nbr.cell[x2][y2] = SQRT2*nbr.cell[x2][y2]*goal.cell[x2][y2]*hab.cell[x2][y2];
    }
    if (write_out) {
      out<<nbr.cell[x2][y2]<<" "; if (x2==1 && y2==1) out<<"         ";
    }
  }
	if (write_out) out<<endl;
}

// determine reciprocal of effective cost for the 8 neighbours
if (write_out) out<<"reciprocal weighted effective costs:"<<endl;
for (y2 = 2; y2 > -1; y2--) {
  for (x2 = 0; x2 < 3; x2++) {
    if (nbr.cell[x2][y2] > 0.0) nbr.cell[x2][y2] = 1.0/nbr.cell[x2][y2];
    if (write_out) {
      out<<nbr.cell[x2][y2]<<" "; if (x2==1 && y2==1) out<<"         ";
    }
    sum_nbrs += nbr.cell[x2][y2];
  }
	if (write_out) out<<endl;
}

// scale effective costs as probabilities summing to 1
if (write_out) out<<"probabilities:"<<endl;
for (y2 = 2; y2 > -1; y2--) {
  for (x2 = 0; x2 < 3; x2++) {
    nbr.cell[x2][y2] = nbr.cell[x2][y2]/sum_nbrs;
    if (write_out) {
      out<<nbr.cell[x2][y2]<<" "; if (x2==1 && y2==1) out<<"      ";
    }
  }
  if (write_out) out<<endl;
}

// select direction at random based on cell selection probabilities
double rnd = sto1animal.Random();
if (write_out) out<<"rnd = "<<rnd<<endl;
double sum_rnd = 0.0;
for (y2 = 0; y2 < 3; y2++) {
  for (x2 = 0; x2 < 3; x2++) {
    // NOTE: THE NEXT STATEMENT ASSUMES UNIT CELL SIZE, WHICH MAY NEED TO BE CHANGED
    if (x2 == 1 || y2 == 1) move.dist = 1.0;
    else move.dist = SQRT2;
    sum_rnd += nbr.cell[x2][y2];
    if (write_out) out<<"dx = "<<x2-1<<" dy = "<<y2-1<<" sum_rnd = "<<sum_rnd<<endl;
    if (rnd < sum_rnd) {
      if ((x+x2-1) < 0 || (x+x2-1) >= xmax || (y+y2-1) < 0 || (y+y2-1) >= ymax) {
        // move goes out-of-bounds
        oob = 1;
        move.cost = move.dist*0.5*(double)(sq->get_cost());
      }
      else {
        oob = 0;
        newsq = pLand->locate_square(x+x2-1,y+y2-1);
        if (newsq == 0) { // move goes to NODATA square
          move.cost = move.dist*0.5*(double)(sq->get_cost());
        }
        else {
          move.cost = move.dist*0.5*(double)(sq->get_cost() + newsq->get_cost());
        }
      }
      relmove(x2-1,y2-1); // make the selected move
      if (write_out) {
        out<<"relative x and y "<<x2-1<<" "<<y2-1<<endl;
        out<<"cost of move "<<move.cost<<endl;
        out<<"move to: x = "<<x<<" y = "<<y;
        if (oob) out<<" ***** OUT OF BOUNDS *****";
        out<<endl;
      }
      y2=999; x2=999; //to break out of x2 and y2 loops.
    }
  }
}
return move;
}


/*------------------------------------------------------------------------------
FUNCTION:		Animal::get_sim_dir
ARGUMENTS:  none
RETURNS:    3x3 array of weightings
NOTES:
------------------------------------------------------------------------------*/

array3x3d Animal::get_sim_dir(void) {

array3x3d d;
locn prev;
double theta;
int xx,yy;

//if (write_out) out<<"step 0"<<endl;
if (memory.empty())
{ // no previous movement, set matrix to unity
  for (xx = 0; xx < 3; xx++) {
    for (yy = 0; yy < 3; yy++) {
       d.cell[xx][yy] = 1;
    }
  }
}
else { // set up the matrix dependent on relationship of previous location to current
//  if (write_out) out<<"step 1"<<endl;
  d.cell[1][1]=0;
  prev = memory.front();
//  if (write_out) out<<"step 2"<<endl;
  if ((x-prev.x) == 0 && (y-prev.y) == 0) {
  // back to 'square 1' (first memory location) - use previous step drn only
    prev = memory.back();
//    if (write_out) out<<"step 3"<<endl;
    if (write_out) out<<"*** using last step only: x,y = "<<prev.x<<","<<prev.y<<endl;
    if ((x-prev.x) == 0 && (y-prev.y) == 0) { // STILL HAVE A PROBLEM!
      for (xx = 0; xx < 3; xx++) {
        for (yy = 0; yy < 3; yy++) {
          d.cell[xx][yy] = 1.0;
        }
      }
//      if (write_out) out<<"step 4"<<endl;
      return d;
    }
  }
  else {
//    if (write_out) out<<"step 5"<<endl;
  }
//  if (write_out) out<<"step 6"<<endl;
  theta = atan2((double)(x-prev.x),(double)(y-prev.y));
  if (write_out) out<<"prev.x,prev.y: "<<prev.x<<","<<prev.y<<" theta: "<<theta<<endl;
  d = calc_weightings(dp,theta);

}
return d;
}


/*------------------------------------------------------------------------------
FUNCTION:		Animal::get_goal_bias
ARGUMENTS:  none
RETURNS:    3x3 array of weightings
NOTES:
------------------------------------------------------------------------------*/

array3x3d Animal::get_goal_bias(void) {

array3x3d d;
double theta;
int xx,yy;

if (goaltype == 0) { // no goal set
  for (xx = 0; xx < 3; xx++) {
    for (yy = 0; yy < 3; yy++) {
      d.cell[xx][yy] = 1.0;
    }
  }
}
else {
  d.cell[1][1]=0;
  if ((x-goalx) == 0 && (y-goaly) == 0) { // at goal, set matrix to unity
    if (write_out) out<<"*** at goal: x,y = "<<goalx<<","<<goaly<<endl;
    for (xx = 0; xx < 3; xx++) {
      for (yy = 0; yy < 3; yy++) {
        d.cell[xx][yy] = 1.0;
      }
    }
    return d;
  }
  if (goaltype == 1) theta = atan2((double)(goalx-x),(double)(goaly-y));
  else theta = atan2((double)(x-goalx),(double)(y-goaly)); // goaltype == 2
  if (write_out) out<<"goalx,goaly: "<<goalx<<","<<goaly<<" theta: "<<theta<<endl;
  d = calc_weightings(gb,theta);
}

return d;
}

/*------------------------------------------------------------------------------
FUNCTION:		Animal::calc_weightings
ARGUMENTS:  direction (radians, -pi to +pi)
RETURNS:    3x3 array of weightings
NOTES:
------------------------------------------------------------------------------*/

array3x3d Animal::calc_weightings(double base, double theta) {

array3x3d d; // 3x3 array indexed from SW corner by xx and yy
int dx,dy,xx,yy;

double i0 = pow(base,0); // direction of theta - lowest cost bias
double i1 = pow(base,1);
double i2 = pow(base,2);
double i3 = pow(base,3);
double i4 = pow(base,4); // opposite to theta - highest cost bias

if (fabs(theta) > 7.0*M_PI/8.0) { dx = 0; dy = -1; }
else {
  if (fabs(theta) > 5.0*M_PI/8.0) { dy = -1; if (theta > 0) dx = 1; else dx = -1; }
  else {
    if (fabs(theta) > 3.0*M_PI/8.0) { dy = 0; if (theta > 0) dx = 1; else dx = -1; }
    else {
      if (fabs(theta) > M_PI/8.0) { dy = 1; if (theta > 0) dx = 1; else dx = -1; }
      else { dy = 1; dx = 0; }
    }
  }
}
//  if (write_out) out<<"goalx,goaly: "<<goalx<<","<<goaly<<" dx,dy: "<<dx<<","<<dy
//    <<" theta: "<<theta<<endl;
d.cell[1][1] = 0; // central cell has zero weighting
d.cell[dx+1][dy+1] = i0;
d.cell[-dx+1][-dy+1] = i4;
if (dx == 0 || dy ==0) { // theta points to a cardinal direction
  d.cell[dy+1][dx+1] = i2; d.cell[-dy+1][-dx+1] = i2;
  if (dx == 0) { // theta points N or S
    xx = dx+1; if (xx > 1) dx -= 2; yy = dy;
    d.cell[xx+1][yy+1] = i1; d.cell[-xx+1][yy+1] = i1;
    d.cell[xx+1][-yy+1] = i3; d.cell[-xx+1][-yy+1] = i3;
  }
  else { // theta points W or E
    yy = dy+1; if (yy > 1) dy -= 2; xx = dx;
    d.cell[xx+1][yy+1] = i1; d.cell[xx+1][-yy+1] = i1;
    d.cell[-xx+1][yy+1] = i3; d.cell[-xx+1][-yy+1] = i3;
  }
}
else { // theta points to an ordinal direction
  d.cell[dx+1][-dy+1] = i2; d.cell[-dx+1][dy+1] = i2;
  xx = dx+1; if (xx > 1) xx -= 2; d.cell[xx+1][dy+1] = i1;
  yy = dy+1; if (yy > 1) yy -= 2; d.cell[dx+1][yy+1] = i1;
  d.cell[-xx+1][-dy+1] = i3; d.cell[-dx+1][-yy+1] = i3;
  }

return d;
}

/*------------------------------------------------------------------------------
FUNCTION:		Animal::get_hab_matrix
ARGUMENTS:  pointer to landscape, maximumum X and Y values
RETURNS:    3x3 array of effective costs
NOTES:
------------------------------------------------------------------------------*/

array3x3d Animal::get_hab_matrix(Landscape* pLand,int xbound,int ybound){

array3x3d w; // array of effective costs to be returned
int ncells,x4,y4,targetseen;
double weight,sumweights;
// NW and SE corners of effective cost array relative to the current cell (x,y):
int xmin, ymin, xmax, ymax,cost;
Square* sq;

for (int x2=-1; x2<2; x2++) {   // index of relative move in x direction
	for (int y2=-1; y2<2; y2++) { // index of relative move in x direction

		w.cell[x2+1][y2+1] = 0.0; // initialise costs array to zeroes

		// set up corners of perceptual range relative to current cell
		if (x2==0 && y2==0) { // current cell - do nothing
			xmin=0; ymin=0; xmax=0; ymax=0;
		}
		else {
			if (x2==0 || y2==0) { // not diagonal (rook move)
				if (x2==0){ // vertical (N-S) move
					//out<<"ROOK N-S: x2 = "<<x2<<" y2 = "<<y2<<endl;
					if(pr%2==0) { xmin=-pr/2; xmax=pr/2; ymin=y2; ymax=y2*pr; } // PR even
					else { xmin=-(pr-1)/2; xmax=(pr-1)/2; ymin=y2; ymax=y2*pr; } // PR odd
				}
				if (y2==0) { // horizontal (E-W) move
					//out<<"ROOK E-W: x2 = "<<x2<<" y2 = "<<y2<<endl;
					if(pr%2==0) { xmin=x2; xmax=x2*pr; ymin=-pr/2; ymax=pr/2; } // PR even
					else { xmin=x2; xmax=x2*pr; ymin=-(pr-1)/2; ymax=(pr-1)/2; } // PR odd
				}
			}
			else { // diagonal (bishop move)
				//out<<"BISHOP: x2 = "<<x2<<" y2 = "<<y2<<endl;
				xmin=x2; xmax=x2*pr; ymin=y2; ymax=y2*pr;
			}
		}
		//out<<"pre  swap: xmin = "<<xmin<<" ymin = "<<ymin<<" xmax = "<<xmax<<" ymax = "<<ymax<<endl;
		if(xmin>xmax) {int z=xmax; xmax=xmin; xmin=z;} // swap xmin and xmax
		if(ymin>ymax) {int z=ymax; ymax=ymin; ymin=z;} // swap ymin and ymax
		//out<<"post swap: xmin = "<<xmin<<" ymin = "<<ymin<<" xmax = "<<xmax<<" ymax = "<<ymax<<endl;
		if (write_out2) {
			out2<<"current x and y  "<<x<<" "<<y<<endl;
			out2<<"x2 and y2  "<<x2<<" "<<y2<<endl;
			out2<<"xmin,ymin "<<xmin<<","<<ymin<<" xmax,ymax "<<xmax<<","<<ymax<<endl;
		}

		// calculate effective mean cost of cells in perceptual range
		ncells = 0; weight = 0.0; sumweights = 0.0;
    targetseen = 0;
		if (x2 != 0 || y2 != 0) { // not central cell (i.e. current cell)
			for (int x3=xmin; x3<=xmax; x3++) {
				for (int y3=ymin; y3<=ymax; y3++) {
          // if cell is out of bounds, treat landscape as a torus
          // for purpose of obtaining a cost,
          if ((x+x3) < 0) x4 = x+x3+xbound;
          else { if ((x+x3) >= xbound) x4 = x+x3-xbound; else x4 = x+x3; }
          if ((y+y3) < 0) y4 = y+y3+ybound;
          else { if ((y+y3) >= ybound) y4 = y+y3-ybound; else y4 = y+y3; }
          if (write_out && (x4 < 0 || y4 < 0)) {
            out<<"ERROR: x "<<x<<" y "<<y<<" x3 "<<x3<<" y3 "<<y3
              <<" xbound "<<xbound<<" ybound "<<ybound<<" x4 "<<x4<<" y4 "<<y4<<endl;
          }
          // add cost of cell to total PR cost
          sq = pLand->locate_square(x4,y4);
          if (sq == 0) cost = nodatacost;
          else cost = sq->get_cost();
          if (prmethod==1) { // arithmetic mean
            w.cell[x2+1][y2+1] += cost;
            ncells++;
          }
          if (prmethod==2) { // harmonic mean
            if (cost > 0) {
              w.cell[x2+1][y2+1] += (1/(double)cost);
              ncells++;
            }
          }
          if (prmethod==3) { // arithmetic mean weighted by inverse distance
            if (cost>0) {
              // NB distance is still given by (x3,y3)
              weight = 1/sqrt((double)(pow((double)x3,2)+pow((double)y3,2)));
              w.cell[x2+1][y2+1] += weight*(double)cost;
              ncells++; sumweights += weight;
            }
          }
          if (write_out2) out2<<x+x3<<","<<y+y3<<","<<cost<<" wt. "<<weight<<endl;
#if GO2TARGET
          if (sq != 0) {
            if (sq->get_target() > 50) targetseen++;
          }
#endif
				} //end of y3 loop
			}  //end of x3 loop
//		if (write_out) out<<"ncells in PR = "<<ncells<<" tot.wt. = "<<sumweights
//      <<" w.cell = "<<w.cell[x2+1][y2+1]<<endl;
		  if (ncells > 0) {
			  if (prmethod == 1) w.cell[x2+1][y2+1] /= ncells; // arithmetic mean
			  if (prmethod == 2) w.cell[x2+1][y2+1] = ncells/w.cell[x2+1][y2+1]; // hyperbolic mean
			  if (prmethod == 3 && sumweights > 0)
          w.cell[x2+1][y2+1] /= sumweights; // weighted arithmetic mean
      }
#if GO2TARGET
      if (targetseen > 0) // target is within PR - set to a very low score
        w.cell[x2+1][y2+1] = (1/(1000000*(double)targetseen));
#endif
		}
		if (write_out2) out2<<"effective mean cost "<<w.cell[x2+1][y2+1]<<endl;

	}//end of y2 loop
}//end of x2 loop

return w;

}

/*------------------------------------------------------------------------------
								THE END
------------------------------------------------------------------------------*/