// ==========================================================================
// camera.c
// (c) 2021, Aurel Dumitru
//
// Description:
// Camera operations (AR0135)
// =========================================================================


#include "camera.h"
#include "utils.h"
#include "devstatus.h"
#include "calibrations.h"


#define CAMERA_I2C_WRITE_ADDR 	0x20
#define CAMERA_I2C_READ_ADDR 	0x21

uint16_t Camera_Green[8];
uint16_t Camera_Red[8];
uint16_t Camera_Blue[8];

const uint16_t Camera_InitReg[][2] =
{
	// PLL and Clocks settings
//	{0x3028, 0x0010}, // ROW_SPEED (phase of PIXCLK); Default 0x10 (falling edge of PIXCLK coincides DOUT change)
	{0x302E, 0x0002}, // PRE_PLL_CLK_DIV (N); Default 0x02  ----> EXTCLK = 16 MHz -> PREPLL = 8MHz
	{0x3030, 0x0032}, // PLL_MULTIPLIER (M); Default 0x2C   ----> VCO = 8*50 = 400MHz
	{0x302C, 0x0001}, // VT_SYS_CLK_DIV (P1); Default 0x01  ----> SYS = 400MHz
	{0x302A, 0x002C}, // VT_PIX_CLK_DIV (P2); Default 0x08  ----> PIX_CLK = 14.29MHz (for 0x002C is 9.1MHz)

//	{0x3032, 0x0000}, // DIGITAL_BINNING; Default 0 (vertical and horizontal binning)
	{0x30B0, 0x0000}, // DIGITAL_TEST; Default 0x480 (0x30B0[10] = 0 (disable short line length of 1388); 0x30B0[7] = 0 (color CFA))
	{0x301A, 0x1190}, // RESET_REGISTER; Default 0x10D8
					  // 0x301A[12] = HiSPi off, 0x301A[11] = disable force PLL on no matter sensor state
	  	  	  	  	  // 0x301A[8] = The input buffers (OUTPUT_ENABLE_N, TRIGGER and STANDBY) are enabled
					  // 0x301A[7] = parallel data interface enabled, 0x301A[4] = standby at EOF

//	{0x3046, 0x0000}, // FLASH; Default 0x0000 (flash disabled); 0x100 for flash enabled

//	{0x300C, 0x056C}, // LINE_LENGTH_PCK; Default 0x056C (minimum allowed)
//	{0x300A, 0x03DE}, // FRAME_LENGTH_LINES; Default 0x3DE (990)

//	{0x3012, 0x0064}, // COARSE_INTEGRATION_TIME; Default 100 (multiple of LINE_LENGTH_PCK time); we set 0x001A
//	{0x3014, 0x0000}, // FINE_INTEGRATION_TIME; Default 0 (multiple of PIXCLK)
//	{0x30A6, 0x0001}, // Y_ODD_INC; Default 1 (row skip factor)

//	{0x3040, 0x0000}, // READ_MODE; Default 0
//	{0x3064, 0x1982}, // EMBEDDED_DATA_CTRL; Default 0x1982 (exclude embedded data and stat 0x1802)

//	{0x30D4, 0xE007}, // COLUMN CORRECTION; Default=0xE007 (enabled - requires 8 frames after power up)

//	{0x3100, 0x0000}, // AE_CTRL_REG; Default 0 (disable auto exposure)
//	{0x305E, 0x0020}, // GLOBAL_GAIN; Default 0x20
//	{0x3056,     48}, // GREEN1_GAIN
//	{0x3058,     86}, // BLUE_GAIN
//	{0x305A,     86}, // RED_GAIN
//	{0x305C,     48}, // GREEN2_GAIN

	{0x3058,      48}, // BLUE_GAIN	  48 = 1.5x
	{0x305A,      48}, // RED_GAIN    48 = 1.5x
	{0x3056,      32}, // GREEN1_GAIN 32 = 1x
	{0x305C,      32}, // GREEN2_GAIN 32 = 1x
	{0x30B0, 0b10<<4}, // ANALOG GAIN    = 4x

	{0x3012,      10}, // COARSE_INTEGRATION_TIME (multiple of LINE_LENGTH_PCK time)
	{0x3014,       0}, // FINE_INTEGRATION_TIME   (multiple of PIXCLK time)

	{0xFFFF,  0xFFFF}
};

uint16_t Camera_ExposureGainReg[][2] =
{
	{0x3012,      10}, // COARSE_INTEGRATION_TIME (multiple of LINE_LENGTH_PCK time)
	{0x3014,       0}, // FINE_INTEGRATION_TIME   (multiple of PIXCLK time)
	{0xFFFF,  0xFFFF}
};

uint8_t		Camera_FrameRcvFlag;


uint32_t Camera_SetRegisterCamera(void)
{
	uint32_t I;
	uint8_t Data[2];

	for (I=0; Camera_InitReg[I][0] != 0xFFFF; I++)
	{
		Data[0] = (uint8_t)(Camera_InitReg[I][1]>>8);
		Data[1] = (uint8_t)(Camera_InitReg[I][1]);
		if (HAL_I2C_Mem_Write(&hi2c4, CAMERA_I2C_WRITE_ADDR, Camera_InitReg[I][0], I2C_MEMADD_SIZE_16BIT, Data, 2, 10) != HAL_OK) return 0;
	}
	return 1;
}


