#include "tsc2046.h"

constexpr char CMD_X=0xD1U;
constexpr char CMD_Y=0x91U;
constexpr char CMD_ENABLE_IRQ=0x80U;

#define RESCALE_FACTOR 1000000
constexpr uint32_t EE_MEM_ADDR=400U;

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(MOSI, MISO, SCK),
    tpCS(CS),
    tpInt(INT, PullUp)
{
//    tpPort = new SPI(MOSI, MISO, SCK);
//    tpCS = new DigitalOut(CS);
//    tpInt = new DigitalIn(INT, PullUp);

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

    (void)readValue(CMD_ENABLE_IRQ); // Enable IRQ
}

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

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

    (void)readValue(CMD_ENABLE_IRQ); // Enable IRQ
}
void TSC2046::GetXY(int32_t* X, int32_t* Y)
{
    uint16_t x;
    uint16_t 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(char port)
{
    static char txbuf[3] = { 0U };
    static char rxbuf[3] = { 0U };

    txbuf[0] = port;
    tpCS.write(0);
    (void)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;
    double temp2;
    double cal_A = 0.0;
    double cal_B = 0.0;
    double cal_C = 0.0;
    double cal_D = 0.0;
    double cal_E = 0.0;
    double cal_F = 0.0;

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

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

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

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

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

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

int TSC2046::getXd1()
{
    return xd1;
}

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

int TSC2046::getYd2()
{
    return yd2;
}
int TSC2046::getYd3()
{
    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() const
{
    return touchCoeff.touchIsCalibrated;
}

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