/*
 * ov2640.c
 *
 *  Created on: Nov 9, 2020
 *      Author: Progettazione2020_02
 */

#include "ov2640.h"
#include <string.h>
#define DEBUG

/* DSP register bank FF=0x00*/
/*
#define R_BYPASS            0x05
#define QS                  0x44
#define CTRLI               0x50
#define HSIZE               0x51
#define VSIZE               0x52
#define XOFFL               0x53
#define YOFFL               0x54
#define VHYX                0x55
#define DPRP                0x56
#define TEST                0x57
#define ZMOW                0x5A
#define ZMOH                0x5B
#define ZMHH                0x5C
#define BPADDR              0x7C
#define BPDATA              0x7D
#define CTRL2               0x86
#define CTRL3               0x87
#define SIZEL               0x8C
#define HSIZE8              0xC0
#define VSIZE8              0xC1
#define CTRL0               0xC2
#define CTRL1               0xC3
#define R_DVP_SP            0xD3
#define IMAGE_MODE          0xDA
#define RESET               0xE0
#define MS_SP               0xF0
#define SS_ID               0xF7
#define SS_CTRL             0xF7
#define MC_BIST             0xF9
#define MC_AL               0xFA
#define MC_AH               0xFB
#define MC_D                0xFC
#define P_CMD               0xFD
#define P_STATUS            0xFE
#define BANK_SEL            0xFF

#define CTRLI_LP_DP         0x80
#define CTRLI_ROUND         0x40

#define CTRL0_AEC_EN        0x80
#define CTRL0_AEC_SEL       0x40
#define CTRL0_STAT_SEL      0x20
#define CTRL0_VFIRST        0x10
#define CTRL0_YUV422        0x08
#define CTRL0_YUV_EN        0x04
#define CTRL0_RGB_EN        0x02
#define CTRL0_RAW_EN        0x01

#define CTRL2_DCW_EN        0x20
#define CTRL2_SDE_EN        0x10
#define CTRL2_UV_ADJ_EN     0x08
#define CTRL2_UV_AVG_EN     0x04
#define CTRL2_CMX_EN        0x01

#define CTRL3_BPC_EN        0x80
#define CTRL3_WPC_EN        0x40

#define R_DVP_SP_AUTO_MODE  0x80

#define R_BYPASS_DSP_EN         0x00
#define R_BYPASS_DSP_BYPAS      0x01

#define IMAGE_MODE_Y8_DVP_EN    0x40
#define IMAGE_MODE_JPEG_EN      0x10
#define IMAGE_MODE_YUV422       0x00
#define IMAGE_MODE_RAW10        0x04
#define IMAGE_MODE_RGB565       0x08
#define IMAGE_MODE_HREF_VSYNC   0x02
#define IMAGE_MODE_LBYTE_FIRST  0x01

#define RESET_MICROC            0x40
#define RESET_SCCB              0x20
#define RESET_JPEG              0x10
#define RESET_DVP               0x04
#define RESET_IPU               0x02
#define RESET_CIF               0x01

#define MC_BIST_RESET           0x80
#define MC_BIST_BOOT_ROM_SEL    0x40
#define MC_BIST_12KB_SEL        0x20
#define MC_BIST_12KB_MASK       0x30
#define MC_BIST_512KB_SEL       0x08
#define MC_BIST_512KB_MASK      0x0C
#define MC_BIST_BUSY_BIT_R      0x02
#define MC_BIST_MC_RES_ONE_SH_W 0x02
#define MC_BIST_LAUNCH          0x01
*/

typedef enum {
    BANK_DSP, BANK_SENSOR, BANK_MAX
} ov2640_bank_t;