void Camera_SetRegisterExposure(uint32_t CoarseTime, uint32_t FineTime)
{
	uint32_t I;
	uint8_t Data[2];

	Camera_ExposureGainReg[0][1] = CoarseTime;
	Camera_ExposureGainReg[1][1] = FineTime;
	for (I=0; Camera_ExposureGainReg[I][0] != 0xFFFF; I++)
	{
		Data[0] = (uint8_t)(Camera_ExposureGainReg[I][1]>>8);
		Data[1] = (uint8_t)(Camera_ExposureGainReg[I][1]);
		HAL_I2C_Mem_Write(&hi2c4, CAMERA_I2C_WRITE_ADDR, Camera_ExposureGainReg[I][0], I2C_MEMADD_SIZE_16BIT, Data, 2, 10);
	}
}


void Camera_SetWindowCamera(void)
{
	uint8_t Data[2];
	uint16_t Tmp;

	Tmp = 482 - (C_CameraVerticalPixels + 4)/2;
	Data[0] = (uint8_t)(Tmp>>8);
	Data[1] = (uint8_t)(Tmp);
	HAL_I2C_Mem_Write(&hi2c4, CAMERA_I2C_WRITE_ADDR, 0x3002, I2C_MEMADD_SIZE_16BIT, Data, 2, 10);
	Tmp += C_CameraVerticalPixels + 4 - 1;
	Data[0] = (uint8_t)(Tmp>>8);
	Data[1] = (uint8_t)(Tmp);
	HAL_I2C_Mem_Write(&hi2c4, CAMERA_I2C_WRITE_ADDR, 0x3006, I2C_MEMADD_SIZE_16BIT, Data, 2, 10);

	Tmp = 642 - (C_CameraHorizontalPixels + 4)/2;
	Data[0] = (uint8_t)(Tmp>>8);
	Data[1] = (uint8_t)(Tmp);
	HAL_I2C_Mem_Write(&hi2c4, CAMERA_I2C_WRITE_ADDR, 0x3004, I2C_MEMADD_SIZE_16BIT, Data, 2, 10);
	Tmp +=  C_CameraHorizontalPixels + 4 - 1;
	Data[0] = (uint8_t)(Tmp>>8);
	Data[1] = (uint8_t)(Tmp);
	HAL_I2C_Mem_Write(&hi2c4, CAMERA_I2C_WRITE_ADDR, 0x3008, I2C_MEMADD_SIZE_16BIT, Data, 2, 10);
}

void Camera_ActivateFlash(uint32_t Activate)
{
	uint8_t Data[2];
	Data[0] = Activate;
	Data[1] = 0x00;
	HAL_I2C_Mem_Write(&hi2c4, CAMERA_I2C_WRITE_ADDR, 0x3046, I2C_MEMADD_SIZE_16BIT, Data, 2, 10);
}

void Camera_DisableEmbeddedStatistics(void)
{
	uint8_t Data[2];
	Data[0] = 0x18;
	Data[1] = 0x02;
	HAL_I2C_Mem_Write(&hi2c4, CAMERA_I2C_WRITE_ADDR, 0x3064, I2C_MEMADD_SIZE_16BIT, Data, 2, 10);
}

void Camera_Init(void)
{
	HAL_GPIO_WritePin(CAMERA_RESET_GPIO_Port, CAMERA_RESET_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(CAMERA_STBY_GPIO_Port,  CAMERA_STBY_Pin,  GPIO_PIN_RESET);
	HAL_Delay(2);  // reset low min 1ms
	HAL_GPIO_WritePin(CAMERA_RESET_GPIO_Port, CAMERA_RESET_Pin, GPIO_PIN_SET);
	HAL_Delay(12); // camera initialization min 160000 CLK = 10ms;
	DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] = DEVSTATUS_CAMERA_OK;
	if (Camera_SetRegisterCamera() == 0)
	{
		DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] = DEVSTATUS_CAMERA_COM_FAIL;
		return;
	}
	Camera_SetWindowCamera();
	HAL_Delay(2);  // PLL lock time min 1ms
	HAL_DCMI_RegisterCallback(&hdcmi, HAL_DCMI_FRAME_EVENT_CB_ID, Camera_EndFrameIsr);

	Camera_FrameRcvFlag = 0;
	HAL_DCMI_Start_DMA(&hdcmi,
						DCMI_MODE_CONTINUOUS,
						(uint32_t)Utils_GetForegroundImgStatPtr(),
						(C_CameraVerticalPixels+4+4)*(C_CameraHorizontalPixels+4)/2);

	// Generate trigger for at least 8 frames (for column correction)
	HAL_GPIO_WritePin(CAMERA_TRIGGER_GPIO_Port, CAMERA_TRIGGER_Pin, GPIO_PIN_SET);
	uint32_t Time = HAL_GetTick();
	uint32_t FrameCnt = 10;
	while (((HAL_GetTick() - Time) < 2000) && (FrameCnt))
		if (Camera_GetFrameRcvFlag())
			FrameCnt--;
	HAL_GPIO_WritePin(CAMERA_TRIGGER_GPIO_Port, CAMERA_TRIGGER_Pin, GPIO_PIN_RESET);
	HAL_DCMI_Stop(&hdcmi);
	if (FrameCnt != 0)
		DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] = DEVSTATUS_CAMERA_PICTURE_FAIL;
}

