#include "DxLib.h"

HANDLE hComm;
void ClosePort() {
    if (hComm != 0) {
        CloseHandle(hComm);
        hComm = 0;
    }
}

bool OpenPort(const char comname[]) {

    char comfullname[64];
    strcpy_s(comfullname, "\\\\.\\");
    strcat_s(comfullname, comname);

    hComm = CreateFileA(comfullname,
        GENERIC_READ | GENERIC_WRITE,
        0,
        NULL,
        OPEN_EXISTING,
        0,
        NULL);

    if (hComm == INVALID_HANDLE_VALUE)
        return false;

    DCB dcb;
    GetCommState(hComm, &dcb);

    dcb.BaudRate = 115200;
    dcb.ByteSize = 8;
    dcb.Parity = NOPARITY;
    dcb.StopBits = ONESTOPBIT;
    dcb.fOutxCtsFlow = FALSE;
    dcb.fRtsControl = RTS_CONTROL_ENABLE;

    SetCommState(hComm, &dcb);

    COMMTIMEOUTS timeout;
    timeout.ReadIntervalTimeout = 0;
    timeout.ReadTotalTimeoutMultiplier = 0;
    timeout.ReadTotalTimeoutConstant = 100;
    timeout.WriteTotalTimeoutMultiplier = 0;
    timeout.WriteTotalTimeoutConstant = 100;

    SetCommTimeouts(hComm, &timeout);

    return true;
}



bool ReadPort(char buf[], int len) {
    buf[0] = '\0';
    if (hComm == 0) {
        return false;
    }
    DWORD lenr = 1;

    char c = 0;
    bool b;
    int i;
    for(i=0;i<100;i++){
        c = 0;
        b = ReadFile(hComm, &c, 1, &lenr, NULL);
        if (b == false || lenr == 0) {
            return false;
        }
        if (b && c == 'C') {
            break;
        }
    }
    buf[0] = 'C';
    buf[1] = '\0';

    b = ReadFile(hComm, &buf[1], 75, &lenr, NULL);
    buf[lenr + 1] = '\0';
    if (b && lenr + 1 == 76) {
        return true;
    }
    return false;
}


bool WritePort(char buf[], int len) {
    if (hComm == 0) {
        return false;
    }
    DWORD lenw;
    WriteFile(hComm, buf, len, &lenw, NULL);
    if (lenw == 1) {
        return true;
    }

    return false;
}


bool splitword(char buf[], float param[], int btn[]) {
    int i = 0;
    int j;
    int k = 0;
    char word[10][32];
    int checkdigit = 0;
    for (j = 0; buf[j] != '\r' && buf[j] != '\n' && buf[j] != '\0'; j++) {
        if (j <= 72) {
            checkdigit += buf[j];
        }
        if (buf[j] == ',') {
            i++;
            k = 0;
        }
        else {
            if (i < 10) {
                word[i][k] = buf[j];
                word[i][k + 1] = '\0';
                k++;
            }
        }
    }

    if (i == 9 && strcmp(word[0], "CZS") == 0) {
        char check1 = checkdigit % 100 / 10 + 0x30;
        char check2 = checkdigit % 10 + 0x30;
        if (check1 == word[9][0] && check2 == word[9][1]) {

            for (j = 0; j < 7; j++) {
                param[j] = (float)atof(word[j + 1]);
            }
            btn[0] = word[8][0] - 0x30;
            btn[1] = word[8][1] - 0x30;
            btn[2] = word[8][2] - 0x30;
            btn[3] = word[8][3] - 0x30;
            btn[4] = word[8][4] - 0x30;
            return true;
        }
    }
    return false;
}

float deg2rad(float deg) {
    return deg / 180.0f * 3.141592f;
}

void compose(float qx, float qy, float qz, float qw, float te[]) {

    float x = qx;
    float y = qy;
    float z = qz;
    float w = qw;
    float x2 = x + x;
    float y2 = y + y;
    float z2 = z + z;
    float xx = x * x2;
    float xy = x * y2;
    float xz = x * z2;
    float yy = y * y2;
    float yz = y * z2;
    float zz = z * z2;
    float wx = w * x2;
    float wy = w * y2;
    float wz = w * z2;

    te[0] = (1.0f - (yy + zz));
    te[1] = (xy + wz);
    te[2] = (xz - wy);
    te[3] = 0.0f;

    te[4] = (xy - wz);
    te[5] = (1.0f - (xx + zz));
    te[6] = (yz + wx);
    te[7] = 0.0f;

    te[8] = (xz + wy);
    te[9] = (yz - wx);
    te[10] = (1.0f - (xx + yy));
    te[11] = 0.0f;

    te[12] = 0.0f;
    te[13] = 0.0f;
    te[14] = 0.0f;
    te[15] = 1.0f;

}

int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow)
{
    char comname[64];
    strcpy_s(comname, "COM19");

    if (__argc > 1) {
        strcpy_s(comname, __argv[1]);
    }

    int mh;

    ChangeWindowMode(TRUE);

    if (DxLib_Init() < 0) return -1;

    bool bport = false;

    char bufw[2];
    char bufr[256];

    float param[7];
    int btn[5];
    strcpy_s(bufw, "D");

    bport = OpenPort(comname);

    mh = MV1LoadModel("3dtest.mv1");
    VECTOR mpos, mrot;
    mpos.x = 0.0f;
    mpos.y = 0.0f;
    mpos.z = 0.0f;

    VECTOR campos;
    campos.x = 0.0f;
    campos.y = 0.0f;
    campos.z = -600.0f;

    SetDrawScreen(DX_SCREEN_BACK);

    while (ProcessMessage() == 0 && CheckHitKey(KEY_INPUT_ESCAPE) == 0)
    {
        if (WritePort(bufw, 1)) {
            if (ReadPort(bufr, 256)) {
                if (splitword(bufr, param, btn)) {
                    ClearDrawScreen();
                    SetCameraPositionAndAngle(campos, 0.0f, 0.0f, 0.0f);
                    MV1SetPosition(mh, mpos);

                    ////////quaternion to matrix/////////
                    MATRIX   matrix;
                    compose(-param[3], -param[4], param[5], param[6], (float*)matrix.m);
                    MV1SetRotationMatrix(mh, matrix);
                    //////////////////////////////////////

                    ////////degree ///////////////////////
                    //mrot.x = deg2rad(-param[0]);
                    //mrot.y = deg2rad(-param[1]);
                    //mrot.z = deg2rad(param[2]);
                    //MV1SetRotationXYZ(mh, mrot);
                    /// //////////////////////////////////


                    MV1DrawModel(mh);

                    int Cr = GetColor(255, 255, 255);
                    bufr[72] = '\0';
                    DrawString(10, 3, &bufr[4], Cr);
                    char statusstr[256];
                    sprintf_s(statusstr, "joystick Left:%d Right:%d Up:%d Down:%d Push:%d"
                        , btn[0], btn[1], btn[2], btn[3], btn[4]);
                    DrawString(10, 19, statusstr, Cr);

                    ScreenFlip();
                }
            }
        }
    }

    DxLib_End();

    ClosePort();

    return 0;
}