#include <cmath>
#include <cassert>
#include <iostream>
#include <vector>

#include "TMatrixD.h"
#include "TVectorD.h"

#include "MpsLaserCoordinates.hh"

class Point {
public:
  Point() {}
  Point(unsigned px, unsigned py, double sx, double sy) :
    _pixelX(px), _pixelY(py), _stageX(sx), _stageY(sy) {}

  unsigned _pixelX;
  unsigned _pixelY;
  double _stageX;
  double _stageY;
};


int main() {
  std::vector<Point> v;
  /*
  v.push_back(Point( 60, 29, -715.4, -395.1));
  v.push_back(Point( 30,100, 2870.0,-2163.8));
  v.push_back(Point( 61,150, 5373.0, -384.8));
  v.push_back(Point( 60, 29, -717.3, -394.0));
  */
  /*
  v.push_back(Point( 30, 30, -674.1,-2138.8)); // 470453
  v.push_back(Point( 32, 93, 2521.9,-2061.0)); // 470458
  v.push_back(Point( 51, 93, 2528.7, -873.8)); // 470459
  v.push_back(Point( 51, 74, 1535.8, -858.0)); // 470460
  v.push_back(Point( 31, 74, 1518.2,-2104.1)); // 470461
  v.push_back(Point( 64, 18,-1257.2, -188.8)); // 470464
  v.push_back(Point( 64,148, 5281.6, -230.8)); // 470465
  v.push_back(Point( 19,148, 5253.4,-2718.6)); // 470466
  v.push_back(Point( 31, 93, 2523.7,-2109.5)); // 470467
  v.push_back(Point( 50, 93, 2531.6, -911.1)); // 470468
  v.push_back(Point( 32, 74, 1529.2,-2054.5)); // 470470
  */
  /*
  v.push_back(Point( 56, 39,  806.0, -422.0)); // 470557
  v.push_back(Point( 61,127, 5254.0, -192.0)); // 470559
  v.push_back(Point( 15,127, 5239.0,-2736.0)); // 470560
  */
  /*
  v.push_back(Point( 23,148, 6301.8,-2549.7)); // 470615
  //v.push_back(Point( 37, 92, 3498.1,-1836.9)); // 470616
  v.push_back(Point( 61,127, 5256.2, -399.9)); // 470617
  v.push_back(Point( 56, 39,  796.1, -630.8)); // 470618
  */
  v.push_back(Point( 14,129, 5340.7,-2997.3)); // 470620
  v.push_back(Point( 61,127, 5247.2, -396.3)); // 470622
  v.push_back(Point( 23,148, 6296.9,-2549.9)); // 470624
  v.push_back(Point( 61, 28,  280.0, -360.0)); // 470630

  MpsLaserCoordinates c;

  c.print() << std::endl;

  for(unsigned i(0);i<v.size();i++) {
    std::cout << "Point " << i << ", pixel x,y = "
	      << std::setw(3) << v[i]._pixelX
	      << std::setw(4) << v[i]._pixelY
	      << ", stage x,y measured = "
	      << v[i]._stageX << ", " << v[i]._stageY << ", aligned = "
	      << c.dStageX(v[i]._pixelX,v[i]._pixelY) << ", "
	      << c.dStageY(v[i]._pixelX,v[i]._pixelY) << ", deltas = "
	      << v[i]._stageX-c.dStageX(v[i]._pixelX,v[i]._pixelY) << ", "
	      << v[i]._stageY-c.dStageY(v[i]._pixelX,v[i]._pixelY) << std::endl;
  }
  /*  
  double initialX(c.x());
  double initialY(c.y());
  double initialXX(c.ls(0));
  double initialXY(c.ls(1));
  double initialYX(c.ls(2));
  double initialYY(c.ls(3));

  double bestX(initialX);
  double bestY(initialY);
  double bestXX(initialXX);
  double bestXY(initialXY);
  double bestYX(initialYX);
  double bestYY(initialYY);
  */
  TMatrixD ei(3,3);
  TVectorD sx(3);
  TVectorD sy(3);

  for(unsigned i(0);i<3;i++) {
    sx(i)=0.0;
    sy(i)=0.0;
    for(unsigned j(0);j<3;j++) {
      ei(i,j)=0.0;
    }
  }

  for(unsigned i(0);i<v.size();i++) {
    double lX(c.localX(v[i]._pixelX));
    double lY(c.localY(v[i]._pixelY));

    ei(0,0)+=lX*lX;
    ei(0,1)+=lX*lY;
    ei(0,2)+=lX;
    ei(1,0)+=lX*lY;
    ei(1,1)+=lY*lY;
    ei(1,2)+=lY;
    ei(2,0)+=lX;
    ei(2,1)+=lY;
    ei(2,2)++;

    sx(0)+=lX*v[i]._stageX;
    sx(1)+=lY*v[i]._stageX;
    sx(2)+=   v[i]._stageX;

    sy(0)+=lX*v[i]._stageY;
    sy(1)+=lY*v[i]._stageY;
    sy(2)+=   v[i]._stageY;
  }

  TMatrixD em(ei);
  em.Invert();

  TVectorD px(3);
  TVectorD py(3);

  px=em*sx;
  py=em*sy;

  double err(4.0);

  std::cout << "x paras = "
	    << 0.001*px(0) << " +/- " << 0.001*err*sqrt(em(0,0)) << ", " 
	    << 0.001*px(1) << " +/- " << 0.001*err*sqrt(em(1,1)) << ", " 
	    <<       px(2) << " +/- " <<       err*sqrt(em(2,2)) << std::endl;
  std::cout << "y paras = "
	    << 0.001*py(0) << " +/- " << 0.001*err*sqrt(em(0,0)) << ", " 
	    << 0.001*py(1) << " +/- " << 0.001*err*sqrt(em(1,1)) << ", " 
	    <<       py(2) << " +/- " <<       err*sqrt(em(2,2)) << std::endl;

  c.x(px(2));
  c.y(py(2));
  c.ls(0.001*px(0),0.001*px(1),0.001*py(0),0.001*py(1));

  double chiSqX(0.0);
  double chiSqY(0.0);
  for(unsigned i(0);i<v.size();i++) {
    double deltaX(v[i]._stageX-c.dStageX(v[i]._pixelX,v[i]._pixelY));
    double deltaY(v[i]._stageY-c.dStageY(v[i]._pixelX,v[i]._pixelY));
    chiSqX+=deltaX*deltaX/(err*err);
    chiSqY+=deltaY*deltaY/(err*err);
  }
	
  /*
  for(double x(initialX-10);x<(initialX+10);x++) {
    std::cout << "x = " << x << std::endl;
    c.x(x);

    for(double y(initialY-10);y<(initialY+10);y++) {
      c.y(y);

      for(double a(initialA-0.01);a<(initialA+0.01);a+=0.0001) {
	c.angle(a);

	for(double s(1.0-0.003);s<(1.0+0.003);s+=0.0001) {
	  c.scaleX(s);

	  for(double t(1.0-0.003);t<(1.0+0.003);t+=0.0001) {
	    c.scaleY(t);

	    double chiSq(0.0);
	    for(unsigned i(0);i<v.size();i++) {
	      double deltaX(v[i]._stageX-c.stageX(v[i]._pixelX,v[i]._pixelY));
	      double deltaY(v[i]._stageY-c.stageY(v[i]._pixelX,v[i]._pixelY));
	      chiSq+=deltaX*deltaX+deltaY*deltaY;
	    }
	    
	    if(chiSq<chiSqMin) {
	      chiSqMin=chiSq;
	      bestX=x;
	      bestY=y;
	      bestA=a;
	      bestS=s;
	      bestT=t;
	    }
	  }
	}
      }
    }
  }
  */


  /*
  unsigned iMin(0);

  while(iMin!=10) {
    double chiSqMin(chiSqBest);
    iMin=10;

    for(unsigned i(0);i<10;i++) {
      if(i==0) c.x(bestX-1.0);
      if(i==1) c.x(bestX+1.0);
      if(i==2) c.y(bestY-1.0);
      if(i==3) c.y(bestY+1.0);
      if(i==4) c.angle(bestA-0.00001);
      if(i==5) c.angle(bestA+0.00001);
      if(i==6) c.scaleX(bestS-0.00001);
      if(i==7) c.scaleX(bestS+0.00001);
      if(i==8) c.scaleY(bestT-0.00001);
      if(i==9) c.scaleY(bestT+0.00001);
      
      double chiSq(0.0);
      for(unsigned j(0);j<v.size();j++) {
	double deltaX(v[j]._stageX-c.stageX(v[j]._pixelX,v[j]._pixelY));
	double deltaY(v[j]._stageY-c.stageY(v[j]._pixelX,v[j]._pixelY));
	chiSq+=deltaX*deltaX+deltaY*deltaY;
      }
      
      if(chiSq<chiSqMin) {
	chiSqMin=chiSq;
	iMin=i;
      }
    }
    
    std::cout << "iMin = " << iMin << std::endl;

    if(iMin==0) bestX-=1.0;
    if(iMin==1) bestX+=1.0;
    if(iMin==2) bestY-=1.0;
    if(iMin==3) bestY+=1.0;
    if(iMin==4) bestA-=0.00001;
    if(iMin==5) bestA+=0.00001;
    if(iMin==6) bestS-=0.00001;
    if(iMin==7) bestS+=0.00001;
    if(iMin==8) bestT-=0.00001;
    if(iMin==9) bestT+=0.00001;
    chiSqBest=chiSqMin;
  }

  */

  /*
  c.x(bestX);
  c.y(bestY);
  c.ls(bestXX,bestXY,bestYX,bestYY);

  double chiSqX(1.0e10);

  for(double x(initialX-5);x<(initialX+5);x+=0.1) {
    std::cout << "x = " << x << std::endl;
    c.x(x);

    for(double xx(initialXX-0.01);xx<(initialXX+0.01);xx+=0.00001) {
      for(double xy(initialXY-0.01);xy<(initialXY+0.01);xy+=0.00001) {
	c.ls(xx,xy,bestYX,bestYY);

	double chiSq(0.0);
	for(unsigned i(0);i<v.size();i++) {
	  double deltaX(v[i]._stageX-c.dStageX(v[i]._pixelX,v[i]._pixelY));
	  chiSq+=deltaX*deltaX;
	}
	
	if(chiSq<chiSqX) {
	  chiSqX=chiSq;
	  bestX=x;
	  bestXX=xx;
	  bestXY=xy;
	}
      }
    }
  }
  */

  std::cout << std::endl << "Minimum X chi-squared = " << chiSqX
	    << ", implied X error = " << sqrt(chiSqX/(v.size()-3))
	    << std::endl << std::endl;

  /*
  c.x(bestX);
  c.y(bestY);
  c.ls(bestXX,bestXY,bestYX,bestYY);

  double chiSqY(1.0e10);

  for(double y(initialY-5);y<(initialY+5);y+=0.1) {
    std::cout << "y = " << y << std::endl;
    c.y(y);

    for(double yx(initialYX-0.01);yx<(initialYX+0.01);yx+=0.00001) {
      for(double yy(initialYY-0.01);yy<(initialYY+0.01);yy+=0.00001) {
	c.ls(bestXX,bestXY,yx,yy);

	double chiSq(0.0);
	for(unsigned i(0);i<v.size();i++) {
	  double deltaY(v[i]._stageY-c.dStageY(v[i]._pixelX,v[i]._pixelY));
	  chiSq+=deltaY*deltaY;
	}
	
	if(chiSq<chiSqY) {
	  chiSqY=chiSq;
	  bestY=y;
	  bestYX=yx;
	  bestYY=yy;
	}
      }
    }
  }
  */
  std::cout << std::endl << "Minimum Y chi-squared = " << chiSqY
	    << ", implied Y error = " << sqrt(chiSqY/(v.size()-3))
	    << std::endl << std::endl;
  /*
  c.x(bestX);
  c.y(bestY);
  c.ls(bestXX,bestXY,bestYX,bestYY);
  */
  std::cout << std::endl << "Minimum total chi-squared = " << chiSqX+chiSqY
	    << std::endl << std::endl;

  c.print() << std::endl;
  c.writeFile("mpsLaserAlignment.txt");

  for(unsigned i(0);i<v.size();i++) {
    std::cout << "Point " << i << ", pixel x,y = "
	      << std::setw(3) << v[i]._pixelX
	      << std::setw(4) << v[i]._pixelY
	      << ", stage x,y measured = "
	      << v[i]._stageX << ", " << v[i]._stageY << ", aligned = "
	      << c.dStageX(v[i]._pixelX,v[i]._pixelY) << ", "
	      << c.dStageY(v[i]._pixelX,v[i]._pixelY) << ", deltas = "
	      << v[i]._stageX-c.dStageX(v[i]._pixelX,v[i]._pixelY) << ", "
	      << v[i]._stageY-c.dStageY(v[i]._pixelX,v[i]._pixelY) << std::endl;
  }
  
  return 0;
}