/* Sensor register bank FF=0x01*/
/*
#define GAIN                0x00
#define COM1                0x03
#define REG04               0x04
#define REG08               0x08
#define COM2                0x09
#define REG_PID             0x0A
#define REG_VER             0x0B
#define COM3                0x0C
#define COM4                0x0D
#define AEC                 0x10
#define CLKRC               0x11
#define COM7                0x12
#define COM8                0x13
#define COM9                0x14 // AGC gain ceiling
#define COM10               0x15
#define HSTART              0x17
#define HSTOP               0x18
#define VSTART              0x19
#define VSTOP               0x1A
#define MIDH                0x1C
#define MIDL                0x1D
#define AEW                 0x24
#define AEB                 0x25
#define VV                  0x26
#define REG2A               0x2A
#define FRARL               0x2B
#define ADDVSL              0x2D
#define ADDVSH              0x2E
#define YAVG                0x2F
#define HSDY                0x30
#define HEDY                0x31
#define REG32               0x32
#define ARCOM2              0x34
#define REG45               0x45
#define FLL                 0x46
#define FLH                 0x47
#define COM19               0x48
#define ZOOMS               0x49
#define COM22               0x4B
#define COM25               0x4E
#define BD50                0x4F
#define BD60                0x50
#define REG5D               0x5D
#define REG5E               0x5E
#define REG5F               0x5F
#define REG60               0x60
#define HISTO_LOW           0x61
#define HISTO_HIGH          0x62

#define REG04_DEFAULT       0x20 //0xD8 esp32
#define REG04_HFLIP_IMG     0x80
#define REG04_VFLIP_IMG     0x40
#define REG04_VREF_EN       0x10
#define REG04_HREF_EN       0x08
#define REG04_SET(x)        (REG04_DEFAULT|x)

#define COM2_STDBY          0x10
#define COM2_OUT_DRIVE_1x   0x00
#define COM2_OUT_DRIVE_2x   0x01
#define COM2_OUT_DRIVE_3x   0x02
#define COM2_OUT_DRIVE_4x   0x03

#define COM3_DEFAULT        0x38
#define COM3_BAND_50Hz      0x04
#define COM3_BAND_60Hz      0x00
#define COM3_BAND_AUTO      0x02
#define COM3_BAND_SET(x)    (COM3_DEFAULT|x)

#define COM7_SRST           0x80
#define COM7_RES_UXGA       0x00 // UXGA
#define COM7_RES_SVGA       0x40 // SVGA
#define COM7_RES_CIF        0x20 // CIF
#define COM7_ZOOM_EN        0x04 // Enable Zoom
#define COM7_COLOR_BAR      0x02 // Enable Color Bar Test

#define COM8_DEFAULT        0xC0
#define COM8_BNDF_EN        0x20 // Enable Banding filter
#define COM8_AGC_EN         0x04 // AGC Auto/Manual control selection
#define COM8_AEC_EN         0x01 // Auto/Manual Exposure control
#define COM8_SET(x)         (COM8_DEFAULT|x)

#define COM9_DEFAULT        0x08
#define COM9_AGC_GAIN_2x    0x00 // AGC:    2x
#define COM9_AGC_GAIN_4x    0x01 // AGC:    4x
#define COM9_AGC_GAIN_8x    0x02 // AGC:    8x
#define COM9_AGC_GAIN_16x   0x03 // AGC:   16x
#define COM9_AGC_GAIN_32x   0x04 // AGC:   32x
#define COM9_AGC_GAIN_64x   0x05 // AGC:   64x
#define COM9_AGC_GAIN_128x  0x06 // AGC:  128x
#define COM9_AGC_SET(x)     (COM9_DEFAULT|(x<<5))

#define COM10_HREF_EN       0x80 // HSYNC changes to HREF
#define COM10_HSYNC_EN      0x40 // HREF changes to HSYNC/
#define COM10_PCLK_FREE     0x20 // PCLK output option: free running PCLK
#define COM10_PCLK_EDGE     0x10 // Data is updated at the rising edge of PCLK
#define COM10_HREF_NEG      0x08 // HREF negative
#define COM10_VSYNC_NEG     0x02 // VSYNC negative
#define COM10_HSYNC_NEG     0x01 // HSYNC negative

#define CTRL1_AWB           0x08 // Enable AWB

#define VV_AGC_TH_SET(h,l)  ((h<<4)|(l&0x0F))

#define REG32_UXGA          0x36
#define REG32_SVGA          0x09
#define REG32_CIF           0x89

#define CLKRC_2X            0x80
#define CLKRC_2X_UXGA       (0x01 | CLKRC_2X)
#define CLKRC_2X_SVGA       CLKRC_2X
#define CLKRC_2X_CIF        CLKRC_2X

*/


I2C_HandleTypeDef *phi2c;
DCMI_HandleTypeDef *phdcmi;
UART_HandleTypeDef *huart_test;

