#include "CoreXY.hpp"

#include "FreeRTOS.h"
#include "task.h"
#include "stdio.h"

void waitForUserInput() {
    getchar();
}

void performGridCall(CoreXYMaschine * machine) {

    while (true) {
    
        MotorPosition p0 = {.x=1296, .y=1802};
        MotorPosition p1 = {.x=16696, .y=18722};

//        machine->homeFast(&p0, false);
//        printf("First Position\n");
//        machine->setMotorEnabled(false);
//        waitForUserInput();
//        machine->setMotorEnabled(true);
//        machine->homeFast(&p0, false);
//
//        printf("Second Position\n");
//        machine->setMotorEnabled(false);
//        waitForUserInput();
//        machine->setMotorEnabled(true);
//        machine->homeFast(&p1, false);
//
//        printf("First Position: %d, %d\n", p0.x, p0.y);
//        printf("Second Position: %d, %d\n", p1.x, p1.y);
//
        BoardMessurements messurements = calcBoardMessurements(p0, p1, 19, 19);
        machine->setBoardMessurements(messurements);
        printf("Hi!\n");
        machine->home(NULL);
        waitForUserInput();
        machine->gotoPosition(10, 9);
        waitForUserInput();
        machine->gotoPosition(9, 10);
        waitForUserInput();
        machine->gotoPosition(8, 9);
        waitForUserInput();
        machine->gotoPosition(9, 8);
        waitForUserInput();
        machine->gotoPosition(10, 7);
        waitForUserInput();
        machine->home(NULL);
    }

    //machine->gotoPosition(7, 6);

    //for(int i = 0; i < 19; i++) {
    //    for(int j = 0; j < 19; j++) {
    //        machine->gotoPosition(i, j);
    //        vTaskDelay(100 / portTICK_PERIOD_MS);
    //    }
    //}

    //machine->home(NULL);
}

void vTaskInitMotorHome(void *pvParameters) {
    CoreXYMaschine * machine = (CoreXYMaschine *)pvParameters;
    vTaskDelay(500 / portTICK_PERIOD_MS);    

    MotorPosition res;
    machine->home(&res);

    performGridCall(machine);

    vTaskDelete(NULL);
}

void CoreXYMaschine::runTasks() {
    xTaskCreate(vTaskInitMotorHome, "Initial Motor Home", 1024, this, 5, NULL);

    vTaskStartScheduler();
    while(1) {
        tight_loop_contents();
    }
}