/******************************include the head files********************************************/
#include<iostream>
#include<math.h>
#include<vector>
#include<cmath>
#include"/home/gaoshengjun/opencv-2.4.13/include/opencv/cv.h"
#include "/home/gaoshengjun/opencv-2.4.13/include/opencv/highgui.h"
#include <stdio.h>
#include <unistd.h>
#include <dirent.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <string.h>
/***********************************************************************************************/






/*******************************Macro definition************************************************/

//define PI , coordinate type, matrix type 
 
#define PI 3.1415926
#define POINT   std::pair<int,int>
#define MATRIX  std::vector<std::vector<float> >
#define VECT    std::vector<float> 
/***********************************************************************************************/


using namespace cv;//using opencv 


/*****************************calculate the angle***********************************************/
float cal_angle(POINT &pt1,POINT &pt2)
{
    double angle;
    
    if ((pt2.first-pt1.first)==0)
    {
       angle=PI/2;
    }
    else
    {
       angle=std::abs(atan((pt2.second-pt1.second)/(pt2.first-pt1.first)));
       
       
    }
    
    return angle;
};
/***********************************************************************************************/




/****************************calculate the distance********************************************/
float cal_distance(POINT &pt1,POINT &pt2,const int offset)
{
   float distance;
   distance=sqrt(pow((pt2.second-pt1.second),2)+pow((pt2.first-pt1.first),2))+offset;
   //std::cout<<"1111:"<<distance<<std::endl;
   return distance;
}
/***********************************************************************************************/



/********produce xmap(size:regionH X 1) according linspace(-regionW/2,regionW/2,regionW)********/
MATRIX produce_xmap_Matrix(float regionW,float regionH)
{
   MATRIX array(regionH);
   int i,j;
   for(i=0;i<array.size();i++)
       array[i].resize(regionW);

   for(i=0;i<array.size();i++)
   { 
      for(j=0;j<array[0].size();j++)
      {
        array[i][j]=-regionW/2+j*(regionW)/(regionW-1);
        
      }
   }
    return array;
}
/***********************************************************************************************/



/******produce ymap(size:1 X regionW) according linspace(-regionH/2,regionH/2,regionH)**********/
MATRIX produce_ymap_Matrix(float regionW,float regionH)
{
   MATRIX array(regionH);
   int i,j;
   for(i=0;i<array.size();i++)
       array[i].resize(regionW);

   for(i=0;i<array[0].size();i++)
   { 
      for(j=0;j<array.size();j++)
      {
        array[j][i]=-regionH/2+j*(regionH)/(regionH-1);
        //std::cout<<j<<"  "<<i<<"  ";
      }
        //std::cout<<std::endl;
   }
    return array;
}
/***********************************************************************************************/




/******************************make SCALED MATRIX***********************************************/
MATRIX matrix_multi_const_number(MATRIX array,float scale)
{
    MATRIX result(array.size());
    int i,j;
    for(i=0;i<array.size();i++)
    result[i].resize(array[0].size());
    for(i=0;i<array.size();i++)
    { 
      for(j=0;j<array[0].size();j++)
      {
        result[i][j]=array[i][j]*scale;
        
      }
        
    }
    return result;
}
/***********************************************************************************************/




/*****************************MATRIX-I + MATRIX-II**********************************************/
MATRIX matrix_add_matrix(MATRIX array0,MATRIX array1)
{
    MATRIX result(array0.size());
    int i,j;
    for(i=0;i<array0.size();i++)
    result[i].resize(array0[0].size());
    for(i=0;i<array0.size();i++)
    {
       for(j=0;j<array0[0].size();j++)
       {  
         result[i][j]=array0[i][j]+array1[i][j];
       }
    }
    return result;
}
/***********************************************************************************************/




