#include <math.h>
#include <vga.h>
#include <vgagl.h>
#include <vgamouse.h>
#include <ctype.h>
#include <unistd.h>
#include <stdio.h>

// Set NITMAX to 100 (or more) to fit parameters.
#define NITMAX 1

// Set FIT_LOSSES to 1 to fit lossy element resistivities.
// Set FIT_LOSSES to 0 to fil model to reference elements.

#define FIT_LOSSES 1

#if (FIT_LOSSES == 0)
#define NO_OF_RODS 21
#else
#define NO_OF_RODS (21+20)
#endif
float alfa=10;

double q_teor[2*NO_OF_RODS];
double q_begin[2*NO_OF_RODS];
char *type_names[]={"Cu, massive    5011", //0
                    "Al, massive    6082", //1
                    "Al, tube       6063", //2
                    "Brass              ", //3
                    "Al, elox 15 um     ", //4
                    "Al, elox 30 um     ", //5
                    "Al, Paint 30 um    ", //6
                    "CO1(corroded Al)   ", //7 
                    "CO2(corroded Al)   ", //8
                    "CO3(corroded Al)   ", //9
                    "Thru boom, glue    ", //10
                    "Thru boom, Starlock", //11
                    "Above boom         " //12
                    };
double type_resistivity[]={17.2, //0
                           33.0, //1
                           31.0, //2
                           73.,  //3
                           36.,  //4
                           38.,  //5
                           67.,  //6
                           34.,  //7
                           78.,  //8
                           48.,  //9
                           66.,  //10
                           226.,  //11
                           55.   //12
                           };
double odmin[]={   3.98,     5.95,     7.97,     9.94,     3.88,
                   5.94,     8.12,    10.0,     15.93,     5.04,
                  14.94,    10.13,     9.91,     9.90,     3.89,
                   5.94,    10.,       4.,       6.,      10.,
                  10., 
                  10.05,     3.88,     5.89,    10.03,     3.88,     
                   5.90,    10.00,     3.89,     5.99,     9.86,
                   9.6,      8.03,     4.,       6.,       10.,
                   4.,       6.,      10.,       6.,       10. };
double odmax[]={   3.98,     5.95,     7.97,     9.94,     3.89,
                   5.95,     8.16,    10.15,    15.94,     5.05,
                  14.95,    10.15,    10.05,    10.23,     3.90,
                   5.96,    10.,       4.,       6.,      10.,
                   10.,
                  10.16,     3.88,     5.90,    10.25,     3.89,
                   5.91,    10.15,     3.93,     6.10,     9.86,
                   9.6,      8.03,     4.,       6.,       10.,
                   4.,       6.,      10.,       6.,       10. };
double id[]={        0,        0,        0,        0,        0,
                     0,      6.025,    8.015,   13.02,       0,
                     0,        8,        8,        8,        0,
                     0,        0,        0,        0,        8,
                     8,
                     8,        0,        0,        8,        0,
                     0,        8,        0,        0,        8,
                     8,        6,        0,        0,        8,
                     0,        0,        8,        0,        8 };
int type[]={         0,        0,        0,        0,        1,
                     1,        2,        2,        2,        3,
                     3,        2,        2,        2,        1,
                     1,        0,        1,        1,        2,
                     2,
                     4,        4,        4,        5,        5,
                     5,        6,        6,        6,        7,
                     8,        9,       10,       10,       10,
                     11,       11,      11,       12,       12 };
double len[]={   320.73,   315.28,   310.61,   307.53,   320.62,
                 315.24,   309.92,   307.16,   297.43,   317.43,
                 299.71,   307.19,   307.16,   307.13,   320.7,
                 315.25,   307.59,   320.70,   315.25,   307.19,
                 307.13,
                 307.19,   320.67,   315.15,   307.08,   320.48,
                 315.14,   306.06,   317.78,   312.72,   307.1,
                 307.4,    310.2 ,   325.56,   320.85,   313.46,
                 326.27,   320.85,   313.58,   319.77,   311.04 };
