#include <SPI.h>
#include <SD.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>

#define SPI_TFT_CS   A2
#define SPI_DC       A1
#define SPI_MISO   MISO
#define SPI_MOSI   MOSI
#define SPI_SCK     SCK
#define SPI_TFT_RST  A0
#define SPI_TFT_BL   -1
#define SPI_SD_CS    A3
#define SWITCH_PIN   A7

Adafruit_ST7789 tft = Adafruit_ST7789(SPI_TFT_CS, SPI_DC, SPI_MOSI, SPI_SCK, SPI_TFT_RST);

#define CLK_PIN SCL
#define DAT_PIN SDA

long DMRC1_WIDTH  = 352;
long DMRC1_HEIGHT = 288;
int  TFT_WIDTH    = 240;
int  TFT_HEIGHT   = 240;

typedef struct {
	uint8_t Signature[2];   // 'BM'
	uint8_t FileSize[4];
	uint8_t reserved1[4];   // unused (=0)
	uint8_t DataOffset[4];
	uint8_t Size[4];        // Size of InfoHeader =40 
	uint8_t Width[4];
	uint8_t Height[4];
	uint8_t Planes[2];      // always 1
	uint8_t BitPerPixel[2];
	uint8_t Compression[4];
	uint8_t ImageSize[4];
	uint8_t XRGBsPerM[4];
	uint8_t YRGBsPerM[4];
	uint8_t ColorsUsed[4];
	uint8_t ImportantColors[4];
} BMP_FORMAT;

typedef struct {
	uint8_t  R;
	uint8_t  G;
	uint8_t  B;
} PIXEL_INFO;

BMP_FORMAT *Bmp  = NULL;
PIXEL_INFO *RGBs = NULL;

SPISettings settingsST7789(24000000, MSBFIRST, SPI_MODE3);

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

void tftSendCommand(uint8_t command) {
	digitalWrite(SPI_TFT_CS, LOW);
	digitalWrite(SPI_DC, LOW); // Command mode
	SPI.transfer(command);
}

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);
}

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);
}

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 drawColor565(uint16_t *t565s, uint16_t width, uint16_t height)
{
	uint8_t h, l;
	uint16_t x, y;

	SPI.beginTransaction(settingsST7789);

	x = 0;
	y = 0;
	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(t565s, width*height*2);
	digitalWrite(SPI_TFT_CS, HIGH);
	digitalWrite(SPI_DC, LOW); // Command mode

	SPI.endTransaction();
}

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.beginTransaction(settingsST7789);

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

	// --- Initial Comands
	tftSendCommand(0x28) ;            // Display OFF
	debug_delay(50);

	tftSendCommand(0x11) ;            // Sleep Out
	debug_delay(50);
	tftSendCommand1(0x3A,0x05) ;      // 16Bit Pixel Mode
	debug_delay(10);
	tftSendCommand1(0x36,B00000000) ; // MX MY MV ML RGB MH x x:縦向き１
	tftSendCommand2(0xB6,0x15,0x02) ; // Frame Rate

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

	// tftSendCommand1(0x36,B10100000) ;// MX MY MV ML RGB MH x x: Horizontal 1
	// tftSendCommand1(0x36,B01100000) ; // MX MY MV ML RGB MH x x：Horizontal 2
	// tftSendCommand1(0x36,B00000000) ; // MX MY MV ML RGB MH x x：Vertical 1
	tftSendCommand1(0x36,B11000000) ; // MX MY MV ML RGB MH x x：Vertical 2

	dispStartLine(82);

	SPI.endTransaction();
}

void debug_delay(uint32_t msec) {
	portTickType delay_ms = msec / portTICK_RATE_MS;
	vTaskDelay(delay_ms);
}

void short2byte(uint8_t *pt, uint16_t val) {
	*pt     = (uint8_t)val      &0x00ff;
	*(pt+1) = (uint8_t)((val>>8)&0x00ff);
}

void long2byte(uint8_t *pt, uint32_t val) {
	*pt     = (uint8_t)val       &0x000000ff;
	*(pt+1) = (uint8_t)((val>> 8)&0x000000ff);
	*(pt+2) = (uint8_t)((val>>16)&0x000000ff);
	*(pt+3) = (uint8_t)((val>>24)&0x000000ff);
}

int readbit() {
	int a;
	digitalWrite(CLK_PIN, HIGH);
	a = digitalRead(DAT_PIN);
	digitalWrite(CLK_PIN, LOW);
	if(a){
		return 1;
	} else {
		return 0;
	}
}

boolean saveBmpFile(const char *path)
{
	long pos;
	char fname[32];
	static int fno = 0;

	digitalWrite(SPI_SD_CS, LOW);

	SPI.setFrequency(16000000); // 16MHz

	if (!SD.begin(SPI_SD_CS, SPI)) return false;

	while (1) {
		sprintf(fname,"%s/%d.bmp",path,++fno);
		if (!SD.exists(fname)) break;
	}

	File fileSD = SD.open(fname, "wb");
	if (!fileSD) return false;

	if (!Bmp) Bmp = (BMP_FORMAT *)malloc(sizeof(BMP_FORMAT));
	memset(Bmp, 0x00, sizeof(BMP_FORMAT));

	memcpy(&Bmp->Signature,(uint8_t*)"BM",2);
	long2byte(Bmp->FileSize,     sizeof(BMP_FORMAT) + DMRC1_WIDTH * DMRC1_HEIGHT * 3);
	long2byte(Bmp->DataOffset,   sizeof(BMP_FORMAT));
	long2byte(Bmp->Size,         40);
	long2byte(Bmp->Width,        DMRC1_WIDTH);
	long2byte(Bmp->Height,       DMRC1_HEIGHT);
	short2byte(Bmp->Planes,      1);
	short2byte(Bmp->BitPerPixel, 24);
	long2byte(Bmp->ImageSize,    DMRC1_WIDTH * DMRC1_HEIGHT * 3);
	long2byte(Bmp->ColorsUsed,   0);

	fileSD.write((uint8_t*)Bmp, sizeof(BMP_FORMAT));

	// Correction for RGB information, as one horizontal row
	//  of the image must be a multiple of 4 bytes
	uint8_t mod = (DMRC1_WIDTH % 4) * 3;

	for(int y = DMRC1_HEIGHT - 1; y >= 0; y--){
		pos = DMRC1_WIDTH * y;
		for(int x=0; x < DMRC1_WIDTH; x++,pos++){
			fileSD.write(&RGBs[pos].B, 1);
			fileSD.write(&RGBs[pos].G, 1);
			fileSD.write(&RGBs[pos].R, 1);
		}
		if (mod>0) fileSD.write(0x00,mod);
	}
	fileSD.close();

	SD.end();

	digitalWrite(SPI_SD_CS, HIGH);

	return true;
}