/************************MATRIX-I + const-number***********************************************/
MATRIX matrix_ADD_const_number(MATRIX array0,float num)
{
    MATRIX result(array0.size());
    int i,j;
    for(i=0;i<array0.size();i++)
    result[i].resize(array0[0].size());
    for(i=0;i<array0.size();i++)
    {
       for(j=0;j<array0[0].size();j++)
       {  
         result[i][j]=array0[i][j]+num;
       }
    }
    return result;
}
/***********************************************************************************************/




/******************roudn each value in the array***********************************************/
VECT round(VECT array,char kind)
{
    VECT result(array.size());
    int n=0;
    for(int i=0;i<array.size();i++)
    {
         if (kind=='f')
         {
           result[i]=floor(array[i]);
         }
         else
         {
          result[i]=ceil(array[i]);
         }
         
    }
    
    return result;
}
/***********************************************************************************************/




/*****************************VECTER-I-VECTOR-II***********************************************/
VECT vector_sub_vector(VECT array0,VECT array1)
{
    VECT result(array0.size());
    int n=0;
    for(int i=0;i<array0.size();i++)
    {
        result[i]=array0[i]-array1[i];
         
    }
    
    return result;
}
/***********************************************************************************************/




/****************************MATRIX to VECTOR**************************************************/
VECT change_format(MATRIX array)
{
    VECT result(array.size()*array[0].size());
    int n=0;
    for(int i=0;i<array[0].size();i++)
    {
      for(int j=0;j<array.size();j++)
      {  
         result[n]=array[j][i];
         n++;
      }
    }
    
    return result;
}
/***********************************************************************************************/




/************************VECTOR-I.*VECTOR-II****************************************************/
VECT  interpolation_by_dot_multi(VECT deltacxx,VECT deltaxfx,VECT deltacyy,VECT deltayfy,VECT imfxfy, 
                                 VECT imfxcy,VECT imcxfy, VECT imcxcy)
{
       VECT roi(deltacxx.size());
       
       for(int i=0;i<deltacxx.size();i++)
       {
         
         roi[i]=(imfxfy[i]*deltacxx[i]*deltacyy[i]+ imfxcy[i]* deltacxx[i]* deltayfy[i]+
                 imcxfy[i]* deltaxfx[i]* deltacyy[i] + imcxcy[i]* deltaxfx[i]* deltayfy[i]);
       }
      return roi;
    
}
/***********************************************************************************************/




/******************************produce ZERO MATRIX**********************************************/
MATRIX zeros(int length,float num)
{
    MATRIX result(length);
    int i,j;
    for(i=0;i<length;i++)
    result[i].resize(num);
    for(i=0;i<length;i++)
    {
       for(j=0;j<num;j++)
       {  
         result[i][j]=0;
       }
    }
    return result;
}
/***********************************************************************************************/




/*****************************produce ONES MATRIX**********************************************/
MATRIX ones(int length,float num)
{
    MATRIX result(length);
    int i,j;
    for(i=0;i<length;i++)
    result[i].resize(num);
    for(i=0;i<length;i++)
    {
       for(j=0;j<num;j++)
       {  
         result[i][j]=1;
       }
    }
    return result;
}

/***********************************************************************************************/




/********************make sure each position of the two points*********************************/

POINT * compare_2_points_left_right(POINT &pt1,POINT &pt2)
{
    int x_left,x_right,y_down,y_up;
    POINT new_pt1,new_pt2;
    static POINT arr[2];
   /*
    pt1.first=std::abs(pt1.first);
    pt2.first=std::abs(pt2.first);
    pt1.second=-std::abs(pt1.second);
    pt2.second=-std::abs(pt2.second);
   */
    if (pt2.first<pt1.first)
    {
       x_left=pt2.first;
       x_right=pt1.first;
    }

    else
    {
       x_left=pt1.first;
       x_right=pt2.first;
    }
    //std::cout<<"x_left:  "<<x_left<<"x_right:"<<x_right<<std::endl;
    if(pt2.second<pt1.second)
    {
       y_down=pt2.second;
       y_up=pt1.second;
     }
    else
    {
       y_down=pt1.second;
       y_up=pt2.second;
    }
    //std::cout<<"y_down:  "<<y_down<<"y_up:"<<y_up<<std::endl;
    new_pt1.first=x_left;
    new_pt1.second=y_up;
    new_pt2.first=x_right;
    new_pt2.second=y_down;
    arr[0]=new_pt1;
    arr[1]=new_pt2;
    return arr;

    
};
/***********************************************************************************************/




