#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#ifdef WIN32
#include <windows.h>
#endif // WIN32
#include "marvelmind_example.h"
#include "marvelmind_devices.h"
#include "marvelmind_utils.h"
#include "marvelmind_pos.h"

typedef enum {
    waitPort, waitDevice, connected
} ConState;
ConState conState= waitPort;

MMDeviceType deviceTypeUSB= unknown;

MarvelmindDeviceVersion usbDevVersion;

static bool robotProgramEnterMode= false;
static uint8_t robotProgramAddress;
static MarvelmindRobotProgramItem robotProgramsItems[256];
static uint8_t robotProgramItemsNumber;

/////////////////////////////////////////////////////////////////////

static void switchToConState(ConState newConState);

bool isRobotProgramEnterMode() {
   return robotProgramEnterMode;
}

static bool readLocationsDisabled() {
    if (isRobotProgramEnterMode())
        return true;

    return false;
}

// Reopen port
static void marvelmindReopenPort() {
    mmClosePort();
    switchToConState(waitPort);
}

// Reads Marvelmind API version
bool marvelmindCheckVersionCommand(char *token1) {
    if (strcmp(token1,"version") != 0)
       return false;

    uint32_t version= 0;
    if (mmAPIVersion(&version)) {
        printf("Marvelmind API version: %d\r\n", (int) version);
    } else {
        printf("Marvelmind API version read failed\r\n");
    }

    return true;
}

// Prints last error
void marvelmindPrintLastError(void) {
    uint32_t error;

    if (mmGetLastError(&error)) {
        printf("Last error: %d\r\n", (int) error);
    } else {
        printf("Get last error failed\r\n");
    }
}

// Change connection state
static void switchToConState(ConState newConState) {
    switch(newConState) {
        case waitPort: {
            printf("Waiting for port...\r\n");
            break;
        }
        case waitDevice: {
            printf("Trying connect to device...\r\n");
            break;
        }
        case connected: {
            printf("Device is connected via USB.\r\n");
            printMMDeviceVersionAndId(&usbDevVersion);

            deviceTypeUSB= getMMDeviceType(usbDevVersion.fwVerDeviceType);
            printMMDeviceType(&deviceTypeUSB);
            break;
        }
    }
    conState= newConState;
}


static bool marvelmindCheckRobotMoveCommand(char *token1, char *token2, char *token3) {
    if (strcmp(token1,"move") != 0)
       return false;

    if (token2 == NULL)
        return true;
    if (token3 == NULL)
        return true;

    uint8_t address= atoi(token2);

    uint8_t moveType= atoi(token3);

    MarvelmindMoveSettings ms;
    ms.moveType= moveType;
    int i;
    for(i=0;i<8;i++)
        ms.reserved[i]= 0;

    if (mmRobotMove(address, &ms)) {
        printf("Move command success\r\n");
    } else {
        printf("Move command failed\r\n");
    }

    return true;
}


static bool marvelmindCheckRobotStopCommand(char *token1, char *token2) {
    if (strcmp(token1,"stop") != 0)
       return false;

    if (token2 == NULL)
        return true;

    uint8_t address= atoi(token2);

    if (mmRobotStop(address)) {
        printf("Stop command success\r\n");
    } else {
        printf("Stop command failed\r\n");
    }

    return true;
}

static bool marvelmindCheckRobotStartCommand(char *token1, char *token2) {
    if (strcmp(token1,"start") != 0)
       return false;

    if (token2 == NULL)
        return true;

    uint8_t address= atoi(token2);
    MarvelmindRobotCommand mrc;
    mrc.commandId= 9;// START command
    mrc.param1= 0;
    mrc.param2= 0;
    mrc.param3= 0;

    if (mmSetRobotCommand(address, &mrc)) {
        printf("Start command success\r\n");
    } else {
        printf("Start command failed\r\n");
    }

    return true;
}

static bool marvelmindCheckRobotPauseCommand(char *token1, char *token2) {
    if (strcmp(token1,"pause") != 0)
       return false;

    if (token2 == NULL)
        return true;

    uint8_t address= atoi(token2);
    MarvelmindRobotCommand mrc;
    mrc.commandId= 4;// PAUSE command
    mrc.param1= 0;
    mrc.param2= 0;
    mrc.param3= 0;

    if (mmSetRobotCommand(address, &mrc)) {
        printf("Pause command success\r\n");
    } else {
        printf("Pause command failed\r\n");
    }

    return true;
}

