#include <Wire.h>
#include <SPI.h>
#include <ArduCAM.h>
#include "memorysaver.h"

#define SPI_SCK   SCK
#define SPI_MISO  MISO
#define SPI_MOSI  MOSI
#define SPI_RST   -1
#define SPI_DC    A1
#define SPI_ARDUCAM_CS   A0
#define SPI_TFT_CS       A2

ArduCAM myCAM( OV2640, SPI_ARDUCAM_CS );

typedef struct {
	uint32_t Width;
	uint32_t Height;
} IMAGE_INFO;

SPISettings settingsArduCam(4000000, MSBFIRST, SPI_MODE0);
SPISettings settingsST7789(24000000, MSBFIRST, SPI_MODE3);

#define LOCK_NONE    0
#define LOCK_ARDUCAM 1
#define LOCK_ST7789  3

void spi_lock(int lock) {
	static int status = -1;
	if (lock==LOCK_NONE) {
		SPI.endTransaction();
		status = -1;
		return;
	}
	if (lock!=status) {
		SPI.endTransaction();
		if (lock==LOCK_ARDUCAM) {
			SPI.beginTransaction(settingsArduCam);
		} else if (lock==LOCK_ST7789) {
			SPI.beginTransaction(settingsST7789);
		}
		status = lock;
	}
}

#define TFT_WIDTH   240
#define TFT_HEIGHT  240

uint16_t *color565;

#define HBYTE(u) ((u >> 8) & 0xFF)
#define LBYTE(u) (u & 0xFF)

// TFTにコマンドを送信
void tftSendCommand(uint8_t command) {
	digitalWrite(SPI_TFT_CS, LOW);
	digitalWrite(SPI_DC, LOW); // Command mode
	SPI.transfer(command);
}

// TFTにコマンド+1バイトデータを送信
void tftSendCommand1(uint8_t command, uint8_t data1) {
	digitalWrite(SPI_TFT_CS, LOW);
	digitalWrite(SPI_DC, LOW);  // Command mode
	SPI.transfer(command);
	digitalWrite(SPI_DC, HIGH); // Data mode
	SPI.transfer(data1);
	digitalWrite(SPI_TFT_CS, HIGH);
}

// TFTにコマンド+2バイトデータを送信
void tftSendCommand2(uint8_t command, uint8_t data1, uint8_t data2) {
	digitalWrite(SPI_TFT_CS, LOW);
	digitalWrite(SPI_DC, LOW);  // Command mode
	SPI.transfer(command);
	digitalWrite(SPI_DC, HIGH); // Data mode
	SPI.transfer(data1);
	SPI.transfer(data2);
	digitalWrite(SPI_TFT_CS, HIGH);
}

// TFTにコマンド+3バイトデータを送信
void tftSendCommand3(uint8_t command, uint8_t data1, uint8_t data2, uint8_t data3) {
	digitalWrite(SPI_TFT_CS, LOW);
	digitalWrite(SPI_DC, LOW);  // Command mode
	SPI.transfer(command);
	digitalWrite(SPI_DC, HIGH); // Data mode
	SPI.transfer(data1);
	SPI.transfer(data2);
	SPI.transfer(data3);
	digitalWrite(SPI_TFT_CS, HIGH);
}

// TFTにコマンド+4バイトデータを送信
void tftSendCommand4(uint8_t command, uint8_t data1, uint8_t data2, uint8_t data3, uint8_t data4) {
	digitalWrite(SPI_TFT_CS, LOW);
	digitalWrite(SPI_DC, LOW);  // Command mode
	SPI.transfer(command);
	digitalWrite(SPI_DC, HIGH); // Data mode
	SPI.transfer(data1);
	SPI.transfer(data2);
	SPI.transfer(data3);
	SPI.transfer(data4);
	digitalWrite(SPI_TFT_CS, HIGH);
}

void drawRGBBitmap(uint16_t x, uint16_t y, uint16_t *rgbs, uint16_t width, uint16_t height) {
	uint8_t h, l;
	width--;
	h = (uint8_t)(width>>8);
	l = (uint8_t)(width&0x00ff);
	tftSendCommand4(0x2A,x,y,h,l); // Colmun Address
	height--;
	h = (uint8_t)(height>>8);
	l = (uint8_t)(height&0x00ff);
	tftSendCommand4(0x2B,x,y,h,l); // Row Address
	digitalWrite(SPI_TFT_CS, LOW);
	digitalWrite(SPI_DC, LOW);  // Command mode
	SPI.transfer(0x2C);
	digitalWrite(SPI_DC, HIGH); // Data mode
	SPI.transfer(rgbs, width*height*2);
	digitalWrite(SPI_TFT_CS, HIGH);
	digitalWrite(SPI_DC, LOW); // Command mode
}

// 表示開始ライン設定
void dispStartLine(uint16_t y) {
	uint8_t yH = (y >> 8) & 0xFF ;
	uint8_t yL = y & 0xFF ;

	digitalWrite(SPI_TFT_CS, LOW);
	digitalWrite(SPI_DC, LOW);  // Command mode
	SPI.transfer(0x37);
	digitalWrite(SPI_DC, HIGH); // Data mode
	SPI.transfer(yH);
	SPI.transfer(yL);
	digitalWrite(SPI_TFT_CS, HIGH);
}

