#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <stdlib.h>

int sensor_get_fps();
int preview_pclk;

#define MCLK (20*1000*1000)
#define MCLK_DIV 1

#define REG_3034  0x18
#define REG_3037  0x4
#define REG_3036 (4*20)
#define REG_3035 0x11
#define REG_3108 0x01
 
unsigned long vts,hts,vts_extra;

int ov5640_get_sysclk();
 
int main(int argc, char** argv) {
	
	unsigned long pclk;
  	unsigned char pre_div,mul,sys_div,pll_rdiv,bit_div,sclk_rdiv;
	
  //INPUT DATA
  pre_div = REG_3037;//0x3037
  mul =REG_3036;// 0x3036//
  sys_div=REG_3035;	 //0x3035
  pll_rdiv =pre_div ;
  bit_div = REG_3034 ;//0x3034
  sclk_rdiv = REG_3108;
  
  pre_div = pre_div & 0x0f;
  
  if(pre_div == 0)
    pre_div = 1;
  
  if(mul >= 128)
    mul = mul/2*2;
    
  sys_div = (sys_div & 0xf0) >> 4;

  pll_rdiv = (pll_rdiv & 0x10) >> 4;
  pll_rdiv = pll_rdiv + 1;
  

  bit_div = (bit_div & 0x0f);
  
  sclk_rdiv = (sclk_rdiv & 0x03);
  sclk_rdiv = sclk_rdiv << sclk_rdiv;
  
  printf("pre_div = %d,mul = %d,sys_div = %d,pll_rdiv = %d,sclk_rdiv = %d\n",\
          pre_div,mul,sys_div,pll_rdiv,sclk_rdiv);
  
  if((pre_div&&sys_div&&pll_rdiv&&sclk_rdiv) == 0){
  
  		printf("Issue \n");
    	return 0;
	}
		
  if(bit_div == 8)
    pclk = MCLK/MCLK_DIV/ pre_div * mul / sys_div / pll_rdiv / 2 / sclk_rdiv;
  else if(bit_div == 10)
    pclk = MCLK/MCLK_DIV / pre_div * mul / sys_div / pll_rdiv * 2 / 5 / sclk_rdiv;
  else
    pclk = MCLK/MCLK_DIV / pre_div * mul / sys_div / pll_rdiv / 1 / sclk_rdiv;
  
  printf("pclk = %ld\n",pclk);
  
  preview_pclk = pclk;
  
  sensor_get_fps();
  
  
  //Calcluate band filter:
  unsigned int band_step60 = ov5640_get_sysclk()* 100 /(double) hts * (double)100 / (double)120;
  unsigned int band_step50 =ov5640_get_sysclk() * 100 /(double) hts * (double)100 / (double)100;
  
  unsigned int capture_max_band50 = (int)((vts - 4)/band_step50);
  unsigned int  capture_max_band60  = (int)((vts - 4)/band_step60);
  
  printf("band 60hz = %ld  50hz =  %ld SYSCLKC  %ld /n Max Band 50 %ld and 60 : %ld    \n",band_step60,band_step50,ov5640_get_sysclk(),capture_max_band50,capture_max_band60);
  
	system("PAUSE");
  return 0;
}

int sensor_get_fps()
{
  unsigned char vts_low,vts_high,hts_low,hts_high,vts_extra_high,vts_extra_low;
  
  double preview_fps;
  
  hts_high = 0x07;	// 0x380c
  hts_low =0xf4;		// 0x380d
//  vts_high = 0x03;	//0x380e,	
//  vts_low = 0xf5; 	//0x380f

  vts_high = 0x07;	//0x380e,
  vts_low = 0xac; 	//0x380f
  vts_extra_high = 0x0 ;	//0x350c
  vts_extra_low = 0x0;	//0x350d
   
  hts = hts_high * 256 + hts_low;
  vts = vts_high * 256 + vts_low;
  vts_extra = vts_extra_high * 256 + vts_extra_low;
  
  printf("hts = %d,vts = %d,vts_extra = %d, max exposure in lines %d (exposure in exposure register is in 16 lines)\n ",hts,vts,vts_extra,hts+vts);
  
  
  if((hts&&(vts+vts_extra)) == 0){
  
  		printf("Issue \n");
    	return 0;
	}
    
 
  preview_fps = (double)preview_pclk / ((double)(vts_extra+vts) *(double) hts);
  printf("preview fps = %f\n",preview_fps);
  
  printf("maximum exposure interval( 1964 x trow): = %f ms\n ",(double)1964000*((double)hts/(double)preview_pclk));
  printf("maximum exposure interval       expected = %f ms\n" ,1000/(double)preview_fps);
  
  return 0;
}


int ov5640_get_sysclk()
{
	 /* calculate sysclk */
	unsigned int xvclk = MCLK / 10000;
	unsigned int multiplier, prediv, VCO, sysdiv, pll_rdiv;
	unsigned int sclk_rdiv_map[] = {1, 2, 4, 8};
	unsigned int bit_div2x = 1, sclk_rdiv, sysclk;
	unsigned char temp1, temp2;
	int ret;

	temp1=REG_3034;

	temp2 = temp1 & 0x0f;
	if (temp2 == 8 || temp2 == 10)
		bit_div2x = temp2 / 2;

	temp1=REG_3035;
	sysdiv = temp1 >> 4;
	if (sysdiv == 0)
		sysdiv = 16;

	temp1=REG_3036;
	multiplier = temp1;

	temp1=REG_3037;
	prediv = temp1 & 0x0f;
	pll_rdiv = ((temp1 >> 4) & 0x01) + 1;

	temp1=REG_3108;	
	temp2 = temp1 & 0x03;
	sclk_rdiv = sclk_rdiv_map[temp2];

	if (!prediv || !sysdiv || !pll_rdiv || !bit_div2x)
		return -EINVAL;

	VCO = xvclk * multiplier / prediv;

	sysclk = VCO / sysdiv / pll_rdiv * 2 / bit_div2x / sclk_rdiv;

	return sysclk;
}