static bool marvelmindCheckRobotContinueCommand(char *token1, char *token2) {
    if (strcmp(token1,"continue") != 0)
       return false;

    if (token2 == NULL)
        return true;

    uint8_t address= atoi(token2);
    MarvelmindRobotCommand mrc;
    mrc.commandId= 8;// CONTINUE command
    mrc.param1= 0;
    mrc.param2= 0;
    mrc.param3= 0;

    if (mmSetRobotCommand(address, &mrc)) {
        printf("Continue command success\r\n");
    } else {
        printf("Continue command failed\r\n");
    }

    return true;
}

static bool marvelmindCheckRobotProgramCommand(char *token1, char *token2) {
    if (strcmp(token1,"program") != 0)
       return false;

    if (token2 == NULL)
        return true;

    uint8_t address= atoi(token2);

    robotProgramEnterMode= true;
    robotProgramAddress= address;
    robotProgramItemsNumber= 0;

    return true;
}


bool marvelmindCheckRobotCommand(char *token1, char *token2, char *token3, char *token4) {
    bool isBoxieCmd= false;

    if (strcmp(token1,"robot") != 0) {
       if (strcmp(token1,"boxie") == 0) {
          isBoxieCmd= true;
       }

       if (!isBoxieCmd) {
        return false;
       }
    }

    if (token2 == NULL)
        return true;

    if (marvelmindCheckRobotMoveCommand(token2,token3,token4))
       return true;

    if (marvelmindCheckRobotStopCommand(token2,token3))
       return true;

    if (marvelmindCheckRobotStartCommand(token2,token3))
       return true;

    if (marvelmindCheckRobotPauseCommand(token2,token3))
       return true;

    if (marvelmindCheckRobotContinueCommand(token2,token3))
       return true;

    if (marvelmindCheckRobotProgramCommand(token2,token3))
       return true;

    return (!isBoxieCmd);
}


static bool marvelmindCheckBoxieTelemetry(char *token1, char *token2) {
    if (strcmp(token1,"tele") != 0)
       return false;

    if (token2 == NULL)
        return true;

    uint8_t address= atoi(token2);

    MarvelmindBoxieTelemetry bt;

    if (mmGetBoxieTelemetry(address, &bt)) {
        printf("Telemetry for boxie n%d :\r\n", (int) address);
        printf("  Voltage:          %.3f V\r\n", ((float) bt.voltage_mv)/1000.0f);
        printf("  Current:          %.3f A\r\n", ((float) bt.current_ma)/1000.0f);
        printf("  Power left        %d %%\r\n", (int) bt.power_left);
        printf("  Power right       %d %%\r\n", (int) bt.power_right);
        printf("  Odo speed left:   %.3f m/s \r\n", ((float) bt.odo_speed_left)/1000.0f);
        printf("  Odo speed right:  %.3f m/s \r\n", ((float) bt.odo_speed_right)/1000.0f);
    } else {
        printf("Telemetry command failed\r\n");
    }

    return true;
}

static bool marvelmindCheckBoxieLidars(char *token1, char *token2) {
    if (strcmp(token1,"lidars") != 0)
       return false;

    if (token2 == NULL)
        return true;

    uint8_t address= atoi(token2);

    MarvelmindBoxieLidars bl;

    if (mmGetBoxieLidars(address, &bl)) {
        printf("Lidars for boxie n%d :\r\n", (int) address);
        for(uint8_t i=0;i<BOXIE_LIDARS_NUM;i++) {
            if (bl.lidars[i].status != 0) {
                 printf("  Lidar %02d: --- \r\n", (int) (i+1));
            } else {
                printf("  Lidar %02d: %.3f meters\r\n",  (int) (i+1), ((float) bl.lidars[i].range_mm)/1000.0f );
            }
        }
    } else {
        printf("Lidars command failed\r\n");
    }

    return true;
}

static bool marvelmindCheckBoxieLocation(char *token1, char *token2) {
    if (strcmp(token1,"location") != 0)
       return false;

    if (token2 == NULL)
        return true;

    uint8_t address= atoi(token2);

    MarvelmindBoxieLocation bpos;

    if (mmGetBoxieLocation(address, &bpos)) {
        printf("Location for boxie n%d :\r\n", (int) address);
        printf("  X:          %.3f meters\r\n", ((float) bpos.x_mm)/1000.0f);
        printf("  Y:          %.3f meters\r\n", ((float) bpos.y_mm)/1000.0f);
        printf("  Angle:      %d degrees \r\n", (int) bpos.angle_yaw);
    } else {
        printf("Location command failed\r\n");
    }

    return true;
}

bool marvelmindCheckBoxieCommand(char *token1, char *token2, char *token3, char *token4) {
    if (strcmp(token1,"boxie") != 0)
       return false;

    if (token2 == NULL)
        return true;

    if (marvelmindCheckBoxieTelemetry(token2,token3))
       return true;

    if (marvelmindCheckBoxieLidars(token2,token3))
       return true;

    if (marvelmindCheckBoxieLocation(token2,token3))
       return true;

    return true;
}

