#include "tsc2046.h"

#define CMD_X 0xD1
#define CMD_Y 0x91
#define CMD_ENABLE_IRQ 0x80

#define RESCALE_FACTOR 1000000
#define EE_MEM_ADDR 400

const constexpr auto dispWidth = 320;
const constexpr auto dispHeight = 240;

const constexpr auto xd1 = (50 * dispWidth) / 100;
const constexpr auto xd2 = (90 * dispWidth) / 100;
const constexpr auto xd3 = (10 * dispWidth) / 100;

const constexpr auto yd1 = (10 * dispHeight) / 100;
const constexpr auto yd2 = (50 * dispHeight) / 100;
const constexpr auto yd3 = (90 * dispHeight) / 100;

TSC2046::TSC2046(EEPROM* mem, PinName MOSI, PinName MISO, PinName SCK, PinName CS, PinName INT)
{
    tpPort = new SPI(MOSI, MISO, SCK);
    tpCS = new DigitalOut(CS);
    tpInt = new DigitalIn(INT, PullUp);

    eeprom = mem;
    eeprom->read(EE_MEM_ADDR, (uint8_t*)&touchCoeff, sizeof(touchCoeff));
    if (touchCoeff.touchIsCalibrated > 2)
        touchCoeff.touchIsCalibrated = false;

    readValue(CMD_ENABLE_IRQ); // Enable IRQ
}

void TSC2046::GetRawXY(uint16_t* X, uint16_t* Y)
{
    readValue(CMD_X); // Dummy read - disable PenIRQ
    *X = readValue(CMD_X); // Read X-Value

    readValue(CMD_Y); // Dummy read - disable PenIRQ
    *Y = readValue(CMD_Y); // Read Y-Value

    readValue(CMD_ENABLE_IRQ); // Enable IRQ
}
void TSC2046::GetXY(int32_t* X, int32_t* Y)
{
    uint16_t x, y;
    if (DetectTouch()) {
        GetRawXY(&x, &y);
        *X = (touchCoeff.cali_A * x + touchCoeff.cali_B * y + touchCoeff.cali_C) / RESCALE_FACTOR;
        *Y = (touchCoeff.cali_D * x + touchCoeff.cali_E * y + touchCoeff.cali_F) / RESCALE_FACTOR;
    }
}

uint16_t TSC2046::readValue(uint16_t port)
{
    static char txbuf[3] = { 0 };
    static char rxbuf[3] = { 0 };

    txbuf[0] = port;
    tpCS->write(0);
    tpPort->write(txbuf, 3, rxbuf, 3);
    tpCS->write(1);

    return ((uint16_t)rxbuf[1] << 5) | (rxbuf[2] >> 3);
}

bool TSC2046::DetectTouch()
{
    return !tpInt->read();
}

void TSC2046::coeffCalc()
{

    double temp1, temp2;
    double cal_A = 0.0, cal_B = 0.0, cal_C = 0.0, cal_D = 0.0, cal_E = 0.0, cal_F = 0.0;

    //A
    temp1 = ((double)xd1 * ((double)yt2 - (double)yt3)) + ((double)xd2 * ((double)yt3 - (double)yt1)) + ((double)xd3 * ((double)yt1 - (double)yt2));
    temp2 = ((double)xt1 * ((double)yt2 - (double)yt3)) + ((double)xt2 * ((double)yt3 - (double)yt1)) + ((double)xt3 * ((double)yt1 - (double)yt2));
    cal_A = temp1 / temp2;
    touchCoeff.cali_A = (int32_t)((double)cal_A * RESCALE_FACTOR);

    //B
    temp1 = (cal_A * ((double)xt3 - (double)xt2)) + (double)xd2 - (double)xd3;
    temp2 = (double)yt2 - (double)yt3;
    cal_B = temp1 / temp2;
    touchCoeff.cali_B = (int32_t)((double)cal_B * RESCALE_FACTOR);

    //C
    cal_C = (double)xd3 - (cal_A * (double)xt3) - (cal_B * (double)yt3);
    touchCoeff.cali_C = (int32_t)(cal_C * RESCALE_FACTOR);

    //D
    temp1 = ((double)yd1 * ((double)yt2 - (double)yt3)) + ((double)yd2 * ((double)yt3 - (double)yt1)) + ((double)yd3 * ((double)yt1 - (double)yt2));
    temp2 = ((double)xt1 * ((double)yt2 - (double)yt3)) + ((double)xt2 * ((double)yt3 - (double)yt1)) + ((double)xt3 * ((double)yt1 - (double)yt2));
    cal_D = (double)temp1 / (double)temp2;
    touchCoeff.cali_D = (int32_t)(cal_D * RESCALE_FACTOR);

    //E
    temp1 = (cal_D * ((double)xt3 - (double)xt2)) + (double)yd2 - (double)yd3;
    temp2 = (double)yt2 - (double)yt3;
    cal_E = (double)temp1 / (double)temp2;
    touchCoeff.cali_E = (int32_t)(cal_E * RESCALE_FACTOR);

    //F
    cal_F = (double)yd3 - cal_D * (double)xt3 - cal_E * (double)yt3;
    touchCoeff.cali_F = (int32_t)(cal_F * RESCALE_FACTOR);
}

uint16_t TSC2046::getXd1(void)
{
    return xd1;
}

uint16_t TSC2046::getXd2(void)
{
    return xd2;
}
uint16_t TSC2046::getXd3(void)
{
    return xd3;
}
uint16_t TSC2046::getYd1(void)
{
    return yd1;
}

uint16_t TSC2046::getYd2(void)
{
    return yd2;
}
uint16_t TSC2046::getYd3(void)
{
    return yd3;
}

void TSC2046::setXt1(uint32_t xt1)
{
    this->xt1 = xt1;
}
void TSC2046::setXt2(uint32_t xt2)
{
    this->xt2 = xt2;
}
void TSC2046::setXt3(uint32_t xt3)
{
    this->xt3 = xt3;
}
void TSC2046::setYt1(uint32_t yt1)
{
    this->yt1 = yt1;
}
void TSC2046::setYt2(uint32_t yt2)
{
    this->yt2 = yt2;
}
void TSC2046::setYt3(uint32_t yt3)
{
    this->yt3 = yt3;
}

bool TSC2046::getCalibrated()
{
    return touchCoeff.touchIsCalibrated;
}

void TSC2046::setCalibrated()
{
    touchCoeff.touchIsCalibrated = true;
    eeprom->write(EE_MEM_ADDR, (uint8_t*)&touchCoeff, sizeof(touchCoeff));
}