// 30fps@24MHz su esp32
const unsigned char OV2640_CIF_INIT_REG_TBL[][2] = {
    {BANK_SEL, BANK_SEL_DSP},
	{ 0x2c ,    0xff },
	{ 0x2e ,    0xdf },
	{BANK_SEL, BANK_SEL_SENS},
	{ 0x3c ,    0x32 },
	{CLKRC, CLKRC_DIV_SET ( 1 )},
	{COM2, COM2_OCAP_Nx_SET ( 3 )},
	{REG04, REG04_DEF | REG04_HREF_EN},
	{COM8, COM8_DEF | COM8_AGC_EN | COM8_AEC_EN},
	// ~ {AEC, 0x00},
	{COM9, COM9_AGC_GAIN_8x | 0x08 },
	{ 0x2c ,    0x0c },
	{ 0x33 ,    0x78 },
	{ 0x3a ,    0x33 },
	{ 0x3b ,    0xfb },
	{ 0x3e ,    0x00 },
	{ 0x43 ,    0x11 },
	{ 0x16 ,    0x10 },
	{ 0x39 ,    0x02 },
	{ 0x35 ,    0x88 },
	{ 0x22 ,    0x0A },
	{ 0x37 ,    0x40 },
	{ 0x23 ,    0x00 },
	{ARCOM2, 0xa0 },
	{ 0x06 ,    0x02 },
	{ 0x06 ,    0x88 },
	{ 0x07 ,    0xc0 },
	{ 0x0d ,    0xb7 },
	{ 0x0e ,    0x01 },
	{ 0x4c ,    0x00 },
	{ 0x4a ,    0x81 },
	{ 0x21 ,    0x99 },
	{AEW,     0x40 },
	{AEB,     0x38 },
	{VV,  VV_HIGH_TH_SET ( 0x08 ) | VV_LOW_TH_SET ( 0x02 )},
	{ 0x5c ,    0x00 },
	{ 0x63 ,    0x00 },
	{FLL,     0x22 },
	{COM3,    0x38 | COM3_BAND_AUTO},
	{REG5D,   0x55 },
	{REG5E,   0x7d },
	{REG5F,   0x7d },
	{REG60,   0x55 },
	{HISTO_LOW,    0x70 },
	{HISTO_HIGH,   0x80 },
	{ 0x7c ,    0x05 },
	{ 0x20 ,    0x80 },
	{ 0x28 ,    0x30 },
	{ 0x6c ,    0x00 },
	{ 0x6d ,    0x80 },
	{ 0x6e ,    0x00 },
	{ 0x70 ,    0x02 },
	{ 0x71 ,    0x94 },
	{ 0x73 ,    0xc1 },
	{ 0x3d ,    0x34 },
	{COM7, COM7_RES_UXGA | COM7_ZOOM_EN},
	{ 0x5a ,    0x57 },
	{BD50,    0xbb },
	{BD60,    0x9c },
	{BANK_SEL, BANK_SEL_DSP},
	{ 0xe5 ,    0x3f },
	{MC_BIST, MC_BIST_RESET | MC_BIST_BOOT_ROM_SEL},
	{ 0x41 ,    0x24 },
	{RESET, RESET_JPEG | RESET_DVP},
	{ 0x76 ,    0xff },
	{ 0x33 ,    0xa0 },
	{ 0x42 ,    0x20 },
	{ 0x43 ,    0x18 },
	{ 0x4c ,    0x00 },
	{CTRL3, CTRL3_BPC_EN | CTRL3_WPC_EN | 0x10 },
	{ 0x88 ,    0x3f },
	{ 0xd7 ,    0x03 },
	{ 0xd9 ,    0x10 },
	{R_DVP_SP, R_DVP_SP_AUTO_MODE | 0x2 },
	{ 0xc8 ,    0x08 },
	{ 0xc9 ,    0x1f },
	{BPADDR, 0x00 },
	{BPDATA, 0x00 },
	{BPADDR, 0x03 },
	{BPDATA, 0x48 },
	{BPDATA, 0x48 },
	{BPADDR, 0x08 },
	{BPDATA, 0x20 },
	{BPDATA, 0x10 },
	{BPDATA, 0x0e },
	{ 0x90 ,    0x00 },
	{ 0x91 ,    0x0e },
	{ 0x91 ,    0x1a },
	{ 0x91 ,    0x31 },
	{ 0x91 ,    0x5a },
	{ 0x91 ,    0x69 },
	{ 0x91 ,    0x75 },
	{ 0x91 ,    0x7e },
	{ 0x91 ,    0x88 },
	{ 0x91 ,    0x8f },
	{ 0x91 ,    0x96 },
	{ 0x91 ,    0xa3 },
	{ 0x91 ,    0xaf },
	{ 0x91 ,    0xc4 },
	{ 0x91 ,    0xd7 },
	{ 0x91 ,    0xe8 },
	{ 0x91 ,    0x20 },
	{ 0x92 ,    0x00 },
	{ 0x93 ,    0x06 },
	{ 0x93 ,    0xe3 },
	{ 0x93 ,    0x03 },
	{ 0x93 ,    0x03 },
	{ 0x93 ,    0x00 },
	{ 0x93 ,    0x02 },
	{ 0x93 ,    0x00 },
	{ 0x93 ,    0x00 },
	{ 0x93 ,    0x00 },
	{ 0x93 ,    0x00 },
	{ 0x93 ,    0x00 },
	{ 0x93 ,    0x00 },
	{ 0x93 ,    0x00 },
	{ 0x96 ,    0x00 },
	{ 0x97 ,    0x08 },
	{ 0x97 ,    0x19 },
	{ 0x97 ,    0x02 },
	{ 0x97 ,    0x0c },
	{ 0x97 ,    0x24 },
	{ 0x97 ,    0x30 },
	{ 0x97 ,    0x28 },
	{ 0x97 ,    0x26 },
	{ 0x97 ,    0x02 },
	{ 0x97 ,    0x98 },
	{ 0x97 ,    0x80 },
	{ 0x97 ,    0x00 },
	{ 0x97 ,    0x00 },
	{ 0xa4 ,    0x00 },
	{ 0xa8 ,    0x00 },
	{ 0xc5 ,    0x11 },
	{ 0xc6 ,    0x51 },
	{ 0xbf ,    0x80 },
	{ 0xc7 ,    0x10 }, /*bilanciamento del bianco */
	{ 0xb6 ,    0x66 },
	{ 0xb8 ,    0xA5 },
	{ 0xb7 ,    0x64 },
	{ 0xb9 ,    0x7C },
	{ 0xb3 ,    0xaf },
	{ 0xb4 ,    0x97 },
	{ 0xb5 ,    0xFF },
	{ 0xb0 ,    0xC5 },
	{ 0xb1 ,    0x94 },
	{ 0xb2 ,    0x0f },
	{ 0xc4 ,    0x5c },
	{ 0xa6 ,    0x00 },
	{ 0xa7 ,    0x20 },
	{ 0xa7 ,    0xd8 },
	{ 0xa7 ,    0x1b },
	{ 0xa7 ,    0x31 },
	{ 0xa7 ,    0x00 },
	{ 0xa7 ,    0x18 },
	{ 0xa7 ,    0x20 },
	{ 0xa7 ,    0xd8 },
	{ 0xa7 ,    0x19 },
	{ 0xa7 ,    0x31 },
	{ 0xa7 ,    0x00 },
	{ 0xa7 ,    0x18 },
	{ 0xa7 ,    0x20 },
	{ 0xa7 ,    0xd8 },
	{ 0xa7 ,    0x19 },
	{ 0xa7 ,    0x31 },
	{ 0xa7 ,    0x00 },
	{ 0xa7 ,    0x18 },
	{ 0x7f ,    0x00 },
	{ 0xe5 ,    0x1f },
	{ 0xe1 ,    0x77 },
	{ 0xdd ,    0x7f },
	{CTRL0, CTRL0_YUV422 | CTRL0_YUV_EN},
	 {0XFF, 0XFF},
};
/*
const unsigned char OV2640_CIF_INIT_REG_TBL[][2] = {
    {BANK_SEL, BANK_DSP},
	{0x2c, 0xff}, //registro riservato
    {0x2e, 0xdf}, //registro riservato
    {BANK_SEL, BANK_SENSOR},
    {0x3c, 0x32}, //registro riservato
    {CLKRC, 0x01}, //Clock rate control CLK=XVCLK/decimal value [5:0],bit[6] reserved bit,[7] internal frequency doublers on/OFF selection
    {COM2, COM2_OUT_DRIVE_3x}, //setting output drive capability
    {REG04, REG04_DEFAULT || 0x08}, //mirror e vertical flip enabled, VREF bit e HREF bit[0]
    {COM8, COM8_DEFAULT | COM8_BNDF_EN | COM8_AGC_EN | COM8_AEC_EN},//AEC auto exposure control, agc auto control e 1/120s minimo exposure time
    {COM9, COM9_AGC_SET(COM9_AGC_GAIN_8x)}, //AGC gain ceiling
    {0x2c, 0x0c}, //registro riservato
    {0x33, 0x78}, //registro riservato
    {0x3a, 0x33}, //registro riservato
    {0x3b, 0xfB}, //registro riservato
    {0x3e, 0x00}, //registro riservato
    {0x43, 0x11}, //registro riservato
    {0x16, 0x10}, //registro riservato
    {0x39, 0x92}, //registro riservato
    {0x35, 0xda}, //registro riservato
    {0x22, 0x1a}, //registro riservato
    {0x37, 0xc3}, //registro riservato
    {0x23, 0x00}, //registro riservato
    {ARCOM2, 0xc0}, //Bit[7:3]reserved bit[2]: Zoom window horizontal start point bit[0:1]: reserved
  //{0x36, 0x1a},
	{0x06, 0x88}, //registro riservato
    {0x07, 0xc0}, //registro riservato
    {COM4, 0x87}, //Bit[7:3]reserved bit[2]: clock output power down pin-status bit[0:1]: reserved
    {0x0e, 0x41}, //Registro riservato
    {0x4c, 0x00}, //Registro riservato
  //{0x48, 0x00},
  //{0x5B, 0x00},
  //{0x42, 0x03},
	{0x4a, 0x81}, //Registro riservato
    {0x21, 0x99}, //Registro riservato
    {AEW, 0x40}, //LUMINANCE SIGNAL HIGH RANGE FOR aec/agc OPERATION
    {AEB, 0x38}, //LUMINANCE SIGNAL LOW RANGE FOR aec/agc OPERATION
    {VV, VV_AGC_TH_SET(8,2)}, //Fast mode large step range threshold - effective only in AEC/AGC fast mode (COM8[7]=1)
    {0x5c, 0x00}, //Registro riservato
    {0x63, 0x00}, //Registro riservato
  //{0x46, 0x00}  //frame lenght adj LSBs ogni bit aggiungerà 1 timing nella linea orizzontale del frame
  //{COM3, 0x3C}, registro controllo snapshot o video continuo frequenza di banding (50 o 60 Hz)
    {HISTO_LOW, 0x70}, //Histogram Algorithm Low Level
    {HISTO_HIGH, 0x80}, //Histogram Algorithm High Level
    {0x7c, 0x05}, //Registro riservato
    {0x20, 0x80}, //Registro riservato
    {0x28, 0x30}, //Registro riservato
    {0x6c, 0x00}, //Registro riservato
    {0x6d, 0x80}, //Registro riservato
    {0x6e, 0x00}, //Registro riservato
    {0x70, 0x02}, //Registro riservato
    {0x71, 0x94}, //Registro riservato
    {0x73, 0xc1}, //Registro riservato
    {0x3d, 0x34}, //Registro riservato
    {0x5a, 0x57}, //Registro riservato
    {BD50, 0xbb}, //50Hz Banding AEC 8 LSBs
    {BD60, 0x9c}, //60Hz Banding AEC 8 LSBs

	//da qui parte l'impostazione della risoluzione
    {COM7, COM7_RES_CIF}, //imposto il cif mode ma c'è un errore secondo me!
    {HSTART, 0x11}, //horizontal window start MSB 8 bits
    {HSTOP, 0x43},  //horizontal window stop MSB 8 bits
    {VSTART, 0x00}, //Vertical window start MSB 8 bits
    {VSTOP, 0x25}, //Vertical window stop MSB 8 bits
    {REG32, 0x89}, //[7:6]pixel clock divide option bit[5:3]horizontal window end position 3LSB bit[2:0]horizontal window start position 3LSB
    {0x37, 0xc0}, //Registro riservato

    {BD50, 0xca}, //banding 50 Hz AEC 8 LSBs
    {BD60, 0xa8}, //banding 60 Hz AEC 8 LSBs
    //{0x5a, 0x23},
	{0x6d, 0x00}, //Registro riservato
    {0x3d, 0x38}, //Registro riservato

	{BANK_SEL, BANK_DSP},
    {0xe5, 0x7f}, //blocco in un registro riservato
    {MC_BIST, MC_BIST_RESET | MC_BIST_BOOT_ROM_SEL}, //vedi datasheet errori memoria, attivazione bist, reset microcontroller
    {0x41, 0x24}, //Registro riservato
    {RESET, RESET_JPEG | RESET_DVP},
    {0x76, 0xff}, //Registro riservato
    {0x33, 0xa0}, //Registro riservato
    {0x42, 0x20}, //Registro riservato
    {0x43, 0x18}, //Registro riservato
    {0x4c, 0x00}, //Registro riservato
    {CTRL3, CTRL3_WPC_EN | 0x10 }, //0xd5 lo scrive sui driver cinesi ma secondo me non ha senso
    {0x88, 0x3f}, //Registro riservato
    {0xd7, 0x03}, //Registro riservato
    {0xd9, 0x10}, //Registro riservato
    {R_DVP_SP, R_DVP_SP_AUTO_MODE | 0x02}, // bit [7] auto mode, bit[6:0] DVP output speed control
    {0xc8, 0x08}, //Registro riservato
    {0xc9, 0x80}, //Registro riservato
    {BPADDR, 0x00}, //SDE Indirect Register Access: Address
    {BPDATA, 0x00}, //SDE Indirect Register Access: DATA
    {BPADDR, 0x03}, //SDE Indirect Register Access: Address
    {BPDATA, 0x48}, //SDE Indirect Register Access: DATA
    {BPDATA, 0x48}, //SDE Indirect Register Access: DATA
    {BPADDR, 0x08}, //SDE Indirect Register Access: Address
    {BPDATA, 0x20}, //SDE Indirect Register Access: DATA
    {BPDATA, 0x10}, //SDE Indirect Register Access: DATA
    {BPDATA, 0x0e}, //SDE Indirect Register Access: DATA
    {0x90, 0x00},   //Registro riservato
    {0x91, 0x0e},   //Registro riservato
    {0x91, 0x1a},   //Registro riservato
    {0x91, 0x31},   //Registro riservato
    {0x91, 0x5a},   //Registro riservato
    {0x91, 0x69},   //Registro riservato
    {0x91, 0x75},   //Registro riservato
    {0x91, 0x7e},   //Registro riservato
    {0x91, 0x88},   //Registro riservato
    {0x91, 0x8f},   //Registro riservato
    {0x91, 0x96},   //Registro riservato
    {0x91, 0xa3},   //Registro riservato
    {0x91, 0xaf},   //Registro riservato
    {0x91, 0xc4},   //Registro riservato
    {0x91, 0xd7},   //Registro riservato
    {0x91, 0xe8},   //Registro riservato
    {0x91, 0x20},   //Registro riservato
    {0x92, 0x00},   //Registro riservato
    {0x93, 0x06},   //Registro riservato
    {0x93, 0xe3},   //Registro riservato
    {0x93, 0x05},   //Registro riservato
    {0x93, 0x05},   //Registro riservato
    {0x93, 0x00},   //Registro riservato
    {0x93, 0x04},   //Registro riservato
    {0x93, 0x00},   //Registro riservato
    {0x93, 0x00},   //Registro riservato
    {0x93, 0x00},   //Registro riservato
    {0x93, 0x00},   //Registro riservato
    {0x93, 0x00},   //Registro riservato
    {0x93, 0x00},   //Registro riservato
    {0x93, 0x00},   //Registro riservato
    {0x96, 0x00}, //Registro riservato
    {0x97, 0x08}, //Registro riservato
    {0x97, 0x19}, //Registro riservato
    {0x97, 0x02}, //Registro riservato
    {0x97, 0x0c}, //Registro riservato
    {0x97, 0x24}, //Registro riservato
    {0x97, 0x30}, //Registro riservato
    {0x97, 0x28}, //Registro riservato
    {0x97, 0x26}, //Registro riservato
    {0x97, 0x02}, //Registro riservato
    {0x97, 0x98}, //Registro riservato
    {0x97, 0x80}, //Registro riservato
    {0x97, 0x00}, //Registro riservato
    {0x97, 0x00}, //Registro riservato
  //{0xc3, 0xef},
	{0xa4, 0x00}, //Registro riservato
    {0xa8, 0x00}, //Registro riservato
    {0xc5, 0x11}, //Registro riservato
    {0xc6, 0x51}, //Registro riservato
    {0xbf, 0x80}, //Registro riservato
    {0xc7, 0x10}, //Registro riservato
    {0xb6, 0x66}, //Registro riservato
    {0xb8, 0xA5}, //Registro riservato
    {0xb7, 0x64}, //Registro riservato
    {0xb9, 0x7C}, //Registro riservato
    {0xb3, 0xaf}, //Registro riservato
    {0xb4, 0x97}, //Registro riservato
    {0xb5, 0xFF}, //Registro riservato
    {0xb0, 0xC5}, //Registro riservato
    {0xb1, 0x94}, //Registro riservato
    {0xb2, 0x0f}, //Registro riservato
    {0xc4, 0x5c}, //Registro riservato

	// parte fatta nel codice cinese per le dimensioni dell'immagine in uscita
		0xc0, 0xc8,
		0xc1, 0x96,
		0x8c, 0x00,
		0x86, 0x3d,
		0x50, 0x00,
		0x51, 0x90,
		0x52, 0x2c,
		0x53, 0x00,
		0x54, 0x00,
		0x55, 0x88,

		0x5a, 0x90,
		0x5b, 0x2C,
		0x5c, 0x05,

		0xd3, 0x02,//auto设置要小心
	//

    {CTRL1, 0xfd}, //il valore di default è FF, con FD disattiva il LENC (sul cinese elimina anche il DG 0xed)
    {0x7f, 0x00}, //Registro riservato
    {0xe5, 0x1f}, //Registro riservato
    {0xe1, 0x67}, //Registro riservato
    {0xdd, 0x7f}, //Registro riservato
    {IMAGE_MODE, 0x00}, //attiva jpeg, formato di uscita dalla DVP, HREF in JPEG, swap per DVP e il pin Y8 per DVP
    {RESET, 0x00}, //reset tutti disabilitati
    {R_BYPASS, R_BYPASS_DSP_EN}, //DSP non bypassato
    {0XFF, 0XFF},
}; */
/*
const unsigned char OV2640_TO_UXGA_REG_TBL[][2] = {
    {BANK_SEL, BANK_SENSOR},
    {COM7, COM7_RES_UXGA},

    //Set the sensor output window
    {COM1, 0x0F},
    {REG32, REG32_UXGA},
    {HSTART, 0x11},
    {HSTOP, 0x75},
    {VSTART, 0x01},
    {VSTOP, 0x97},

    //{CLKRC, 0x00},
    {0x3d, 0x34},
    {BD50, 0xbb},
    {BD60, 0x9c},
    {0x5a, 0x57},
    {0x6d, 0x80},
    {0x39, 0x82},
    {0x23, 0x00},
    {0x07, 0xc0},
    {0x4c, 0x00},
    {0x35, 0x88},
    {0x22, 0x0a},
    {0x37, 0x40},
    {ARCOM2, 0xa0},
    {0x06, 0x02},
    {COM4, 0xb7},
    {0x0e, 0x01},
    {0x42, 0x83},
    {BANK_SEL, BANK_DSP},
    {RESET, RESET_DVP},

    //Set the sensor resolution (UXGA, SVGA, CIF)
    {HSIZE8, 0xc8},
    {VSIZE8, 0x96},
    {SIZEL, 0x00},

    //Set the image window size >= output size
    {HSIZE, 0x90},
    {VSIZE, 0x2c},
    {XOFFL, 0x00},
    {YOFFL, 0x00},
    {VHYX, 0x88},
    {TEST, 0x00},

    {CTRL2, CTRL2_DCW_EN | 0x1d},
    {CTRLI, 0x00},
    //{R_DVP_SP, 0x06},
    {0xff, 0xff},
};

const unsigned char OV2640_TO_SVGA_REG_TBL[][2]=

       {{BANK_SEL, BANK_SENSOR},
	    {COM7, COM7_RES_SVGA},

	    //Set the sensor output window
	    {COM1, 0x0A},
	    {REG32, REG32_SVGA},
	    {HSTART, 0x11},
	    {HSTOP, 0x43},
	    {VSTART, 0x00},
	    {VSTOP, 0x4b},

	    //{CLKRC, 0x00},
	    {0x37, 0xc0},
	    {BD50, 0xca},
	    {BD60, 0xa8},
	    {0x5a, 0x23},
	    {0x6d, 0x00},
	    {0x3d, 0x38},
	    {0x39, 0x92},
	    {0x35, 0xda},
	    {0x22, 0x1a},
	    {0x37, 0xc3},
	    {0x23, 0x00},
	    {ARCOM2, 0xc0},
	    {0x06, 0x88},
	    {0x07, 0xc0},
	    {COM4, 0x87},
	    {0x0e, 0x41},
	    {0x42, 0x03},
	    {0x4c, 0x00},
	    {BANK_SEL, BANK_DSP},
	    {RESET, RESET_DVP},

	    //Set the sensor resolution (UXGA, SVGA, CIF)
	    {HSIZE8, 0x64},
	    {VSIZE8, 0x4B},
	    {SIZEL, 0x00},

	    //Set the image window size >= output size
	    {HSIZE, 0xC8},
	    {VSIZE, 0x96},
	    {XOFFL, 0x00},
	    {YOFFL, 0x00},
	    {VHYX, 0x00},
	    {TEST, 0x00},

	    {CTRL2, CTRL2_DCW_EN | 0x1D},
	    {CTRLI, CTRLI_LP_DP | 0x00},
	    //{R_DVP_SP, 0x08},
	    {0xff, 0xff},
	  };

const unsigned char OV2640_YUV422[][2] = {
	    {BANK_SEL, BANK_DSP},
	    {RESET, RESET_DVP},
	    {IMAGE_MODE, IMAGE_MODE_YUV422},
	    {0xD7, 0x01},
	    {0xE1, 0x67},
	    {RESET, 0x00},
	    {0xff, 0xff},
	};

const unsigned char OV2640_JPEG[][2] = { {0xff, 0x01},
		{0xe0, 0x14},
		{0xe1, 0x77},
		{0xe5, 0x1f},
		{0xd7, 0x03},
		{0xda, 0x10},
		{0xe0, 0x00}, };
*/
const unsigned char OV2640_JPEG[][2] =
        { {0xff, 0x00},
		{RESET, 0x14}, //resetta DVP e JPEG
		{0xda, 0x12}, //Abilito JPEG e pongo HREF=VSYNC e il formato di uscita dal DVP=YUV422
		{0xd7, 0x03}, //registro riservato
		{0xE1, 0x77}, //registro riservato
	    {0xE5, 0x1F}, //registro riservato
	    {0xD9, 0x10}, //registro riservato
		{0xDF, 0x80}, //registro riservato
	    {0x33, 0x80}, //registro riservato
	    {0x3C, 0x10}, //registro riservato
	    {0xEB, 0x30}, //registro riservato blocco
		{0xDD, 0x7F}, //registro riservato
		{RESET, 0x00}, //disabilita tutti i reset
		{0xff, 0xff}, };