void init_tft() {

	spi_lock(LOCK_ST7789);

	// --- HARD Ware Reset
	if (SPI_RST >= 0) {
		digitalWrite(SPI_RST, HIGH);
		delay(500);                  // VDD goes high at start, pause for 500 ms
		digitalWrite(SPI_RST, LOW);  // Bring reset low
		delay(100);                  // Wait 100 ms
		digitalWrite(SPI_RST, HIGH); // Bring out of reset
		delay(500);                  // Wait 500 ms, more then 120 ms
	}
	// --- SOFT Ware Reset
	tftSendCommand(0x01) ;       // SOFTWARE RESET
	delay(50);

	// --- Initial Comands
	tftSendCommand(0x28) ;            // Display OFF
	delay(500);

	tftSendCommand(0x11) ;            // Sleep Out
	delay(500);
	tftSendCommand1(0x3A,0x05) ;      // 16Bit Pixel Mode
	delay(10);
	tftSendCommand1(0x36,B00000000) ; // MX MY MV ML RGB MH x x:縦向き１
	tftSendCommand2(0xB6,0x15,0x02) ; // フレームレート設定

	tftSendCommand(0x13) ;            // NomalDisplayMode
	tftSendCommand(0x21) ;            // Display Inversion Off
	tftSendCommand(0x29) ;            // Display ON

	tftSendCommand1(0x36,B11000000) ; // MX MY MV ML RGB MH x x：縦向き２

	dispStartLine(80);

	spi_lock(LOCK_NONE);
}

void show() {
	spi_lock(LOCK_ST7789);
	drawRGBBitmap(0,0,color565,240,240);
	spi_lock(LOCK_NONE);
}

void capture() {
	// put your main code here, to run repeatedly:
	uint8_t temp = 0xff, temp_last = 0;
	bool is_header = false;
	uint16_t *imgpt;

	temp = 0xff;
	myCAM.flush_fifo();
	myCAM.clear_fifo_flag();
	myCAM.start_capture();

	spi_lock(LOCK_ARDUCAM);

	while(1) {
		if (myCAM.get_bit(ARDUCHIP_TRIG, CAP_DONE_MASK)) {
			delay(50);
			uint8_t temp, temp_last;
			uint32_t length = 0;
			length = myCAM.read_fifo_length();
			if (length >= MAX_FIFO_SIZE ) {
				myCAM.clear_fifo_flag();
				spi_lock(LOCK_NONE);
				return;
			}
			if (length == 0 ) { //0 kb
				myCAM.clear_fifo_flag();
				spi_lock(LOCK_NONE);
				return;
			}
			myCAM.CS_LOW();
			myCAM.set_fifo_burst();//Set fifo burst mode

			SPI.transfer(0x00);
			char VH, VL;
			int y = 0, x = 0;
			int col = 0;
			imgpt = color565;
			for (y=0; y<240; y++) {
				imgpt += 240;
				for (x=0; x<320; x++) {
					VH = SPI.transfer(0x00);
					VL = SPI.transfer(0x00);
					if ((x>39)&&(x<280)) {
						imgpt--;
						*imgpt = VH|(VL<<8);
					}
					delayMicroseconds(12);
				}
				imgpt += 240;
			}
			myCAM.CS_HIGH();
			//Clear the capture done flag
			myCAM.clear_fifo_flag();
			break;
		} else {
			delay(100);
		}
	}
	spi_lock(LOCK_NONE);
}

void init_ArduCam() {

	uint8_t vid, pid;
	uint8_t temp;

	Serial1.println(F("ACK CMD ArduCAM Start! END"));

	//Reset the CPLD
	myCAM.write_reg(0x07, 0x80);
	delay(100);
	myCAM.write_reg(0x07, 0x00);
	delay(100);
	while(1){
		//Check if the ArduCAM SPI bus is OK
		myCAM.write_reg(ARDUCHIP_TEST1, 0x55);
		temp = myCAM.read_reg(ARDUCHIP_TEST1);
		if (temp != 0x55){
			Serial1.println(F("ACK CMD SPI interface Error! END"));
			delay(1000);continue;
		}else{
			Serial1.println(F("ACK CMD SPI interface OK. END"));break;
		}
	}

	while(1){
		//Check if the camera module type is OV2640
		myCAM.wrSensorReg8_8(0xff, 0x01);
		myCAM.rdSensorReg8_8(OV2640_CHIPID_HIGH, &vid);
		myCAM.rdSensorReg8_8(OV2640_CHIPID_LOW,  &pid);
		if ((vid != 0x26 ) && (( pid != 0x41 ) || ( pid != 0x42 ))){
			Serial1.println(F("ACK CMD Can't find OV2640 module! END"));
			delay(1000);continue;
		} else {
			Serial1.println(F("ACK CMD OV2640 detected. END"));break;
		}
	}

	myCAM.OV2640_set_JPEG_size(OV2640_320x240);  delay(1000);
	myCAM.set_format(BMP);
	myCAM.InitCAM();
	myCAM.wrSensorReg16_8(0x3818, 0x81);
	myCAM.wrSensorReg16_8(0x3621, 0xA7);
}

void setup() {

	color565 = (uint16_t*)malloc(240*240*2);
	if (!color565) {
		while(1);
	}

	Wire.begin();

	// set the CS as an output:
	pinMode(SPI_ARDUCAM_CS,OUTPUT);
	pinMode(SPI_TFT_CS,    OUTPUT);
	pinMode(SPI_DC,        OUTPUT);

	digitalWrite(SPI_ARDUCAM_CS, HIGH);
	digitalWrite(SPI_TFT_CS, HIGH);

	SPI.begin();
	init_ArduCam();
	init_tft();
}

void loop() {
	capture();
	show();
}