#include <arpa/inet.h>
#include <cmath>
#include <iostream>
#include "subframe.h"
#include <math.h>


#define RATIO 100
#define  PRMODF    75


SubFrameClass::SubFrameClass()
{
     mQ0 = new char[SCREEN_PIXEL*2];
	 mQ90 = new char[SCREEN_PIXEL*2];
	 mQ180 = new char[SCREEN_PIXEL*2];
	 mQ270 = new char[SCREEN_PIXEL*2];
     
	 mvQ0.resize(SCREEN_PIXEL, 0);
#ifdef  ABAX_TOGGER_DEBUG   	 
	 mvQ90.resize(SCREEN_PIXEL, 0);
	 mvQ180.resize(SCREEN_PIXEL, 0);
	 mvQ270.resize(SCREEN_PIXEL, 0);
#endif
     mfDis.resize(SCREEN_PIXEL, 0.0);

 	 mQ0b = new char[SCREEN_PIXEL*2];
	 m_nFreq = 20; 
}

SubFrameClass::~SubFrameClass()
{
    delete [] mQ0;
    delete [] mQ90;
    delete [] mQ180;
	delete [] mQ270;

    mfDis.clear();  
	delete [] mQ0b;
}

/* 原始4子帧像素转换为深度 */
u16 static convertPixel(char* subFrames[4], int offsetA, int offsetB)
{
	static int Q0, Q90, Q180, Q270;
	int q0_q180;
	float tmp;
	u16 ret;
		
	/* toggle */
	Q0 = (int)htons(*(u16*)(subFrames[0] + offsetA)) + htons(*(u16*)(subFrames[2] + offsetB));
	Q90 = (int)htons(*(u16*)(subFrames[1] + offsetA)) + htons(*(u16*)(subFrames[3] + offsetB));
	Q180 = (int)htons(*(u16*)(subFrames[2] + offsetA)) + htons(*(u16*)(subFrames[0] + offsetB));
	Q270 = (int)htons(*(u16*)(subFrames[3] + offsetA)) + htons(*(u16*)(subFrames[1] + offsetB));
	
	/* 套用正弦测距公式 */
	q0_q180 = Q0 - Q180;
	if (q0_q180 == 0) {
		return (u16)(1.57f * RATIO);
	}
	
	tmp = (float)(Q90 - Q270) / q0_q180;
	if (tmp < 0) {
		return 0;
	}
	
	ret = (u16)(RATIO * atanf(tmp));

	return ret;
}

void SubFrameClass::setFreq(int vFreq)
{
	m_nFreq = vFreq;
}


void SubFrameClass::convertDistance(float *dst)
{
	int Q0, Q90, Q180, Q270;
	int q0_q180;
	float tmp;
	float f_tval =  PRMODF/PI/m_nFreq;

	for(int i=0; i<SCREEN_PIXEL;i++)
	{
         Q0 =  *((u16 *)mQ0 + i) ;
		 Q90 =  *((u16 *)mQ90 + i) ;
		 Q180 =  *((u16 *)mQ180 + i) ;
		 Q270 =  *((u16 *)mQ270 + i) ;
        /* 套用正弦测距公式 */
    	q0_q180 = Q0 - Q180;
   
        tmp = f_tval * atan2(((float)(Q90 - Q270)), (float)q0_q180);
	//	tmp =  atan2(((float)(Q90 - Q270)), (float)q0_q180);
	    *(dst+i) = abax_ModeF<float>(m_nFreq, tmp);
	     mfDis[i] =  *(dst+i);
	}
}

/* 对整副图像进行4子帧转深度处理 */
int SubFrameClass::convertData(char *subFrames[4], u16 *dst, int width, int height)
{
	int i, j, cnt = 0;
	int offsetA, offsetB;
	int begin, end;

	/* 数据顺序：A1 B2 A3 B4 B1 A2 B3 A4 */
	for (j = height - 1; j > 0; j--) {
		begin = j * width * 2;
		end = (j + 1) * width * 2;
		for (i = begin; i < end; i += 4 * 2) {
			offsetA = i;
			offsetB = i + 4;
			*(u16*)(dst + cnt) = convertPixel(subFrames, offsetA, offsetB);
			cnt++;
			
			offsetA = i + 5;
			offsetB = i + 1;
			*(u16*)(dst + cnt) = convertPixel(subFrames, offsetA, offsetB);
			cnt++;
			
			offsetA = i + 2;
			offsetB = i + 6;
			*(u16*)(dst + cnt) = convertPixel(subFrames, offsetA, offsetB);
			cnt++;
			
			offsetA = i + 7;
			offsetB = i + 3;
			*(u16*)(dst + cnt) = convertPixel(subFrames, offsetA, offsetB);
			cnt++;
		}
	}
	
	return 0;
}

u16 SubFrameClass::convertPixelab(char* subFrames[4], int i)
{
    int Q0, Q90, Q180, Q270;
    int k = i/2;
    Q0 = ntohs(*(u16*)(subFrames[0]+i*2)) + ntohs(*(u16*)(subFrames[2]+i*2 +2));
    Q90 =  ntohs(*(u16*)(subFrames[1]+i*2)) + ntohs(*(u16*)(subFrames[3]+i*2 +2));
    Q180 = ntohs(*(u16*)(subFrames[2]+i*2)) + ntohs(*(u16*)(subFrames[0]+i*2 +2));
    Q270 =  ntohs(*(u16*)(subFrames[3]+i*2)) + ntohs(*(u16*)(subFrames[1]+i*2 +2));

    *((u16 *)mQ0+i/2) = Q0;
	mvQ0[k] = Q0;
	*((u16 *)mQ90+i/2) = Q90;
#ifdef  ABAX_TOGGER_DEBUG  
	mvQ90[k] = Q90;
	mvQ180[k] = Q180;
	mvQ270[k] = Q270;
#endif	
	*((u16 *)mQ180+i/2) = Q180;

    *((u16 *)mQ270+i/2) = Q270;

    return Q0;
}