boolean capture()
{
	unsigned short marker = 0x0;
	long i,k;
	int bit;
	float u, y0, v, y1;
	unsigned char r = 0, g = 0, b = 0;
	unsigned char d;
	long pos = 0;
	long pos_max = DMRC1_WIDTH * DMRC1_HEIGHT;

	// find start marker
	while((marker & 0xffff) != 0xaa55) {
		marker <<= 1;
		if(readbit()) marker |= 0x01;
	}

	// skip 28 bytes
	for(i=0; i<28*8; i++) readbit();

	// check data start marker
	marker = 0; bit = 0;
	while (1) {
		if(readbit()) marker |= 0x01;
		if ((++bit)<16) {
			marker <<= 1;
		} else {
			break;
		}
	}
	if (marker != 0x55aa) {
		return false;
	}

	for( k=0; k < DMRC1_WIDTH * DMRC1_HEIGHT * 4; k++) {
		d = 0;
		for (i=0; i<8; i++) {
			d <<= 1;
			if(readbit()) d |= 0x1;
		}
		switch (k%4) {
			case 0:
				u = d;
				break;
			case 1:
				y0 = d;
				break;
			case 2:
				v = d;
				break;
			case 3:
				y1 = d;

				r = y0 +                   1.140*(v - 128);
				g = y0 - 0.394*(u - 128) - 0.581*(v - 128);
				b = y0 + 2.032*(u - 128);

				if(255 < r) r = 255;
				if(r < 0) r = 0;
				if(255 < g) g = 255;
				if(g < 0) g = 0;
				if(255 < b) b = 255;
				if(b < 0) b = 0;
				if (pos < pos_max) {
					RGBs[pos].R = r;
					RGBs[pos].G = g;
					RGBs[pos].B = b;
				}
				r = y1 +                   1.140*(v - 128);
				g = y1 - 0.394*(u - 128) - 0.581*(v - 128);
				b = y1 + 2.032*(u - 128);

				if(255 < r) r = 255;
				if(r < 0) r = 0;
				if(255 < g) g = 255;
				if(g < 0) g = 0;
				if(255 < b) b = 255;
				if(b < 0) b = 0;
				if (pos < pos_max) {
					RGBs[pos+1].R = r;
					RGBs[pos+1].G = g;
					RGBs[pos+1].B = b;
					pos+=2;
				}
				break;
		}
	}
	return true;
}

void show()
{
	uint16_t from_w, to_w, from_h, to_h;
	uint16_t color565;
	long pos1 = 0;
	long pos2 = 0;

	from_h = (DMRC1_HEIGHT - TFT_HEIGHT)/2;
	to_h   =  from_h + TFT_HEIGHT;
	from_w = (DMRC1_WIDTH - TFT_WIDTH)/2;
	to_w   =  from_w + TFT_WIDTH;

	uint16_t *color565s = (uint16_t*)malloc(TFT_WIDTH * TFT_HEIGHT * 2);
	if (color565s) {
		for (int y = 0; y < DMRC1_HEIGHT; y++) {
			for (int x = 0; x < DMRC1_WIDTH; x++) {
				if ((y >= from_h)&&(y < to_h)&&(x >= from_w)&&(x < to_w)) {
					color565 = ((RGBs[pos1].R >> 3)<<11) | ((RGBs[pos1].G >> 2)<<5) | (RGBs[pos1].B >> 3);
					color565s[pos2] = ((color565>>8)&0x00FF)|((color565<<8)&0xFF00);
					pos2++;
				}
				pos1++;
			}
		}
		drawColor565(color565s,TFT_WIDTH,TFT_HEIGHT);
		free(color565s);
	}
}

void setup()
{
	pinMode(CLK_PIN,     OUTPUT);
	pinMode(DAT_PIN,     INPUT);

	pinMode(SPI_SD_CS,   OUTPUT);
	pinMode(SPI_TFT_CS,  OUTPUT);
	pinMode(SPI_DC,      OUTPUT);
	pinMode(SPI_TFT_RST, OUTPUT);
	pinMode(SWITCH_PIN,  INPUT );
	digitalWrite(SPI_SD_CS, HIGH);

	RGBs = (PIXEL_INFO *)malloc(DMRC1_WIDTH * DMRC1_HEIGHT * 3);
	if (!RGBs) while(1);

	if (SPI_TFT_BL != -1) {
		ledcSetup(0,12800,8); 
		ledcAttachPin(SPI_TFT_BL,0);
		ledcWrite(0,64);
	}

	SPI.begin();

	init_tft();
}

void loop()
{
	if ( capture() ) {
		show();
		if (!digitalRead(SWITCH_PIN)) {
			saveBmpFile("/dmrc1");
		}
	}
}