#include <iostream> 
#include <iomanip> 
#include <cmath> 

void TrapParamAdjustment(const double (&in)[11], double (&out)[11])
{
  // get input
  double dy1 = in[3], dx1 = in[4], dx2 = in[5], alp1 = in[6];
  double dy2 = in[7], dx3 = in[8], dx4 = in[9], alp2 = in[10];

  // adjust alpha
  //alp1 = alp2 = (alp1 + alp2)/2.;
  alp1 = alp2 = std::atan((std::tan(alp1) + std::tan(alp2))/2.);

  // adjust dx
  double k1 = (dx1 - dx2)/(dy1 + dy1);
  double k2 = (dx3 - dx4)/(dy2 + dy2) ;
  double k  = (k1 + k2)/2.;
  double m1 = (dx1 + dx2)/2.;
  double m2 = (dx3 + dx4)/2.;
  dx1 = m1 + k*dy1; dx2 = m1 - k*dy1; 
  dx3 = m2 + k*dy2; dx4 = m2 - k*dy2; 

  // set output
  out[0] = in[0]; out[1] = in[1]; out[2] = in[2];
  out[3] = in[3]; out[4] = dx1; out[5] = dx2; out[6]  = alp1;
  out[7] = in[7]; out[8] = dx3; out[9] = dx4; out[10] = alp2;
} 

//  Example for EHAWR1
//
int main()
{
  const double deg = M_PI/180.;

  double pDz    = 3044;
  double pTheta =    1.03516586152568*deg;
  double pPhi   =  -90.4997606158588*deg;

  double pDy1    = 295;
  double pDx1    = 22.1097030639648;
  double pDx2    = 27.2561702728271;
  double pAlp1   = 0.499765795343479*deg;

  double pDy2    = 185;
  double pDx3    = 22.1097030639648;
  double pDx4    = 25.3371486663818;
  double pAlp2   = 0.499766062147744*deg;

  double in [11] = { pDz,pTheta,pPhi, pDy1,pDx1,pDx2,pAlp1, pDy2,pDx3,pDx4,pAlp2 };

  double fTthetaCphi = std::tan(pTheta)*std::cos(pPhi); 
  double fTalpha1 = std::tan(pAlp1);
  double fTalpha2 = std::tan(pAlp2);

  // compute x coordinates of corners before adjustment
  double xbefore[8];
  xbefore[0] = -pDz*fTthetaCphi - pDy1*fTalpha1 - pDx1;
  xbefore[1] = -pDz*fTthetaCphi - pDy1*fTalpha1 + pDx1;
  xbefore[2] = -pDz*fTthetaCphi + pDy1*fTalpha1 - pDx2;
  xbefore[3] = -pDz*fTthetaCphi + pDy1*fTalpha1 + pDx2;

  xbefore[4] =  pDz*fTthetaCphi - pDy2*fTalpha2 - pDx3;
  xbefore[5] =  pDz*fTthetaCphi - pDy2*fTalpha2 + pDx3;
  xbefore[6] =  pDz*fTthetaCphi + pDy2*fTalpha2 - pDx4;
  xbefore[7] =  pDz*fTthetaCphi + pDy2*fTalpha2 + pDx4;

  // adjust parameters
  double out[11];
  TrapParamAdjustment(in, out);

  // compute x coordinates of corners after adjustment
  pDx1 =  out[4];
  pDx2 =  out[5];
  pAlp1 = out[6];
  pDx3 =  out[8];
  pDx4 =  out[9];
  pAlp2 = out[10];
  fTalpha1 = std::tan(pAlp1);
  fTalpha2 = std::tan(pAlp2);

  double xafter[8];
  xafter[0] = -pDz*fTthetaCphi - pDy1*fTalpha1 - pDx1;
  xafter[1] = -pDz*fTthetaCphi - pDy1*fTalpha1 + pDx1;
  xafter[2] = -pDz*fTthetaCphi + pDy1*fTalpha1 - pDx2;
  xafter[3] = -pDz*fTthetaCphi + pDy1*fTalpha1 + pDx2;

  xafter[4] =  pDz*fTthetaCphi - pDy2*fTalpha2 - pDx3;
  xafter[5] =  pDz*fTthetaCphi - pDy2*fTalpha2 + pDx3;
  xafter[6] =  pDz*fTthetaCphi + pDy2*fTalpha2 - pDx4;
  xafter[7] =  pDz*fTthetaCphi + pDy2*fTalpha2 + pDx4;

  // find max discrepance in x
  double delta = 0;
  for (int i=0; i<8; i++) {
    double d = std::abs(xbefore[i] - xafter[i]);
    if (d > delta) delta = d;
  }

  // print out
  std::cout << std::setprecision(16);
  std::cout << "\n===   Adjustment of parameters for EHAWR1   ===" << std::endl;
  std::cout << "\nInput :" << std::endl;
  std::cout << "dz=" <<in[0]<<"   theta="<<in[1]/deg<<"   phi="<<in[2]/deg<< std::endl;
  std::cout << "dy1="<<in[3]<<"   dx1="  <<in[4]<<"   dx2="<<in[5]<<"   alp1="<< in[6]/deg << std::endl;
  std::cout << "dy2="<<in[7]<<"   dx3="  <<in[8]<<"   dx4="<<in[9]<<"   alp2="<< in[10]/deg<< std::endl;

  std::cout << "\nOutput :" << std::endl;
  std::cout << "dz=" <<out[0]<<"   theta="<<out[1]/deg<<"   phi="<<out[2]/deg<< std::endl;
  std::cout << "dy1="<<out[3]<<"   dx1="  <<out[4]<<"   dx2="<<out[5]<<"   alp1="<< out[6]/deg << std::endl;
  std::cout << "dy2="<<out[7]<<"   dx3="  <<out[8]<<"   dx4="<<out[9]<<"   alp2="<< out[10]/deg<< std::endl;

  std::cout << "\nMax shift in x : " << delta << std::endl << std::endl; 
  return 0;
}