#ifndef MpsLaserCoordinates_HH
#define MpsLaserCoordinates_HH

#include <iostream>
#include <fstream>
#include <string>
#include <cmath>

#include "MpsSensor1Hit.hh"


class MpsLaserCoordinates {
public:
  
  MpsLaserCoordinates() {
    _originX=0.0;
    _originY=0.0;
    ls(0.0,-1.0,1.0,0.0);

    readFile();
  }

  virtual ~MpsLaserCoordinates() {
  }

  bool readFile(const std::string &s="MpsLaserCoordinates.txt") {
    std::ifstream fin(s.c_str());
    if(!fin) return false;

    fin >> _originX;
    if(!fin) return false;

    fin >> _originY;
    if(!fin) return false;

    double xx,xy,yx,yy;

    fin >> xx;
    if(!fin) return false;

    fin >> xy;
    if(!fin) return false;

    fin >> yx;
    if(!fin) return false;

    fin >> yy;
    if(!fin) return false;

    ls(xx,xy,yx,yy);

    return true;
  }
  
  bool writeFile(const std::string &s="MpsLaserCoordinates.txt") {
    std::ofstream fout(s.c_str());
    if(!fout) return false;

    fout << _originX << std::endl;
    fout << _originY << std::endl;

    fout << _lsXX << std::endl;
    fout << _lsXY << std::endl;
    fout << _lsYX << std::endl;
    fout << _lsYY << std::endl;

    return fout;
  }

  unsigned pixelX(double localX) const {
    return MpsSensor1Hit::pixelX(localX);
  }

  unsigned pixelY(double localY) const {
    return MpsSensor1Hit::pixelY(localY);
  }

  double localX(unsigned pixelX) const {
    return MpsSensor1Hit::localX(pixelX);
  }

  double localY(unsigned pixelY) const {
    return MpsSensor1Hit::localY(pixelY);
  }

  double localX(int stageX, int stageY) const {
    //return 0.001*( _c*(_scaleX*stageX-_originX)+_s*(_scaleY*stageY-_originY));
    return 0.001*(_slXX*(stageX-_originX)+_slXY*(stageY-_originY));
  }

  double localY(int stageX, int stageY) const {
    //return 0.001*(-_s*(_scaleX*stageX-_originX)+_c*(_scaleY*stageY-_originY));
    return 0.001*(_slYX*(stageX-_originX)+_slYY*(stageY-_originY));
  }
  
  double dStageX(double localX, double localY) const {
    return 1000.0*(_lsXX*localX+_lsXY*localY)+_originX;
  }
  
  double dStageY(double localX, double localY) const {
    return 1000.0*(_lsYX*localX+_lsYY*localY)+_originY;
  }
  
  int stageX(double localX, double localY) const {
    double s(dStageX(localX,localY));
    if(s<0.0) s-=0.5;
    else      s+=0.5;
    return (int)s;
  }

  int stageY(double localX, double localY) const {
    double s(dStageY(localX,localY));
    if(s<0.0) s-=0.5;
    else      s+=0.5;
    return (int)s;
  }

  double dStageX(unsigned pixelX, unsigned pixelY) const {
    return dStageX(localX(pixelX),localY(pixelY));
  }

  double dStageY(unsigned pixelX, unsigned pixelY) const {
    return dStageY(localX(pixelX),localY(pixelY));
  }

  int stageX(unsigned pixelX, unsigned pixelY) const {
    return stageX(localX(pixelX),localY(pixelY));
  }
  
  int stageY(unsigned pixelX, unsigned pixelY) const {
    return stageY(localX(pixelX),localY(pixelY));
  }
  
  unsigned pixelX(int stageX, int stageY) const {
    return pixelX(localX(stageX,stageY));
  }

  unsigned pixelY(int stageX, int stageY) const {
    return pixelY(localY(stageX,stageY));
  }


  double x() const {
    return _originX;
  }

  void x(double x) {
    _originX=x;
  }

  double y() const {
    return _originY;
  }

  void y(double y) {
    _originY=y;
  }

  /*
  double angle() const {
    return atan2(_c,_s);
  }

  void angle(double a) {
    _c=sin(a);
    _s=cos(a);
  }

  double scaleX() const {
    return _scaleX;
  }

  void scaleX(double x) {
    _scaleX=x;
  }

  double scaleY() const {
    return _scaleY;
  }

  void scaleY(double y) {
    _scaleY=y;
  }
  */

  void ls(double xx, double xy, double yx, double yy) {
    _lsXX=xx;
    _lsXY=xy;
    _lsYX=yx;
    _lsYY=yy;
    
    double det(_lsXX*_lsYY-_lsXY*_lsYX);

    _slXX= _lsYY/det;
    _slYY= _lsXX/det;
    _slXY=-_lsXY/det;
    _slYX=-_lsYX/det;
  }

  double ls(unsigned i) const {
    assert(i<4);
    if(i==0) return _lsXX;
    if(i==1) return _lsXY;
    if(i==2) return _lsYX;
    return _lsYY;
  }


  std::ostream& print(std::ostream &o=std::cout, std::string s="") {
    o << s << "MpsLaserCoordinates::print()" << std::endl;

    o << s << " Value of stage coordinates at local origin = "
      << _originX << ", " << _originY << std::endl;
    o << s << " Value of local coordinates at stage origin = "
      << localX(0,0) << ", " << localY(0,0) << std::endl;

    o << s << " Transformation matrix  = "
      << std::setw(10) << _lsXX << " "
      << std::setw(10) << _lsXY << std::endl;
    o <<  s << "                          "
      << std::setw(10) << _lsYX << " "
      << std::setw(10) << _lsYY << std::endl;


    o << s << " Transformation inverse = "
      << std::setw(10) << _slXX << " "
      << std::setw(10) << _slXY << std::endl;
    o << s << "                        = "
      << std::setw(10) << _slYX << " "
      << std::setw(10) << _slYY << std::endl;

    /*
    o << s << " Stage coordinate scale = "
      << _scaleX << ", " << _scaleY << std::endl;

    o << s << " Rotation angle cosine, sine = "
      << _c << ", " << _s << std::endl;
    */

    return o;
  }


private:
  double _originX;
  double _originY;
  double _lsXX,_lsXY,_lsYX,_lsYY;
  double _slXX,_slXY,_slYX,_slYY;
};


#endif
