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

#define SPI_TFT_CS   A0
#define SPI_DC       A1
#define SPI_MISO   MISO
#define SPI_MOSI   MOSI
#define SPI_SCK     SCK
#define SPI_RST      A2
#define SPI_SD_SS    A3
#define SPI_BLK      A6
#define SWITCH_PIN   A7

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

#define CLK_PIN SCL
#define DAT_PIN SDA

SPIClass SDSPI(HSPI);

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 XpixelsPerM[4];
	uint8_t YpixelsPerM[4];
	uint8_t ColorsUsed[4];
	uint8_t ImportantColors[4];
} BMP_FORMAT;

typedef struct {
	uint32_t Width;
	uint32_t Height;
	uint16_t BitPerPixel;
	uint32_t FileSize;
	uint32_t DataOffset;
	uint32_t ImageSize;
	uint32_t ColorsUsed;
} IMAGE_INFO;

IMAGE_INFO *img;

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

PIXEL_INFO *RGBs   = NULL;
PIXEL_INFO *pixels = NULL;

uint16_t swap16(uint8_t *pt) {
    uint16_t ret;
    ret  = *pt;
    ret |= (*(pt+1)<<8)&0xff00;
    return ret;
}

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

uint32_t swap32(uint8_t *pt) {
	uint32_t ret;
	ret  = *pt;
	ret |= (*(pt+1)<< 8)&0x0000ff00;
	ret |= (*(pt+2)<<16)&0x00ff0000;
	ret |= (*(pt+3)<<24)&0xff000000;
	return ret;
}

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

int saveBmpFile(const char *fileName) {

	BMP_FORMAT *Bmp;
	PIXEL_INFO *imgpt = RGBs;
	char fname[20];
	static int fno = 0;

	fno++;
	sprintf(fname,"%s%d.bmp",fileName,fno);
	SDSPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI, SPI_SD_SS);
	pinMode(SPI_SD_SS, OUTPUT);

	if (!SD.begin(SPI_SD_SS, SDSPI, 20000000)) return 0;

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

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

	img->ImageSize = img->Width * img->Height * 3;

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

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

	int32_t x, y;
	int32_t pos = 0;

	uint8_t mod = (img->Width % 4) * 3; // RGB情報は画像横一列が4byteの倍数でなければならないための補正
	for(y=0; y < img->Height; y++){
		for(x=0; x < img->Width; x++,pos++,imgpt++){
			fileSD.write(&imgpt->B, 1);
			fileSD.write(&imgpt->G, 1);
			fileSD.write(&imgpt->R, 1);
		}
		if (mod>0) fileSD.write(0x00,mod);
	}
	fileSD.close();

	SD.end();
	SDSPI.end();
	free(Bmp);
	return 1;
}

void capture() {

	long magic = 0,i,k;
	int v = 0, u = 0, y = 0;
	int r = 0, g = 0, b = 0;
	unsigned char d;
	PIXEL_INFO *pt = pixels;

	while((magic & 0xffffff) != 0xaa55ff) {
		magic <<= 1;
		if(readbit()) magic |= 0x01;
	}
	for(i=0; i<29*8; i++) readbit();

	for( k=0; k < img->Width*img->Height*2; k++) {
		d = 0;
		for (i=0; i<8; i++) {
			d <<= 1;
			if(readbit()) d |= 0x1;
		}
		if((k & 0b11) == 0b00) { // 1st Byte
			v = d - 128;
		} else if((k & 0b11) == 0b10) { // 3rd Byte
			u = d - 128;
		} else if((k & 0b11) == 0b01 || (k & 0b11) == 0b11) { // Y0 or Y1
			y = d;
			r = u + y;
			g = 0.98 * y - 0.53 * u - 0.19 * v;
			b = v + y;
			if(r > 255) r = 255;
			if(r <   0) r = 0;
			if(g > 255) g = 255;
			if(g <   0) g = 0;
			if(b > 255) b = 255;
			if(b <   0) b = 0;

			pt->R = r;
			pt->G = g;
			pt->B = b;
			pt++;
		}
	}

	pt = pixels;
	for( int y=0; y < img->Height; y++) {
		long pos = img->Width * (y + 1) -1;
		for( int x=0; x < img->Width; x++, pos--, pt++) {
			RGBs[pos].R = pt->R;
			RGBs[pos].G = pt->G;
			RGBs[pos].B = pt->B;
		}
	}
}

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

	img = (IMAGE_INFO *)malloc(sizeof(IMAGE_INFO));
	img->Width  = 96;
	img->Height = 72;

	RGBs   = (PIXEL_INFO *)malloc(img->Width * img->Height * sizeof(PIXEL_INFO));
	pixels = (PIXEL_INFO *)malloc(img->Width * img->Height * sizeof(PIXEL_INFO));

	ledcSetup(0,12800,8); 
	ledcAttachPin(SPI_BLK,0);
	ledcWrite(0,64);

	tft.init(135,240);
	tft.setRotation(0);
	tft.fillScreen(ST77XX_BLACK);
}

void loop() {
	int x, y;
	long cnt;

	capture();
	for(y=0, cnt=0; y < img->Height; y++) {
		for(x=img->Width - 1; x >= 0; x--, cnt++) {
			tft.drawPixel(x+19,y+84,tft.color565(RGBs[cnt].R,RGBs[cnt].G,RGBs[cnt].B));
		}
	}
	if (!digitalRead(SWITCH_PIN)) {
		saveBmpFile("/treva");
		tft.init(135,240);
		tft.setRotation(0);
		tft.fillScreen(ST77XX_BLACK);
	}
}