/**************************calculate the mid of two points*************************************/
POINT cal_two_points_mid(POINT &pt1,POINT &pt2)
{
   POINT midPoint;
   midPoint.first=(pt1.first+pt2.first)/2;
   midPoint.second=(pt1.second+pt2.second)/2;
   return midPoint;
}
/***********************************************************************************************/




/**********************************MATRIX-I + MATRIX-II*****************************************/
MATRIX VECT_TO_MATRIX(VECT input,int h,int w)
{
    MATRIX result(h);
    int i,j;
    int n=0;
    for(i=0;i<h;i++)
    result[i].resize(w);
    for(i=0;i<w;i++)
    {
       for(j=0;j<h;j++)
       {  
         result[j][i]=input[n++];
       }
    }
    return result; 
}
/***********************************************************************************************/




/*********************************if anyone is non-zero return true*****************************/
bool any_compare(VECT vec)
{
    for(int i=0;i<vec.size();i++)
    {
        if (vec[i]!=0)
         {  
            return true;
         }
        else
        {
            return false;
        }
    }
}
/***********************************************************************************************/




/********************VECT compare with const number*********************************************/
VECT vec_compare_const_number(VECT vec,int num,char type)
{
    VECT result(vec.size());
    for(int i=0;i<vec.size();i++)
    {
       if (type=='s')
       {
           if (vec[i]<num)
           {
              result[i]=1;
           }
           else result[i]=0;
       }
       else
       {
           if (vec[i]>num)
           {
              result[i]=1;
           }
           else result[i]=0;               
       }
    }
     return result;
}
/*******************************extract_Image_pixel in a special way****************************/ 
VECT extract_Image_pixel(Mat img,VECT map1,VECT map2,VECT map3,int channel_choose)
{
   VECT result(map1.size());
   for(int i=0;i<map1.size();i++)
   {
       int k0,k1,k2;
       k0=map1[i];
       k1=map2[i];
       k2=map3[i];
       Vec3b pix = img.at<Vec3b>(k0-1,k1-1);
       result[i]=pix[channel_choose];
   }
    return result;
} 
/***********************************************************************************************/




/***********************************extract_ROI*************************************************/
MATRIX extract_ROI( Mat img, MATRIX xmapInImage, MATRIX ymapInImage)
{
   MATRIX roi_new;
   int h,w,length;
   int channel_choose=2;
   VECT xmapInImage0,ymapInImage0,fxmap,fymap,cxmap
     ,cymap,deltacxx,deltaxfx,deltacyy,deltayfy,roi,zmap,imfxfy,imfxcy,imcxfy,imcxcy;


   h=xmapInImage.size();
   w=xmapInImage[0].size();
   length=h*w;

   xmapInImage0=change_format(xmapInImage);
   ymapInImage0=change_format(ymapInImage);

   fxmap=round(xmapInImage0,'f');
   fymap=round(ymapInImage0,'f');
   cxmap=round(xmapInImage0,'c');
   cymap=round(ymapInImage0,'c');

   deltacxx=vector_sub_vector(cxmap,xmapInImage0);
   deltaxfx=vector_sub_vector(xmapInImage0,fxmap);
   deltacyy=vector_sub_vector(cymap,ymapInImage0);
   deltayfy=vector_sub_vector(ymapInImage0,fymap);

   roi=change_format(zeros(length,1));
   zmap=change_format(ones(length,1));
   imfxfy=change_format(zeros(length,1));
   imfxcy=change_format(zeros(length,1));
   imcxfy=change_format(zeros(length,1));
   imcxcy=change_format(zeros(length,1));

   imfxfy=extract_Image_pixel(img,fymap,fxmap,zmap,channel_choose);
   imfxcy=extract_Image_pixel(img,cymap,fxmap,zmap,channel_choose);
   imcxfy=extract_Image_pixel(img,fymap,cxmap,zmap,channel_choose);
   imcxcy=extract_Image_pixel(img,cymap,cxmap,zmap,channel_choose);

   roi=interpolation_by_dot_multi(deltacxx,deltaxfx,deltacyy,deltayfy,imfxfy,  imfxcy,imcxfy, imcxcy);   

   roi_new=VECT_TO_MATRIX(roi,h,w);

    return roi_new;
}
/***********************************************************************************************/




