#include <math.h>
#include <iostream.h>
#include "HalconCpp.h"
#include <String.h>

HImage a; 
HImage b; 
HImage c; 
HImage aa;
HImage bb;
HImage image[3];


double ps[3];
double qs[3];
double pq[3];
int th=10;
int E[3];
int EMax[3];
double **pqMap = NULL;
int **imagePix=NULL;
double *tt=NULL;
double *albedo=NULL;
double tempPQ[3];
int HI;
int WI;
double p,q,E0;

bool solvePQ(int i1,int i2,int i3){
	  double r1,r2,r3,a,b,c,d,e,f;
	  if(E[i1]<=th) return false;
	  r1=(pq[i2]/pq[i1])*(1.0*E[i2]/E[i1])*(1.0*EMax[i1]/EMax[i2]);
	  r2=(pq[i3]/pq[i1])*(1.0*E[i3]/E[i1])*(1.0*EMax[i1]/EMax[i3]);
	  a=ps[i2]-r1*ps[i1];
	  b=qs[i2]-r1*qs[i1];
	  c=1-r1;
	  d=ps[i3]-r2*ps[i1];
	  e=qs[i3]-r2*qs[i1];
	  f=1-r2;
	  r3=a*e-b*d;
	  if(r3==0) return false;
	  tempPQ[0]=(b*f-e*c)/r3;               //p
	  tempPQ[1]=(d*c-a*f)/r3;               //q
	  double ns=(tempPQ[0]*ps[i1]+tempPQ[1]*qs[i1]+1);
	  ns/=sqrt(tempPQ[0]*tempPQ[0]+tempPQ[1]*tempPQ[1]+1);
	  ns/=pq[i1];
	  ns*=EMax[i1];
	  tempPQ[2]=E[i1]/ns;           //albedo
	  if(tempPQ[2]>1) tempPQ[2]=1;
	  if(tempPQ[2]<0) tempPQ[2]=0;
	 
	  return true;
}


void computeN(){
	HI=a.Height();
	WI=a.Width();

	pqMap = (double **) new double*[HI*WI];
	imagePix = (int **) new int*[HI*WI];
	tt=(double *) new double[HI*WI];
	for (int i=0; i<HI*WI; i++) {
		pqMap[i] = (double *) new double[3];
		imagePix[i]=(int *) new int[3];
	}	
	
	albedo= (double*) new double[HI*WI];
	image[0]=a;
	image[1]=b;
	image[2]=c;

	for(int i=0; i<3;i++){
		pq[i]=ps[i]*ps[i]+qs[i]*qs[i]+1;
		pq[i]=(float)sqrt(pq[i]);
		int Emax=-1;
		for(int y=0;y<HI;y++){
			for(int x=0; x<WI; x++){
				imagePix[x*WI+y][i]=image[i].GetPixVal(x,y);
				if(((int)image[i].GetPixVal(x,y))>Emax){
				    Emax=image[i].GetPixVal(x,y);
				}
			}
		}
		EMax[i]=Emax;
		//cout <<"maxGreyValue"<<Emax<< endl;    =>255
	}//end for(int i=0; i<3;i++){...

	int count=0;
	double value;

	//calculate p,q for every point
	for(int i=0;i<WI*HI;i++){
	  for(int j=0;j<3;j++){ E[j]=imagePix[i][j];}
	  pqMap[i][0]=0;
	  pqMap[i][1]=0;
	  albedo[i]=0;

	  if(E[0]>th ||E[1]>th ||E[2]>th){
	    count=0;	
	    if(solvePQ(0,1,2)){
                 pqMap[i][0]+=tempPQ[0];
	     	 pqMap[i][1]+=tempPQ[1];
	     	 albedo[i]=tempPQ[2];
	     	 count++;
	     }
	     if(count==0&&solvePQ(1,0,2)){
		 pqMap[i][0]+=tempPQ[0];
	     	 pqMap[i][1]+=tempPQ[1];
	     	 albedo[i]=tempPQ[2];
	     	 count++;
	     }
	     if(count==0&&solvePQ(2,1,0)){
		 pqMap[i][0]+=tempPQ[0];
	     	 pqMap[i][1]+=tempPQ[1];
	     	 albedo[i]=tempPQ[2];
	     	 count++;
	     }
	    
	     value=pqMap[i][0]*pqMap[i][0]+pqMap[i][1]*pqMap[i][1]+1;
	     value=sqrt(value);
	     pqMap[i][0]=pqMap[i][0]/value;//normalize

	     pqMap[i][1]=pqMap[i][1]/value;//normalize

	     pqMap[i][2]=1/value;     //normalize
		 tt[i]=1/value;				//normalize		

	  }//end if if(E[0]>th && E[1]>th && E[2]>th)
	  
	  
	}//end for(int i=0;i<width*height;i++){...

	//cout <<"it is done"<< endl;
}
void setPQMap(){
       int interval=5;
       int height=HI;
       int width=WI;
       double x,y;
       int index;
  	 
  	 HWindow w(0,0,255,255);
 	 sleep(1);

 	 for (int j=interval/2; j<height-interval/2; j=j+interval){
        for (int i=interval/2; i<width-interval/2; i=i+interval){
			index=i+j*width;
                        x= pqMap[index][1]*interval*3/4;
                        y= pqMap[index][0]*interval*3/4;
                        w.DispLine(i,j,x+i,y+j);
			x=0; y=0;
		}
     }
  
  	 //w.DumpWindow("jpeg","needle");
  	 //w.CloseWindow(); 
	 w.Click();
      
} //end setPQMap()

void reconstruct(float p, float q, float E0){
  
  double value=sqrt(p*p+q*q+1);
  p/=value;q/=value;
  double ns;
  int ind;

  for(int j=0;j<HI;j++){
    for(int i=1; i<WI;i++){
      ind=i+j*WI;	 
      ns=pqMap[ind][0]*p+pqMap[ind][1]*q+(int)tt[ind]/value; 
      
      ns*=E0*albedo[ind];
      int pix=(int)ns;
 
      bb.SetPixVal(j,i,pix);
    }
  }
      
}//end reconstruct

main(int argc, char* argv[]){
	
	try {
		a = HImage((String)argv[1]+"_1"); 
		b = HImage((String)argv[1]+"_2"); 
		c = HImage((String)argv[1]+"_3"); 
		ps[0]=atof(argv[2]);
		ps[1]=atof(argv[3]);
		ps[2]=atof(argv[4]);
		qs[0]=atof(argv[5]);
		qs[1]=atof(argv[6]);
		qs[2]=atof(argv[7]);
		aa = a;
		bb = b;				
	} catch (HException &except) {		
		return (-1);	
	}

  HWindow w1(0,0,255,255);   
  
  computeN();
  setPQMap();
    
  reconstruct(0.0,0.5,255);
  cout << argv[1] << endl;
  bb.Display(w1);
  w1.Click();
 
  reconstruct(0.5,1.0,255);
  bb.Display(w1);
  w1.Click();
  
  reconstruct(0.5,-1.0,255);
  bb.Display(w1);
  w1.Click();
 
return(0);
}