#include <pigpio.h>
#include <iostream>
#include <unistd.h>
#include <cstring>
#include <vector>

class Pixel {
public:
	Pixel(uint8_t red = 0, uint8_t green = 0, uint8_t blue = 0);
	uint8_t operator[](unsigned index);
private:
	uint8_t m_red;
	uint8_t m_green;
	uint8_t m_blue;
};

class Frame {
public:
	Frame(unsigned height = 64, unsigned width = 13);
	~Frame();
	std::vector<uint16_t> get_raw_data();
	void transform();
private:
	unsigned m_height;
	unsigned m_width;
	Pixel** m_data;	
	std::vector<uint16_t> m_raw_data;
};

Frame::Frame(unsigned height, unsigned width) : m_height(height), m_width(width)
{
	m_data = new Pixel*[height];
	for (int i = 0; i < height; i++) {
		m_data[i] = new Pixel[width];
	}
}

Frame::~Frame()
{
	for (int i = 0; i < m_height; i++) {
		delete[] m_data[i];
	}
	delete[] m_data;
}

std::vector<uint16_t> Frame::get_raw_data()
{
	return m_raw_data;
}

void Frame::transform()
{
	for (int x = 0; x < m_width; x++) {
		for (int i = 0; i < 24; i++) {
			uint16_t temp_data;
			for (int y = 0; y < m_height; y++) {
				temp_data += m_data[y][x][i] >> y;
			}
			m_raw_data.push_back(temp_data);
			temp_data = 0;
		}
	}
}

Pixel::Pixel(uint8_t red, uint8_t green, uint8_t blue) : m_red(red), m_green(green), m_blue(blue)
{
}

uint8_t Pixel::operator[](unsigned index)
{
	if (index < 8) {
		return m_red >> index;
	}
	else if (index < 16) {
		return m_green >> (index - 8);
	}
	else if (index < 24) {
		return m_blue >> (index - 16);
	}
}


using namespace std;

const uint8_t WIDTH = 40;
const uint8_t HEIGHT = 13;


uint16_t dataRed[40*24] = {0};
uint16_t dataGreen[40*24] = {0};
uint8_t dataRed2[40*24*2] = {0};
uint8_t dataGreen2[40*24*2] = {0};
uint8_t ures[40*24*2+4] = {0};
uint8_t teli[40*24*2+4] = {0};

void initData()
{
  for(size_t i = 0; i < 40; i++)
  {
    for(size_t j = 0; j < 24; j++)
    {
      dataRed[24*i+j] = 0;
      dataGreen[24*i+j] = 0;
    }    
    dataRed[24*i] = 0x0100;
    dataGreen[24*i+8] = 0x0100;
  }
  for(size_t i = 0; i < 40*24; i++)
  {
    dataRed2[2*i] = dataRed[i] >> 8;
    dataRed2[2*i + 1] = dataRed[i];
    dataGreen2[2*i] = dataGreen[i] >> 8;
    dataGreen2[2*i + 1] = dataGreen[i];
  }
  teli[0] = 0x55;
  teli[1] = 0x55;
  teli[2] = 0x00;
  teli[3] = 0x02;
  for(size_t i = 4; i < 40*24*2+4; i++)
  {
    if (i % 2 == 0)
    {
      teli[i] = 1;
    }
  }
  
}

uint8_t* addHeader(uint8_t* ptr, uint16_t duration)
{
  uint8_t* outData = new uint8_t[40*24*2 + 4];
  memcpy(outData + 4, ptr, 40*24*2);
  outData[0] = 0x55;
  outData[1] = 0x55;
  outData[2] = duration >> 8;
  outData[3] = duration;
  return outData;
}

void waitGPIO()
{
  int tmp;
  usleep(12000);
  while((tmp = gpioRead(26)) != 1)
  {
    usleep(1000);
    //cout << "GPIO26 = " << tmp << endl;
//    usleep(000);
  }
}

int main(int argc, char *argv[])
{
  
  initData();
  gpioInitialise();
  gpioSetMode(26, PI_INPUT);

  uint8_t* outR = addHeader(dataRed2, 1);
  uint8_t* outG = addHeader(dataGreen2, 1);
  uint32_t flags = (1 << 20);
  int spiHandle = spiOpen(0, 4000000, flags);
  if(spiHandle >= 0) cout << "SPI Opened" << endl;
  char buff[2] = {0, 1}; //ez lesz így 0x0001

  cout << "Reading GPIO" << endl;
  // while((tmp = gpioRead(26)) != 1)
  // {
  //   cout << "GPIO26 = " << tmp << endl;
  //   sleep(1);
  // }
  // cout << "GPIO OK" << endl;

  ures[0] = 0x55;
  ures[1] = 0x55;  
  ures[2] = 0;
  ures[3] = 2;
  uint32_t tmp = 4;
//  ures[40*24*2+3] = 0x55;
  int mode = 1;
  while(1)
  {
    spiWrite(spiHandle, (char*)ures, 40*24*2 + 4);
    waitGPIO();
    spiWrite(spiHandle, (char*)teli, 40*24*2 + 4);
    waitGPIO();
    ures[tmp] = mode;
    tmp += 2;
    tmp += 2;
    tmp += 2;
    tmp += 2;
    if (tmp >= 40*24*2)
    {
      tmp = 4;
      mode = (mode + 1) % 2;
    }
  }

  int cnt = 0;
  for(size_t i = 0; i < 40*24*2+4; i++)
  {
    if (outR[i] == 1)
      cnt++;
  }
  cout << cnt << endl;
  
  // spiWrite(spiHandle, outG, 40*24*2 + 4);
  // usleep(10000);
  delete outR;
  delete outG;
  
  return 0;
}