static void sendRobotProgram(void) {
    if (robotProgramItemsNumber == 0) {
        return;
    }

    int i,j;
    bool ok;
    for(j=0;j<4;j++) {
        ok= true;
        for(i=0;i<robotProgramItemsNumber;i++) {
            robotProgramsItems[i].totalItemsNum= robotProgramItemsNumber;
            if (mmSetRobotProgramItem(robotProgramAddress, &robotProgramsItems[i])) {
                printf("Send program item %d/%d : success\r\n", (int) (i+1), (int) robotProgramItemsNumber);
            } else {
                printf("Send program item %d/%d : failed\r\n", (int) (i+1), (int) robotProgramItemsNumber);
                ok= false;
                break;
            }
        }

        if (ok)
            break;

        sleep_ms(500);
    }
}

static void nextRobotProgram(uint8_t opCode, int16_t param1, int16_t param2, int16_t param3) {
    MarvelmindRobotProgramItem *p= &robotProgramsItems[robotProgramItemsNumber];

    p->itemIndex= robotProgramItemsNumber;
    p->opCode= opCode;
    p->param1= param1;
    p->param2= param2;
    p->param3= param3;

    robotProgramItemsNumber++;
}

bool marvelmindCheckRobotProgramItem(char *token1, char *token2, char *token3, char *token4) {
    if (strcmp(token1,"end") == 0) {
        robotProgramEnterMode= false;
        sendRobotProgram();
        return true;
    }

    int16_t param1= 0;
    int16_t param2= 0;
    int16_t param3= 0;

    if (token2 != NULL) {
        param1= atoi(token2);
    }
    if (token3 != NULL) {
        param2= atoi(token3);
    }
    if (token4 != NULL) {
        param3= atoi(token4);
    }

    if (strcmp(token1,"forward") == 0) {
        nextRobotProgram(0, param1, param2, param3);
        return true;
    }

    if (strcmp(token1,"backward" ) == 0) {
        nextRobotProgram(1, param1, param2, param3);
        return true;
    }

    if (strcmp(token1,"clockwise") == 0) {
        nextRobotProgram(2, param1, param2, param3);
        return true;
    }

    if (strcmp(token1,"counterclockwise") == 0) {
        nextRobotProgram(3, param1, param2, param3);
        return true;
    }

    if (strcmp(token1,"waypoint") == 0) {
        nextRobotProgram(6, param1, param2, param3);
        return true;
    }

    printf("Unknown command \r\n");
    return false;
}

////////////////////////

// Working cycle if modem is connected via USB
void marvelmindModemCycle() {
    static uint8_t failCounter= 0;

    marvelmindDevicesReadIfNeeded();

    if (readLocationsDisabled()) {
        return;
    }

    switch(marvelmindLocationsReadIfNeeded()) {
        case readSuccess: {
            failCounter= 0;
            break;
        }
        case readFail: {
            failCounter++;
            if (failCounter>10) {
                marvelmindReopenPort();
                break;
            }
            break;
        }
        case notRead: {
            break;
        }
    }
}

// Working cycle if beacon is connected via USB
void marvelmindBeaconCycle() {
    //TODO
}

// Working cycle if robot is connected via USB
void marvelmindRobotCycle() {
    //TODO
}

// Marvelmind communication state machine
void marvelmindCycle() {
        switch(conState) {
        case waitPort: {
            //if (mmOpenPortByName("com10")) {
            if (mmOpenPort()) {
                switchToConState(waitDevice);
                break;
            }
            sleep_ms(1);
            break;
        }
        case waitDevice: {
            if (mmGetVersionAndId(MM_USB_DEVICE_ADDRESS, &usbDevVersion)) {
                switchToConState(connected);
            }
            sleep_ms(1);
            break;
        }
        case connected: {
            switch(deviceTypeUSB) {
                case modem: {
                    marvelmindModemCycle();
                    break;
                }
                case beacon:
                case hedgehog: {
                    marvelmindBeaconCycle();
                    break;
                }

                case robot: {
                    marvelmindRobotCycle();
                    break;
                }

                case unknown: {
                    break;
                }
            }
            break;
        }
    }
}

void marvelmindStart() {
    marvelmindAPILoad();// Load Marvelmind API library

    initMarvelmindDevicesList();
    initMarvelmindPos();
    switchToConState(waitPort);// Start waiting port connection
}

void marvelmindFinish() {
    mmClosePort();// Close port (if was opened)

    marvelmindAPIFree();// Free Marvelmind API library memory
}