/*const unsigned char OV2640_JPEG[][2] = {{0xff, 0x00}, { 0xe0, 0x14 }, { 0xe1, 0x77 }, { 0xe5,
		0x1f }, { 0xd7, 0x03 }, { 0xda, 0x10 }, { 0xe0, 0x00 }, { 0xFF, 0x01 },
		{ 0x04, 0x08 }, { 0xff, 0xff }, };
*/
const unsigned char OV2640_160x120_JPEG[][2] = {
		{0xff, 0x00},
		{ 0xe0, 0x14 },
		{ 0xe1, 0x77 },
		{ 0xe5, 0x1f },
		{ 0xd7, 0x03 },
		{ 0xda, 0x10 },
		{ 0xe0, 0x00 },
		{ 0xFF,0x01  },
		{ 0x04, 0x08 },
		{ 0x12, 0x02 }, //Enable Color Bar Test Pattern
		{ 0xff, 0xff }, };

const unsigned char OV2640_320x240_JPEG[][2] = { { 0xff, 0x00 }, { 0x12, 0x40 },
		{ 0x17, 0x11 }, { 0x18, 0x43 }, { 0x19, 0x00 }, { 0x1a, 0x4b }, { 0x32,
				0x09 }, { 0x4f, 0xca }, { 0x50, 0xa8 }, { 0x5a, 0x23 }, { 0x6d,
				0x00 }, { 0x39, 0x12 }, { 0x35, 0xda }, { 0x22, 0x1a }, { 0x37,
				0xc3 }, { 0x23, 0x00 }, { 0x34, 0xc0 }, { 0x36, 0x1a }, { 0x06,
				0x88 }, { 0x07, 0xc0 }, { 0x0d, 0x87 }, { 0x0e, 0x41 }, { 0x4c,
				0x00 }, { 0xff, 0x00 }, { 0xe0, 0x04 }, { 0xc0, 0x64 }, { 0xc1,
				0x4b }, { 0x86, 0x35 }, { 0x50, 0x89 }, { 0x51, 0xc8 }, { 0x52,
				0x96 }, { 0x53, 0x00 }, { 0x54, 0x00 }, { 0x55, 0x00 }, { 0x57,
				0x00 }, { 0x5a, 0x50 }, { 0x5b, 0x3c }, { 0x5c, 0x00 }, { 0xe0,
				0x00 }, { 0xff, 0xff }, };

