/* For copyright information, see olden_v1.0/COPYRIGHT */

/* compute.c
 *
 * By:  Martin C. Carlisle
 * 6/15/94
 *
 * Implements computation phase of the Power Pricing problem
 * based on code by: Steve Lumetta, Sherry Li, and Ismail Khalil
 * University of California at Berkeley
 *
 */

#include "power.h"

/*----------------------------------------------------------------------*/
/* Leaf optimization 'global' variables               */

static double P=1.0;
static double Q=1.0;


/*----------------------------------------------------------------------*/
/* Leaf optimization procedures                 */

void optimize_node (double pi_R, double pi_I);
double find_g ();
double find_h ();
double find_gradient_f (double pi_R, double pi_I, double* gradient);
double find_gradient_g (double* gradient);
double find_gradient_h (double* gradient);
void find_dd_grad_f (double pi_R, double pi_I, double* dd_grad);
double make_orthogonal (double* v_mod, double* v_static);


void Compute_Tree(Root r) {
  int i;
  Lateral l;
  Demand a;
  Demand tmp;
  double theta_R,theta_I;

  tmp.P = 0.0;
  tmp.Q = 0.0;
  for (i=0; i<NUM_FEEDERS; i++) {
    l = r->feeders[i];
    theta_R = r->theta_R;
    theta_I = r->theta_I;
    a = Compute_Lateral(l,theta_R,theta_I,theta_R,theta_I);
    tmp.P += a.P;
    tmp.Q += a.Q;
  }
  r->D.P = tmp.P;
  r->D.Q = tmp.Q;
}

Demand Compute_Lateral(Lateral l, double theta_R, double theta_I, 
                       double pi_R, double pi_I) {
  Demand a1;
  Demand a2;
  double new_pi_R, new_pi_I;
  double a,b,c,root;
  Lateral next;
  Branch br;
  
  new_pi_R = pi_R + l->alpha*(theta_R+(theta_I*l->X)/l->R);
  new_pi_I = pi_I + l->beta*(theta_I+(theta_R*l->R)/l->X);

  next = l->next_lateral;
  if (next != NULL) 
    a1 = Compute_Lateral(next,theta_R,theta_I,new_pi_R,new_pi_I);

  br = l->branch;
  a2 = Compute_Branch(br,theta_R,theta_I,new_pi_R,new_pi_I);

  if (next != NULL) {
    l->D.P = a1.P + a2.P;
    l->D.Q = a1.Q + a2.Q;
  } else {
    l->D.P = a2.P;
    l->D.Q = a2.Q;
  }

  /* compute P,Q */
  a = l->R*l->R + l->X*l->X;  
  b = 2*l->R*l->X*l->D.Q - 2*l->X*l->X*l->D.P - l->R;
  c = l->R*l->D.Q - l->X*l->D.P;
  c = c*c + l->R*l->D.P;
  root = (-b-sqrt(b*b-4*a*c))/(2*a);
  l->D.Q = l->D.Q + ((root-l->D.P)*l->X)/l->R;
  l->D.P = root;

  /* compute alpha, beta */
  a = 2*l->R*l->D.P;
  b = 2*l->X*l->D.Q;
  l->alpha = a/(1-a-b);
  l->beta = b/(1-a-b);
  return l->D;
}

Demand Compute_Branch(Branch br, double theta_R, double theta_I, 
                       double pi_R, double pi_I) {
  Demand a2,tmp;
  double new_pi_R, new_pi_I;
  double a,b,c,root;
  Leaf l;
  Branch next;
  int i;
  Demand a1;
  
  new_pi_R = pi_R + br->alpha*(theta_R+(theta_I*br->X)/br->R);
  new_pi_I = pi_I + br->beta*(theta_I+(theta_R*br->R)/br->X);

  next = br->next_branch;
  if (next != NULL)  {
    a1 = Compute_Branch(next,theta_R,theta_I,new_pi_R,new_pi_I);
  }

  /* Initialize tmp */
  tmp.P = 0.0; tmp.Q = 0.0;

  for (i=0; i<LEAVES_PER_BRANCH; i++) {
    l = br->leaves[i];
    a2 = Compute_Leaf(l,new_pi_R,new_pi_I);
    tmp.P += a2.P;
    tmp.Q += a2.Q;
  }
  if (next != NULL) {
    br->D.P = a1.P + tmp.P;
    br->D.Q = a1.Q + tmp.Q;
  } else {
    br->D.P = tmp.P;
    br->D.Q = tmp.Q;
  }

  /* compute P,Q */
  a = br->R*br->R + br->X*br->X;  
  b = 2*br->R*br->X*br->D.Q - 2*br->X*br->X*br->D.P - br->R;
  c = br->R*br->D.Q - br->X*br->D.P;
  c = c*c + br->R*br->D.P;
  root = (-b-sqrt(b*b-4*a*c))/(2*a);
  br->D.Q = br->D.Q + ((root-br->D.P)*br->X)/br->R;
  br->D.P = root;
  /* compute alpha, beta */
  a = 2*br->R*br->D.P;
  b = 2*br->X*br->D.Q;
  br->alpha = a/(1-a-b);
  br->beta = b/(1-a-b);

  return br->D;
}