/***********************************extractEntranceLineRegion***********************************/
MATRIX extractEntranceLineRegion(POINT &pt1,POINT &pt2,Mat img)
{
   const float regionW=96;
   const float regionH=24;
   const int offset=24;
   int ImageHeight=img.rows;
   int ImageWidth=img.cols;
   double angle;
   bool outofboundary=false;
   MATRIX xmap,ymap,xmapScaled,ymapScaled,xmapInImage,ymapInImage,roi;
   POINT new_pt1,new_pt2,midPoint;
   POINT * arr;
   float widthOfTheRealRegion ;
   xmap=produce_xmap_Matrix(regionW,regionH);
   ymap=produce_ymap_Matrix(regionW,regionH);

   arr=compare_2_points_left_right(pt1,pt2);
   new_pt1=arr[0];
   new_pt2=arr[1];

   angle=cal_angle(new_pt1,new_pt2);

   widthOfTheRealRegion=cal_distance(new_pt1,new_pt2,offset);

   midPoint=cal_two_points_mid(new_pt1,new_pt2);

   xmapScaled=matrix_multi_const_number(xmap,widthOfTheRealRegion/regionW);
   ymapScaled=matrix_multi_const_number(ymap,regionH/regionH);
   
   xmapInImage=matrix_ADD_const_number(matrix_add_matrix(matrix_multi_const_number(xmapScaled,cos(angle)),matrix_multi_const_number(ymapScaled,sin(angle))),midPoint.first);
   ymapInImage=matrix_ADD_const_number(matrix_add_matrix(matrix_multi_const_number(xmapScaled,-sin(angle)),matrix_multi_const_number(ymapScaled,cos(angle))),midPoint.second);
       /*make sure the pixel is bigger than 1 and the size of the point is under the  readed image */
   if (any_compare(vec_compare_const_number(change_format(xmapInImage),1,'s'))or (any_compare(vec_compare_const_number(change_format(xmapInImage),ImageWidth,'b'))) or
       (any_compare(vec_compare_const_number(change_format(ymapInImage),1,'s')))or(any_compare(vec_compare_const_number(change_format(ymapInImage),ImageHeight,'b'))))
   {
      outofboundary = true;
   }
   if (!outofboundary)
   {
        roi = extract_ROI(img,xmapInImage,ymapInImage);    
   } 
   else
       roi=zeros(regionH,regionW);  
   
   return roi;
}
/***********************************************************************************************/





/*******************************FUNCTION MAIN()*************************************************/
int main()
{
   MATRIX roi;
   POINT point1(171,213);
   POINT point2(171,145);
   Mat img=imread("./000338.jpg", CV_LOAD_IMAGE_UNCHANGED);

   roi=extractEntranceLineRegion(point1,point2,img);
   for(int i=0;i<roi.size();i++)
   {
     for(int j=0;j<roi[0].size();j++)
     {
        std::cout<<roi[i][j]<<" ";
     }
        std::cout<<std::endl<<std::endl;
   }
   return 0;
}

/*******************************END***********************************************************/