const unsigned char OV2640_640x480_JPEG[][2] = { { 0xff, 0x00 }, { 0x11, 0x01 },
		{ 0x12, 0x00 }, { 0x17, 0x11 }, { 0x18, 0x75 }, { 0x32, 0x36 }, { 0x19,
				0x01 }, { 0x1a, 0x97 }, { 0x03, 0x0f }, { 0x37, 0x40 }, { 0x4f,
				0xbb }, { 0x50, 0x9c }, { 0x5a, 0x57 }, { 0x6d, 0x80 }, { 0x3d,
				0x34 }, { 0x39, 0x02 }, { 0x35, 0x88 }, { 0x22, 0x0a }, { 0x37,
				0x40 }, { 0x34, 0xa0 }, { 0x06, 0x02 }, { 0x0d, 0xb7 }, { 0x0e,
				0x01 }, { 0xff, 0x00 }, { 0xe0, 0x04 }, { 0xc0, 0xc8 }, { 0xc1,
				0x96 }, { 0x86, 0x3d }, { 0x50, 0x89 }, { 0x51, 0x90 }, { 0x52,
				0x2c }, { 0x53, 0x00 }, { 0x54, 0x00 }, { 0x55, 0x88 }, { 0x57,
				0x00 }, { 0x5a, 0xa0 }, { 0x5b, 0x78 }, { 0x5c, 0x00 }, { 0xd3,
				0x04 }, { 0xe0, 0x00 }, { 0xff, 0xff }, };