void Camera_StartDmaGetFrame(uint32_t* Dest)
{
	Camera_FrameRcvFlag = 0;
	HAL_DCMI_Start_DMA(&hdcmi,
						DCMI_MODE_SNAPSHOT,
						(uint32_t)Dest,
						(C_CameraVerticalPixels+4+4)*(C_CameraHorizontalPixels+4)/2);

	// Generate 5ms trigger
	HAL_GPIO_WritePin(CAMERA_TRIGGER_GPIO_Port, CAMERA_TRIGGER_Pin, GPIO_PIN_SET);
	uint32_t Time = HAL_GetTick();
	while ((HAL_GetTick() - Time) < 5);
	HAL_GPIO_WritePin(CAMERA_TRIGGER_GPIO_Port, CAMERA_TRIGGER_Pin, GPIO_PIN_RESET);
}

void Camera_StartDmaGetFrameNoStatistics(uint32_t* Dest)
{
	Camera_FrameRcvFlag = 0;
	HAL_DCMI_Start_DMA(&hdcmi,
						DCMI_MODE_SNAPSHOT,
						(uint32_t)Dest,
						(C_CameraVerticalPixels+4)*(C_CameraHorizontalPixels+4)/2);

	// Generate 5ms trigger
	HAL_GPIO_WritePin(CAMERA_TRIGGER_GPIO_Port, CAMERA_TRIGGER_Pin, GPIO_PIN_SET);
	uint32_t Time = HAL_GetTick();
	while ((HAL_GetTick() - Time) < 5);
	HAL_GPIO_WritePin(CAMERA_TRIGGER_GPIO_Port, CAMERA_TRIGGER_Pin, GPIO_PIN_RESET);
}

void Camera_EndFrameIsr(DCMI_HandleTypeDef * hdcmi)
{
	Camera_FrameRcvFlag = 1;
}


uint8_t Camera_GetFrameRcvFlag(void)
{
	uint8_t Flag = Camera_FrameRcvFlag;
	if (Flag)
		Camera_FrameRcvFlag = 0;
	return Flag;
}


void Camera_EnterStandby()
{
	HAL_GPIO_WritePin(CAMERA_STBY_GPIO_Port,  CAMERA_STBY_Pin,  GPIO_PIN_SET);
	Utils_DelayMicroseconds(100);

	GPIO_InitTypeDef GPIO_InitStruct;
	GPIO_InitStruct.Pin = CAMERA_EXTCLK_Pin;
	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	HAL_GPIO_Init(CAMERA_EXTCLK_GPIO_Port, &GPIO_InitStruct);
}

void Camera_ExitStandby()
{
	/*Configure GPIO pin : CAMERA_EXTCLK_Pin */
	GPIO_InitTypeDef GPIO_InitStruct;
	GPIO_InitStruct.Pin = CAMERA_EXTCLK_Pin;
	GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
	GPIO_InitStruct.Alternate = GPIO_AF0_MCO;
	HAL_GPIO_Init(CAMERA_EXTCLK_GPIO_Port, &GPIO_InitStruct);
	Utils_DelayMicroseconds(100);

	HAL_GPIO_WritePin(CAMERA_STBY_GPIO_Port,  CAMERA_STBY_Pin,  GPIO_PIN_RESET);
	Utils_DelayMicroseconds(1100);
}


void Camera_AdaptExposure(uint32_t Type)
{
	uint16_t FineTime   = 0;
	uint16_t CoarseTime = 10;

	if ((C_DeactivateAutoGainExposure == 0) && (Type))
	{
		double MeanFloat, MeanFloatInt;
		uint16_t* MeanPtr	= (uint16_t*)Utils_GetForegroundImgPtr() + (C_CameraVerticalPixels+4+1)*(C_CameraHorizontalPixels+4) + 2;
		int32_t Mean = (*MeanPtr++)<<8;
		Mean += ((*MeanPtr)>>2) - 166;
		Mean  = __USAT(Mean, 24);
		MeanFloat = 10.0 - ((double)Mean)/512;
		FineTime   = (uint16_t)(1388.0*(modf(MeanFloat, &MeanFloatInt)));
		CoarseTime = (uint16_t)MeanFloatInt;
		if (FineTime > 646)
		{
			if (FineTime <= 1000)	 FineTime = 646;
			else					{FineTime = 0; CoarseTime++;}
		}
	}
	// Set camera new exposure
	Camera_SetRegisterExposure(CoarseTime, FineTime);
}