double bw1[]={    72.24,    53.34,    44.27,    38.27,   100.97,
                  75.18,    57.85,    49.82,    36.96,   116.81,
                  54.74,       -1,       -1,      -1,       -1,
                     -1,       -1,       -1,      -1,       -1,
                     -1,   
                     -1,       -1,       -1,      -1,       -1,
                     -1,       -1,       -1,      -1,       -1,
                     -1,       -1,       -1,      -1,       -1,
                     -1,       -1,       -1,      -1,       -1 };
double freq1[]={ 413.1627, 413.4702, 413.9129, 413.5039, 414.2176,
                 413.8954, 414.5748, 414.1087, 415.0896, 413.4087,
                 413.5706, 413.698,  414.074,  413.875,  413.268,
                 413.538,  413.323,  413.285,  413.538,  413.733,
                 413.876,
                 413.704,  413.389,  413.576,  413.731,  413.249,
                 413.536,  413.750,  413.405,  413.522,  414.239,
                 413.409,  414.261,  414.569,  414.415,  414.591,
                 414.520,  414.127,  414.483,  413.365,  414.304  };
double q1[]={      5723,     7743,     9345,    10800,     4081,
                   5500,     7147,     8320,    11200,     3537,
                   7563,     8191,     8150,     8295,     4097,
                   5470,    10780,     4030,     5485,     8313,
                   8350,
                   7825,     3908,     5315,     7663,     3748,
                   5145,     6272,     3026,     3846,     7900,
                   5576,     5926,     3295,     4525,     5700,
                   2120,     2875,     3250,     4407,     6651 };
double loss1[]={  52.55,    48.85,    46.23,    44.75,    55.05,
                  50.96,    47.68,    47.14,    43.39,    55.89,
                  47.26,    50.68,    50.67,    50.33,    58.69,
                  55.54,    48.8,     59.0,     55.4,     50.2,
                  50.8,   
                  50.92,    58.97,    55.65,    51.39,    59.32,
                  56.06,    52.72,    60.93,    58.77,    51.2,
                  54.0,     53.8,     59.9,     56.6,     53.8,
                  63.7,     60.5,     58.8,     57.9,     52.6  };
double bw2[]={    74.52,    55.69,    48.81,    42.91,   102.88,
                  75.62,    60.01,    53.21,    45.01,   115.93,
                  59.6,      -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1 };
double freq2[]={ 408.8191, 408.8197, 409.0333, 408.3346, 409.8517,
                 409.2518, 409.6101, 408.9654, 409.5114, 408.9406,
                 408.0118,   -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1 };
double q2[]={     5485,     7345,     8380,     9516,     3983,
                  5411,     6825,     7685,     9098,     3527,
                  6843,      -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1 };
double loss2[]={  51.18,    48.25,    45.71,    44.28,    54.05,
                  50.2,     47.41,    46.02,    42.96,    54.49,
                  46.66,     -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1,
                   -1,       -1,       -1,       -1,       -1 };
double qbox1=18899.052713;
double qbox2=14421.010924;
double qrod_fac=3502.648812;
double boxcap1=10.244590;
double boxcap2=74.357215;
double respar1=3164.674215;
double respar2=1738.655660;


#define LLSQ_MAXPAR 15
#define LLSQ_MAXEQ (2*NO_OF_RODS+LLSQ_MAXPAR)
int llsq_neq;
int llsq_npar;
double llsq_derivatives[LLSQ_MAXEQ*LLSQ_MAXPAR];
double llsq_errors[LLSQ_MAXEQ];
double llsq_steps[2*LLSQ_MAXPAR];
double par[LLSQ_MAXPAR];
double qrod_i(int i);