const unsigned char OV2640_Contrast[][2] = { { 0xff, 0x00 }, { 0x7c, 0x00 }, {
		0x7d, 0x04 }, { 0x7c, 0x07 }, { 0x7d, 0x20 }, { 0x7d, 0x28 }, { 0x7d,
		0x0c }, { 0x7d, 0x06 }, { 0xff, 0x00 }, { 0x7c, 0x00 }, { 0x7d, 0x04 },
		{ 0x7c, 0x09 }, { 0x7d, 0x40 }, { 0x7d, 0x00 }, };

void reset_ov2640() {
	/* Assert the camera STANDBY pin (active high)  */
	  HAL_GPIO_WritePin(GPIOG, DCMI_PWR_EN_Pin, GPIO_PIN_SET);

	  /* Assert the camera RSTI pin (active low) */
	  HAL_GPIO_WritePin(GPIOG, DCMI_RST_Pin, GPIO_PIN_RESET);

	  HAL_Delay(100);   /* RST and XSDN signals asserted during 100ms */

	  /* De-assert the camera STANDBY pin (active high) */
	  HAL_GPIO_WritePin(GPIOG, DCMI_PWR_EN_Pin, GPIO_PIN_RESET);

	  HAL_Delay(3);     /* RST de-asserted and XSDN asserted during 3ms */

	  /* De-assert the camera RSTI pin (active low) */
	  HAL_GPIO_WritePin(GPIOG, DCMI_RST_Pin, GPIO_PIN_SET);

	  HAL_Delay(3);     /* RST de-asserted during 3ms */

}