Demand Compute_Leaf(Leaf l, double pi_R, double pi_I) {
  P = l->D.P;
  Q = l->D.Q;
  
  optimize_node(pi_R,pi_I);

  if (P<0.0) {
    P = 0.0;
    Q = 0.0;
  }
  l->D.P = P;
  l->D.Q = Q;
  return l->D;
}

/*----------------------------------------------------------------------*/

void optimize_node (double pi_R, double pi_I)
{
    double	    g;
    double	    h;

    double	    grad_f[2];
    double	    grad_g[2];
    double	    grad_h[2];
    double	    dd_grad_f[2];
    double	    magnitude;

    int		    i;
    double	    total;
    double	    max_dist;

    do {
	/* Move onto h=0 line */
	h=find_h ();
	if (fabs (h)>H_EPSILON) {
	    magnitude=find_gradient_h (grad_h);
	    total=h/magnitude;
	    P-=total*grad_h[0];
	    Q-=total*grad_h[1];
	}

	/* Check that g is still valid */
	g=find_g ();
	if (g>G_EPSILON) {
	    magnitude=find_gradient_g (grad_g);
	    find_gradient_h (grad_h);
	    magnitude*=make_orthogonal (grad_g,grad_h);
	    total=g/magnitude;
	    P-=total*grad_g[0];
	    Q-=total*grad_g[1];
	}

	/* Maximize benefit */
	magnitude=find_gradient_f (pi_R,pi_I,grad_f);
	find_dd_grad_f (pi_R,pi_I,dd_grad_f);
	total=0.0;
	for (i=0; i<2; i++)
	    total+=grad_f[i]*dd_grad_f[i];
	magnitude/=fabs (total);
	find_gradient_h (grad_h);
	magnitude*=make_orthogonal (grad_f,grad_h);
	find_gradient_g (grad_g);
	total=0.0;
	for (i=0; i<2; i++)
	    total+=grad_f[i]*grad_g[i];
	if (total>0) {
	    max_dist=-find_g ()/total;
	    if (magnitude>max_dist)
		magnitude=max_dist;
	}
	P+=magnitude*grad_f[0];
	Q+=magnitude*grad_f[1];

	h=find_h ();
	g=find_g ();
	find_gradient_f (pi_R,pi_I,grad_f);
	find_gradient_h (grad_h);

    } while (fabs (h)>H_EPSILON || g>G_EPSILON ||
	    (fabs (g)>G_EPSILON &&
		fabs (grad_f[0]*grad_h[1]-grad_f[1]*grad_h[0])>F_EPSILON));
}

double find_g ()
{
    return (P*P+Q*Q-0.8);
}

double find_h ()
{
    return (P-5*Q);
}

double find_gradient_f (double pi_R, double pi_I, double* gradient)
{
    int		    i;
    double	    magnitude=0.0;

    gradient[0]=1/(1+P)-pi_R;
    gradient[1]=1/(1+Q)-pi_I;
    for (i=0; i<2; i++)
	magnitude+=gradient[i]*gradient[i];
    magnitude=sqrt (magnitude);
    for (i=0; i<2; i++)
	gradient[i]/=magnitude;

    return magnitude;
}

double find_gradient_g (double* gradient)
{
    int		    i;
    double	    magnitude=0.0;

    gradient[0]=2*P;
    gradient[1]=2*Q;
    for (i=0; i<2; i++)
	magnitude+=gradient[i]*gradient[i];
    magnitude=sqrt (magnitude);
    for (i=0; i<2; i++)
	gradient[i]/=magnitude;

    return magnitude;
}

double find_gradient_h (double* gradient)
{
    int		    i;
    double	    magnitude=0.0;

    gradient[0]=1.0;
    gradient[1]=-5.0;
    for (i=0; i<2; i++)
	magnitude+=gradient[i]*gradient[i];
    magnitude=sqrt (magnitude);
    for (i=0; i<2; i++)
	gradient[i]/=magnitude;

    return magnitude;
}

void find_dd_grad_f (double pi_R, double pi_I, double* dd_grad)
{
    double	    P_plus_1_inv=1/(P+1);
    double	    Q_plus_1_inv=1/(Q+1);
    double	    P_grad_term=P_plus_1_inv-pi_R;
    double	    Q_grad_term=Q_plus_1_inv-pi_I;
    double	    grad_mag;
    
    grad_mag=sqrt (P_grad_term*P_grad_term+Q_grad_term*Q_grad_term);

    dd_grad[0]=-P_plus_1_inv*P_plus_1_inv*P_grad_term/grad_mag;
    dd_grad[1]=-Q_plus_1_inv*Q_plus_1_inv*Q_grad_term/grad_mag;
}

double make_orthogonal (double* v_mod, double* v_static)
{
    int		    i;
    double	    total=0.0;
    double	    length=0.0;

    for (i=0; i<2; i++)
	total+=v_mod[i]*v_static[i];
    for (i=0; i<2; i++) {
	v_mod[i]-=total*v_static[i];
	length+=v_mod[i]*v_mod[i];
    }
    length=sqrt (length);
    for (i=0; i<2; i++)
	v_mod[i]/=length;

    if (1-total*total<0)    /* Roundoff error--vectors are parallel */
	return 0;

    return sqrt (1-total*total);
}

/*----------------------------------------------------------------------*/