void compute_q(void)
{
double c1,c2;
double t1, davg, tr;
int i;
llsq_neq=0;
for(i=0;i<NO_OF_RODS; i++)
  {
  tr=type_resistivity[type[i]];
  davg=0.5*(odmin[i]+odmax[i]);
// We regard an element as a parallel resonant circuit with
// a Q that is proportional to the load resistance that is
// in parallel with L and C.
// Assume (to the first order) that the box adds another resistor
// in parallel and that the coupling to it is independent of
// element diameter and resistivity.
// Connecting resistors in parallel is the same as adding conductances (1/R)
  c1=1/qrod_i(i);  
  if(q1[i] > 0)
    {
// Compute Q for the large box
    c2=1/qbox1;
// Add the conductances of losses of element and box (first order)
    t1=c1+c2;
// The coupling may depend on the diameter.
// This expression does not fit too badly:    
    t1-=.000000001*boxcap1/(davg*davg*c1);
// A small correction that depends on the resistivity
    t1-=.0000001*respar1/tr;
    q_teor[llsq_neq]=1/t1;
    }
  else
    {
    q_teor[llsq_neq]=q1[i];
    }  
  llsq_neq++;
  if(q2[i] > 0)
    {
// Compute Q for the small box the same way      
    c2=1/qbox2;
    t1=c1+c2;
    t1-=.000000001*boxcap2/(davg*davg*c1);
    t1-=.0000001*respar2/tr;
    q_teor[llsq_neq]=1/t1;
    }
  else
    {
    q_teor[llsq_neq]=q2[i];
    }  
  llsq_neq++;
  }
llsq_neq+=llsq_npar;
}

double qrod_i(int i)
{
// Make the Q-value proportional to the average diameter
// and inversely proportional to the resistivity.
return qrod_fac*(odmax[i]+odmin[i])/sqrt(type_resistivity[type[i]]);
}


#if(FIT_LOSSES != 0)
#define NN 9

void store_par(void)
{
int i;
for(i=0; i<NN; i++)par[i]=type_resistivity[i+4];
llsq_npar=NN;
}

void recover_par(void)
{
int i;
for(i=0; i<NN; i++)type_resistivity[i+4]=par[i];
}
#else

void store_par(void)
{
//int i;
//for(i=1; i<5; i++)par[3+i]=type_resistivity[i];
par[0]=qbox1;
par[1]=qbox2;
par[2]=qrod_fac;
par[3]=boxcap1;
par[4]=boxcap2;
par[5]=respar1;
par[6]=respar2;

llsq_npar=7;
}

void recover_par(void)
{
//int i;
//for(i=1; i<5; i++)type_resistivity[i]=par[3+i];
qbox1=par[0];
qbox2=par[1];
qrod_fac=par[2];
boxcap1=par[3];
boxcap2=par[4];
respar1=par[5];
respar2=par[6];
}

#endif