void ov2640_init(I2C_HandleTypeDef *hi2c2_b, DCMI_HandleTypeDef *hdcmi_b) {
	phi2c = hi2c2_b;
	phdcmi = hdcmi_b;

	reset_ov2640();

	//Software reset: reset all registers to default values

	sccb_write(0xff, 0x01);
	sccb_write(0x12, 0x80);
	HAL_Delay(100);


#ifdef DEBUG
	uint8_t pid; uint8_t ver; uint8_t pdata_read;
	//uint8_t reg_addr_test=0x04;

	//sccb_read(reg_addr_test, &pdata_read);
	//my_printf("Il registro: 0x%x ha il seguente valore 0x%x \r\n", reg_addr_test, pdata_read);

    sccb_read(0x0a, &pid);  // pid is 0x26 (0x76)
	sccb_read(0x0b, &ver);  // ver is 0x42 (0x73)
	my_printf("PID: 0x%x, VER: 0x%x \r\n", pid, ver);

#endif
	// Stop capturing
    ov2640_stop_dcmi();

}

void test_register (uint8_t reg_addr_test)
{
	uint8_t pdata_read;
	sccb_read(reg_addr_test, &pdata_read);
	my_printf("Il registro: 0x%x ha il seguente valore 0x%x \r\n", reg_addr_test, pdata_read);
}