u16 SubFrameClass::convertPixelba(char* subFrames[4], int i)
{
    int Q0, Q90, Q180, Q270;
    int k = i/2;
    Q0 = ntohs(*(u16*)(subFrames[0]+i*2+2)) + ntohs(*(u16*)(subFrames[2]+i*2));
    Q90 =  ntohs(*(u16*)(subFrames[1]+i*2+2)) + ntohs(*(u16*)(subFrames[3]+i*2));
    Q180 = ntohs(*(u16*)(subFrames[2]+i*2+2)) + ntohs(*(u16*)(subFrames[0]+i*2));
    Q270 =  ntohs(*(u16*)(subFrames[3]+i*2+2)) + ntohs(*(u16*)(subFrames[1]+i*2));
    mvQ0[k] = Q0;
    *((u16 *)mQ0+i/2) = Q0;
#ifdef  ABAX_TOGGER_DEBUG  	
	mvQ90[k] = Q90;
	mvQ180[k] = Q180;
	mvQ270[k] = Q270;
#endif	
	*((u16 *)mQ90+i/2) = Q90;
	
	*((u16 *)mQ180+i/2) = Q180;
	
    *((u16 *)mQ270+i/2) = Q270;
    return Q0;
}

void SubFrameClass::getTapA(char* subFrames[4], int i, int j)
{
    *((u16 *)mQ0+i/2) =  ntohs(*(u16*)(subFrames[0]+i*2+j));
 //   mQ90[i/2] = ntohs(*(u16*)(subFramesb.data()+i*2+j));
 //   mQ180[i/2] =  ntohs(*(u16*)(subFramesc.data()+i*2+j));
 //   mQ270[i/2] =  ntohs(*(u16*)(subFramesd.data()+i*2+j));
}

void SubFrameClass::getTapB(char* subFrames[4], int i, int j)
{
    //int k = i/2;

     *((u16 *)mQ0b +i/2) = ntohs(*(u16*)(subFrames[2]+i*2 +2 -j));
    //    mQ180b[k] = *pixData = ntohs(*(u16*)(subFramesa.data()+i*2 +2 -j));
    //    mQ270b[k] = ntohs(*(u16*)(subFramesb.data()+i*2 +2 -j));
    //   mQ0b[k] =  ntohs(*(u16*)(subFramesc.data()+i*2 +2 -j));
    //   mQ90b[k] = ntohs(*(u16*)(subFramesc.data()+i*2 +2 -j));
    //    pixData++;
    //    *pixData =  ntohs(*(u16*)(subFramesa.data()+i*2 + 4));
}



void  SubFrameClass::convertDataabba(char *subFrames[4], float *dst, int width, int height)
{
    int i, j;
    int begin, end;

    /* 数据顺序：A1 B1 B2 A2 */
    for (j = height - 1; j >= 0; j--) {
        begin = j * width * 2;
        end = (j + 1) * width * 2;
        for (i = begin; i < end; i += 4) {
//             getTapA(subFrames,i,0);
//			 getTapB(subFrames,i,0);
//			 getTapB(subFrames,i+2, 2);
//             getTapA(subFrames,i+2, 2);
			 convertPixelab(subFrames,i);
             convertPixelba(subFrames, i+2);

        }
    }
	convertDistance(dst);
   
}

void  SubFrameClass::convertDataabba(char *subFrames[4], int width, int height)
{
    int i, j;
    int begin, end;

    /* 数据顺序：A1 B1 B2 A2 */
    for (j = height - 1; j >= 0; j--) {
        begin = j * width * 2;
        end = (j + 1) * width * 2;
        for (i = begin; i < end; i += 4) {
//             getTapA(subFrames,i,0);
//			 getTapB(subFrames,i,0);
//			 getTapB(subFrames,i+2, 2);
//             getTapA(subFrames,i+2, 2);
			 convertPixelab(subFrames,i);
             convertPixelba(subFrames, i+2);

        }
    }
    
}


u16 SubFrameClass::getQ0(int i)
{
   return *((u16 *)mQ0 + i) ;
   //return mvQ0[i];
}

u16 SubFrameClass::getQ90(int i)
{
   return *((u16 *)mQ90 + i) ;
   //return mvQ0[i];
}

u16 SubFrameClass::getQ180(int i)
{
   return *((u16 *)mQ180 + i) ;
   //return mvQ0[i];
}

u16 SubFrameClass::getQ270(int i)
{
   return *((u16 *)mQ270 + i) ;
   //return mvQ0[i];
}

u16 SubFrameClass::getSum(int i)
{
	return getQ0(i) + getQ180(i) + getQ90(i) + getQ270(i);	
}

void SubFrameClass::setValue(int i,  u16 q0, u16 q90, u16 q180, u16 q270)
{
	*((u16 *)mQ0 + i) = q0;
	*((u16 *)mQ90 + i) = q90;
	*((u16 *)mQ180 + i) = q180;
	*((u16 *)mQ270 + i) = q270;
}