int llsq1(void)
{
double aux[2*LLSQ_MAXPAR];
int ipiv[2*LLSQ_MAXPAR];
int kpiv;
int i,j,k;
double t1,sig,piv,beta;
if(llsq_npar > LLSQ_MAXPAR)
  {
  printf("\nTOO MANY LLSQ PARAMETERS");
  }
kpiv=piv=0;
for(k=0; k<llsq_npar; k++)
  {
  ipiv[k]=k;
  t1=0;
  for(i=0; i<llsq_neq; i++)
    {
    t1+=pow(llsq_derivatives[k*llsq_neq+i],2.0);
    }
  aux[k]=t1;
  if(t1 > piv)
    {
    piv=t1;
    kpiv=k;
    }
  }
if(piv == 0)return -1;
sig=sqrt(piv);
for(k=0; k<llsq_npar; k++)
  {
  if(kpiv>k)
    {
    t1=aux[k];
    aux[k]=aux[kpiv];
    aux[kpiv]=t1;
    for(i=k; i<llsq_neq; i++)
      {
      t1=llsq_derivatives[k*llsq_neq+i];
      llsq_derivatives[k*llsq_neq+i]=llsq_derivatives[kpiv*llsq_neq+i];
      llsq_derivatives[kpiv*llsq_neq+i]=t1;
      }
    }
  if(k > 0)
    {
    sig=0;
    for(i=k; i<llsq_neq; i++)
      {
      sig+=llsq_derivatives[k*llsq_neq+i]*llsq_derivatives[k*llsq_neq+i];
      }
    sig=sqrt(sig);
    }
  t1=llsq_derivatives[k*llsq_neq+k];
  if(t1 < 0 )sig=-sig;
  ipiv[kpiv]=ipiv[k];
  ipiv[k]=kpiv;
  beta=t1+sig;
  llsq_derivatives[k*llsq_neq+k]=beta;
  beta=1/(sig*beta);
  j=llsq_npar+k;
  aux[j]=-sig;
  if(k<llsq_npar-1)
    {
    piv=0;
    kpiv=k+1;
    for(j=k+1; j<llsq_npar; j++)
      {
      t1=0;
      for(i=k; i<llsq_neq; i++)
        {
        t1+=llsq_derivatives[k*llsq_neq+i]*llsq_derivatives[j*llsq_neq+i];
        }
      t1=beta*t1;
      for(i=k; i<llsq_neq; i++)
        {
        llsq_derivatives[j*llsq_neq+i]-=llsq_derivatives[k*llsq_neq+i]*t1;
        }
      t1=aux[j]-llsq_derivatives[j*llsq_neq+k]*llsq_derivatives[j*llsq_neq+k];
      aux[j]=t1;
      if(t1 > piv)
        {
        piv=t1;
        kpiv=j;
        }
      }
    }
  t1=0;
  for(i=k; i<llsq_neq; i++)
    {
    t1+=llsq_derivatives[k*llsq_neq+i]*llsq_errors[i];
    }
  t1*=beta;
  for(i=k; i<llsq_neq; i++)
    {
    llsq_errors[i]-=llsq_derivatives[k*llsq_neq+i]*t1;
    }
  }
llsq_steps[llsq_npar-1]=llsq_errors[llsq_npar-1]/aux[2*llsq_npar-1];
if(llsq_npar == 1)return 0;
for(k=llsq_npar-2; k>=0; k--)
  {
  t1=llsq_errors[k  ];
  for(i=k+1; i<llsq_npar; i++)
    {
    t1-=llsq_derivatives[i*llsq_neq+k]*llsq_steps[i];
    }
  t1/=aux[llsq_npar+k];
  llsq_steps[k]=llsq_steps[ipiv[k]];
  llsq_steps[ipiv[k]  ]=t1;
  }
return 0;
}