void ov2640_conf(short opt) {

#ifdef DEBUG
	my_printf("Starting configuration \n");
#endif

	load_config(OV2640_CIF_INIT_REG_TBL);
	load_config(OV2640_JPEG);

	/*
	HAL_Delay(20);
	sccb_write(0xff, 0x01);
	HAL_Delay(20);
	sccb_write(0x15, 0x00); \\da attivare se il DVP è bypassato

	switch( opt )
	{
	case 0:
		load_config(OV2640_160x120_JPEG);
	    break;
	case 1:
		load_config(OV2640_320x240_JPEG);
	    break;
	case 2:
		load_config(OV2640_640x480_JPEG);
	    break;
	default:
		load_config(OV2640_320x240_JPEG);
	    break;

	}*/



#ifdef DEBUG
	my_printf("Finalize configuration \n");
#endif
}



void load_config(const unsigned char arr[][2]) {

	unsigned short i = 0;
	unsigned char flag=1;

	uint8_t reg_addr, data, data_read;

	while (1 && flag==1) {
		reg_addr = arr[i][0];
		data = arr[i][1];
		if (reg_addr == 0xff && data == 0xff) {
			break;
		}
		sccb_read(reg_addr, &data_read);

		sccb_write(reg_addr, data);

#ifdef DEBUG
		my_printf("sccb write: 0x%x 0x%x=>0x%x \r\n", reg_addr, data_read, data);
#endif

		sccb_read(reg_addr, &data_read);
	if (data != data_read) {
#ifdef DEBUG
			my_printf("-->sccb write failure: 0x%x 0x%x \r\n", reg_addr, data_read);
			flag=0;
#endif
		}
	else	i++;
	}
}

 int sccb_write(uint8_t reg_addr, uint8_t data) {

	 uint8_t buf[2] = { 0 };


	HAL_StatusTypeDef status;
	buf[0] = reg_addr;
	buf[1] = data;
	//status = HAL_I2C_Master_Transmit(phi2c, OV2640_DEVICE_WRITE_ADDRESS, buf, 2, 100);
	status=HAL_I2C_Mem_Write(phi2c, OV2640_DEVICE_WRITE_ADDRESS, (uint16_t)buf[0], I2C_MEMADD_SIZE_8BIT,&buf[1], 1, 100);
	if (status == HAL_OK) {
		return 1;
	} else {
		return 0;
	}
}

 int sccb_read(uint8_t reg_addr, uint8_t *pdata) {


	uint8_t buf[1] = { 0 };
 	HAL_StatusTypeDef status;
 	buf[0] = reg_addr;
 	status = HAL_I2C_Master_Transmit(phi2c, (uint16_t) 0x60, buf, 1, 100);
 	if (status == HAL_OK) {
 		//status = HAL_I2C_Master_Receive(phi2c, (uint16_t) 0x60, pdata, 1, 100); //provare a mettere 61
 	   status= HAL_I2C_Mem_Read(phi2c, 0x60, reg_addr, I2C_MEMADD_SIZE_8BIT, pdata, 1, 1000);

 		if (status == HAL_OK) {
 			return 1;
 		} else {
 			return 0;
 		}
 	} else {
 		return 1;
 	}
}

 void DCMI_SingleRandomWrite(uint8_t Addr, uint8_t Data)
 {
	 HAL_I2C_Mem_Write(phi2c, OV2640_DEVICE_WRITE_ADDRESS, (uint16_t)Addr, I2C_MEMADD_SIZE_8BIT, &Data, 1, 100);
 }

 uint8_t CAMERA_IO_Read(uint8_t Addr, uint8_t Reg)
 {
   uint8_t Value = 0;
   HAL_I2C_Mem_Read(phi2c, Addr, Reg, I2C_MEMADD_SIZE_8BIT, &Value, 1, 1000);
   return Value;
 }

void ov2640_stop_dcmi(void) {
	HAL_DCMI_Stop(phdcmi);
	HAL_Delay(30);
}

void ov2640_capture_snapshot(uint32_t buf_addr, int len) {
	//ov2640_stop_dcmi();
	HAL_DCMI_Start_DMA(phdcmi, DCMI_MODE_SNAPSHOT, buf_addr, len);
}