int main(int argc, char *argv[])
{
double t1,t2;
FILE *file;
int i,j,nit;
double errsum;
char chr, s[256],sh[128];
// Make silly use of parameters to suppress warning
chr=argv[0][0];
i=argc;
par[3]=0;
par[4]=0;
par[5]=0;
par[6]=0;
store_par();
file=fopen("qval_table","w");
sprintf(sh,
"no    material   alloy odmin odmax  id    le      bw     freq      Q   loss");
sprintf(s,"\n\n                        Box 70 x 30 x 30\n%s",sh);  
fprintf(file,"%s",s);
printf("%s",s);  
for(i=0;i<NO_OF_RODS; i++)
  {
  sprintf(s,"\n%2d %s %5.2f %5.2f %5.2f %6.2f %6.2f %8.4f %5.0f %5.2f",
                         i,type_names[type[i]],odmin[i],odmax[i],id[i],len[i],
                         bw1[i],freq1[i],q1[i],loss1[i]);
  fprintf(file,"%s",s);
  printf("%s",s);  
  }
sprintf(s,"\n                          Box 50 x 30 x 30\n%s",sh);  
fprintf(file,"%s",s);
printf("%s",s);  
for(i=0;i<NO_OF_RODS; i++)
  {
  sprintf(s,"\n%2d %s %5.2f %5.2f %5.2f %6.2f %6.2f %8.4f %5.0f %5.2f",
                         i,type_names[type[i]],odmin[i],odmax[i],id[i],len[i],
                         bw2[i],freq2[i],q2[i],loss2[i]);
  fprintf(file,"%s",s);
  printf("%s",s);  
  }
// We expect Q, bw and freq to have a linear dependence: Q=freq/bw
sprintf(s,"\nError (in%%) in relation Q=freq/bw\n     Box70   Box50");
fprintf(file,"%s",s);
printf("%s",s);  
for(i=0;i<NO_OF_RODS; i++)
  {
  if(bw1[i]>0)
    {
    t1=1000*freq1[i]/bw1[i];
    }
  else
    {
    t1=q1[i];
    }  
  if(bw2[i]>0)
    {
    t2=1000*freq2[i]/bw2[i];
    }
  else
    {
    t2=q2[i];
    }  
  sprintf(s,"\n%2d  %6.3f  %6.3f ",i,100*(1-t1/q1[i]),100*(1-t2/q2[i]));
  fprintf(file,"%s",s);
  printf("%s",s);  
  }
// Zero order approximation.
// Qrod=k*diam/sqrt(resistivity);
// Qbox=constant
// Qtotal=(Qbox*Qrod)/(Qbox+Qrod)  
sprintf(s,"\nNo  Qteo  Qexp  ratio   Qteo  Qexp  ratio");
fprintf(file,"%s",s);
printf("%s",s);  
nit=0;
compute_q();
minimize:;
nit++;
errsum=0;
for(i=0;i<llsq_neq;i++)llsq_errors[i]=0;
for(i=0;i<NO_OF_RODS; i++)
  {
  sprintf(s,"\n%2d %5.0f %5.0f %7.4f %5.0f %5.0f %7.4f",i,
  q_teor[2*i],q1[i],q_teor[2*i]/q1[i],q_teor[2*i+1],q2[i],q_teor[2*i+1]/q2[i]);
  llsq_errors[2*i  ]=q1[i]-q_teor[2*i  ];
  llsq_errors[2*i+1]=q2[i]-q_teor[2*i+1];
  q_begin[2*i  ]=q_teor[2*i  ];
  q_begin[2*i+1]=q_teor[2*i+1];
printf("\n%d %f  %f",i,llsq_errors[2*i  ],llsq_errors[2*i+1]);
  errsum+=llsq_errors[2*i  ]*llsq_errors[2*i  ];
  errsum+=llsq_errors[2*i+1]*llsq_errors[2*i+1];
  fprintf(file,"%s",s);
  printf("%s",s);  
  }
sprintf(s,"\n RMS error: %f",sqrt(errsum/llsq_neq));
fprintf(file,"%s",s);
printf("%s",s);  
if(NITMAX < 1)goto skip_minim;
for(i=0; i<llsq_npar;i++)
  {
  par[i]+=1;  
  recover_par();
  compute_q();
  for(j=0; j<llsq_neq-llsq_npar; j++)
     {
     llsq_derivatives[i*llsq_neq+j]=q_teor[j]-q_begin[j];
     }
  for(j=llsq_neq-llsq_npar; j<llsq_neq; j++)
     {
     llsq_derivatives[i*llsq_neq+j]=0;
     }
  llsq_derivatives[i*llsq_neq+llsq_neq-llsq_npar+i]=alfa;
  par[i]-=1;  
  }
if(alfa > 0.1)alfa*=.75;  
llsq1();     
for(i=0;i<llsq_npar;i++)
  {
  par[i]+=llsq_steps[i];
  sprintf(s,"\n%d  %f  %f",i,par[i],llsq_steps[i]);     
  fprintf(file,"%s",s);
  printf("%s",s);  
  }
recover_par();
compute_q();
if(nit <= NITMAX)goto minimize;
skip_minim:;
sprintf(sh,
"no    material   alloy  diam  le     QTfree   QTbox   rat  Qfree     Qexp");
sprintf(s,"\n\n%s",sh);  
fprintf(file,"%s",s);
printf("%s",s);  
for(i=0;i<NO_OF_RODS; i++)
  {
  t1=qrod_i(i);  
  t2=0;
  sprintf(s,"\n%2d %s %5.2f %6.2f  %5.0f   %5.0f  %5.3f   %5.0f   %5.0f",
       i,type_names[type[i]],0.5*(odmin[i]+odmax[i]),len[i],t1,
       q_teor[2*i],t1/q_teor[2*i],q1[i]*t1/q_teor[2*i],q1[i]);
  fprintf(file,"%s",s);
  printf("%s",s);  
  }
sprintf(s,"\n RMS error: %f",sqrt(errsum/llsq_neq));
fprintf(file,"%s",s);
printf("%s",s);  

fclose(file);
return 0;
}