/*
* @Author: Wang Feixuan
* @Email:  cnfxwang@gmail.com
* @Last Modified time: 2016-08-25 11:13:48
*/


#include "eyebot.h"

// for lcd area
#define HOLLOW 0
#define FILLED 1

// for key codes
/**
 * KEY1 1
 * KEY2 2
 * KEY3 4
 * KEY4 8
 * NOKEY 0
 * ANYKEY 0xffffffff
 */
#define LINE_SPEED_DEC 11
#define LINE_SPEED_INC 12
#define ANG_SPEED_DEC  13
#define ANG_SPEED_INC  14
#define LINE_DIST_DEC  15
#define LINE_DIST_INC  16
#define CURVE_DIST_DEC 17
#define CURVE_DIST_INC 18
#define TURN_ANG_DEC   19
#define TURN_ANG_INC   20
#define TURN_REVERSE   22

#define VV_DEC 27
#define VV_INC 28
#define TV_DEC 29
#define TV_INC 30
#define VW_DEC 31
#define VW_INC 32
#define TW_DEC 33
#define TW_INC 34

#define STEP_DEC  37
#define STEP_INC  38

// for motors
int lin_speed = 50;
int ang_speed = 50;
int lin_dist = 20;
int curve_dist = 20;
int turn_ang = 90;

// step added every time
int incr_step = 1;

// for PID controllers
int Vv = 75;
int Tv = 45;
int Vw = 65;
int Tw = 10;

// for LCDSetPrintf()
#define x1 10
#define x2 125
#define x3 345
#define x4 460


#define col1 0
#define col2 30
#define col3 12

#define step 20

// for KEY_Decode
#define LEFT   1
#define RIGHT  2
#define MIDDLE 3


int side_x(int x) {
    if (x1 < x && x < x2)
        return LEFT;
    if (x3 < x && x < x4)
        return RIGHT;
    return MIDDLE;
}


// there's already a private function called KEYDecode() in eyebot.h
int KEY_Decode(int x, int y) {


    // KEY1 to KEY4
    if (y > 285) {
        if (x < 120)
            return KEY1;
        if (x < 240)
            return KEY2;
        if (x < 360)
            return KEY3;
        return KEY4;
    }

    int col = side_x(x);
    int row = y / step;


    if (col == MIDDLE) {
        if (row == 4)
            return TURN_REVERSE;
        return NOKEY;
    }

    if (row == 5 || row == 6 || row == 7 || row == 12 || row > 13)
        return NOKEY;


    return 10 + col + row * 2;
}


// reprint the parameter information every time something is changed
// also redraw triangles as somehow the colors can be changed otherwise
void lcd_print_info() {

    int i = 0;
    for (; i < 15; i++) {
        if (i == 6 || i == 7 || i == 8 || i == 13)  // blank lines
            continue;
        LCDArea(x1, step * (i - 1), x2, step * i, GREEN, HOLLOW);
        LCDArea(x3, step * (i - 1), x4, step * i, RED, HOLLOW);
    }


    LCDSetFontSize(14);
    LCDSetFont(COURIER, BOLD);

    LCDSetColor(WHITE, BLACK);

    LCDSetPrintf(0, col3, "line  speed = %3d", lin_speed);
    LCDSetPrintf(1, col3, "angle speed = %3d", ang_speed);
    LCDSetPrintf(2, col3, "line  dist  = %3d", lin_dist);
    LCDSetPrintf(3, col3, "curve dist  = %3d", curve_dist);
    LCDSetPrintf(4, col3, "turn angle  = %3d", turn_ang);

    LCDSetPrintf(6, col3, "For PID controllers:");

    LCDSetPrintf(8, col3, "(straight)Vv =%3d", Vv);
    LCDSetPrintf(9, col3, "(straight)Tv =%3d", Tv);
    LCDSetPrintf(10, col3, "(turning) Vw =%3d", Vw);
    LCDSetPrintf(11, col3, "(turning) Tw =%3d", Tw);

    LCDSetPrintf(13, col3, "incr step = %3d", incr_step);

    // turn reverse(clockwise / anti-clockwise)
    i = 5;
    LCDArea(x2 + 15, step * (i - 1), x3 - 15, step * i, YELLOW, HOLLOW);

}

// "+""-" on the buttons can be drawn once only
void draw_keys() {

    LCDSetFontSize(14);
    LCDSetFont(COURIER, BOLD);

    LCDSetColor(WHITE, GREEN);

    LCDSetPrintf(0, col1, "     -     ");
    LCDSetPrintf(1, col1, "     -     ");
    LCDSetPrintf(2, col1, "     -     ");
    LCDSetPrintf(3, col1, "     -     ");
    LCDSetPrintf(4, col1, "     -     ");

    LCDSetPrintf(8, col1, "     -     ");
    LCDSetPrintf(9, col1, "     -     ");
    LCDSetPrintf(10, col1, "     -     ");
    LCDSetPrintf(11, col1, "     -     ");

    LCDSetPrintf(13, col1, "     -     ");

    LCDSetColor(WHITE, RED);

    LCDSetPrintf(0, col2, "     +     ");
    LCDSetPrintf(1, col2, "     +     ");
    LCDSetPrintf(2, col2, "     +     ");
    LCDSetPrintf(3, col2, "     +     ");
    LCDSetPrintf(4, col2, "     +     ");

    LCDSetPrintf(8, col2, "     +     ");
    LCDSetPrintf(9, col2, "     +     ");
    LCDSetPrintf(10, col2, "     +     ");
    LCDSetPrintf(11, col2, "     +     ");

    LCDSetPrintf(13, col2, "     +     ");

}


time_t rawtime;
struct tm *start_time, *end_time;

/**
 * @brief      save the all the parameters to a file for next time
 */
void VWExit() {
    VWSetSpeed(0,0);

    LCDClear();
    LCDPrintf("\n\nFinal parameters:\n");
    LCDPrintf("(straight)Vv = %3d\n", Vv);
    LCDPrintf("(straight)Tv=  %3d\n", Tv);
    LCDPrintf("(turning) Vw = %3d\n", Vw);
    LCDPrintf("(turning) Tw = %3d\n", Tw);
    LCDPrintf("\n\nExiting the program..\n");
    sleep(2);

    // writing to the log file
    stdout = freopen("/home/pi/usr/software/control/PID_tuning.log", "a+", stdout);

    time(&rawtime);
    end_time = localtime(&rawtime);


    printf("*********************************\n");
    printf("from %s", asctime(start_time));
    printf("to   %s", asctime(end_time));
    printf("\n");
    printf("Final parameters\n");
    printf("(straight proportional) Vv = %3d\n", Vv);
    printf("(straight integral    ) Tv=  %3d\n", Tv);
    printf("(turning  proportioanl) Vw = %3d\n", Vw);
    printf("(turning  integral    ) Tw = %3d\n", Tw);
    printf("*********************************\n\n\n");

    // save final parameters for next time
    stdout = freopen("/home/pi/usr/software/control/PID_tuning.parameters.txt", "w", stdout);
    printf("%d\n", lin_speed);
    printf("%d\n", ang_speed);
    printf("%d\n", lin_dist);
    printf("%d\n", curve_dist);
    printf("%d\n", turn_ang);

    printf("%d\n", Vv);
    printf("%d\n", Tv);
    printf("%d\n", Vw);
    printf("%d\n", Tw);

    printf("%d\n", incr_step);

    exit(-1);
}


int main() {

    time(&rawtime);
    start_time = localtime(&rawtime);


    // read parameters from last test
    // if no parameters provided, use default
    if ((stdin = freopen("/home/pi/usr/software/control/PID_tuning.parameters.txt", "r", stdin))) {
        scanf("%d\n", &lin_speed);
        scanf("%d\n", &ang_speed);
        scanf("%d\n", &lin_dist);
        scanf("%d\n", &curve_dist);
        scanf("%d\n", &turn_ang);
        scanf("%d\n", &Vv);
        scanf("%d\n", &Tv);
        scanf("%d\n", &Vw);
        scanf("%d\n", &Tw);
        scanf("%d\n", &incr_step);
    }

    LCDMenu("STRAIGHT", "TURN", "CURVE", "EXIT");

    LCDSetFontSize(14);
    LCDSetFont(COURIER, BOLD);


    draw_keys();

    lcd_print_info();

    VWControl(Vv, Tv, Vw, Tw);

    int x = 0, y = 0;
    KEYGetXY(&x, &y);
    int key_code = KEY_Decode(x, y);

    while (true) {

        switch (key_code) {
        case KEY1:
            VWStraight(lin_dist * 10, lin_speed);
            // VWWait();
            //  usleep(500);
            // VWSetSpeed(0, 0);
//                VWSetPosition(0, 0, 0);
            break;
        case KEY2:
            VWTurn((int) (turn_ang * 31.4 / 1.8), ang_speed);
            // VWWait();
            // usleep(500);
            // VWSetSpeed(0, 0);
//                VWSetPosition(0, 0, 0);
            break;
        case KEY3:
            VWCurve(curve_dist * 10, (int) (turn_ang * 3.14 / 1.8), lin_speed);
            // VWWait();
            // usleep(500);
            // VWSetSpeed(0, 0);
//                VWSetPosition(0, 0, 0);
            break;
        case KEY4:
            VWExit();
            break;
        case LINE_SPEED_DEC:
            if (lin_speed - incr_step >= 0)
                lin_speed -= incr_step;
            break;
        // max safe speed
        case LINE_SPEED_INC:
            lin_speed += incr_step;
            break;
        case ANG_SPEED_DEC:
            if (ang_speed - incr_step >= 0)
                ang_speed -= incr_step;
            break;
        case ANG_SPEED_INC:
            ang_speed += incr_step;
            break;
        case LINE_DIST_DEC:
            if (lin_dist - incr_step >= 0)
                lin_dist -= incr_step;
            break;
        case LINE_DIST_INC:
            lin_dist += incr_step;
            break;
        case CURVE_DIST_DEC:
            if (curve_dist - incr_step >= 0)
                curve_dist -= incr_step;
            break;
        case CURVE_DIST_INC:
            curve_dist += incr_step;
            break;
        // angles can be either above and below zero
        // for anti-clockwise and clock-wise
        case TURN_ANG_DEC:
            if (turn_ang >= 0) {
                if (turn_ang - incr_step >= 0)
                    turn_ang -= incr_step;
            } else {
                if (turn_ang + incr_step <= 0)
                    turn_ang += incr_step;
            }
            break;
        case TURN_ANG_INC:
            if (turn_ang >= 0) {
                turn_ang += incr_step;
            } else {
                turn_ang -= incr_step;
            }
            break;
        case TURN_REVERSE:
            turn_ang *= -1;
            break;
        case VV_DEC:
            if (Vv - incr_step >= 0)
                Vv -= incr_step;
            break;
        case VV_INC:
            Vv += incr_step;
            break;
        case TV_DEC:
            if (Tv - incr_step >= 0)
                Tv -= incr_step;
            break;
        case TV_INC:
            Tv += incr_step;
            break;
        case VW_DEC:
            if (Vw - incr_step >= 0)
                Vw -= incr_step;
            break;
        case VW_INC:
            Vw += incr_step;
            break;
        case TW_DEC:
            if (Tw - incr_step >= 0)
                Tw -= incr_step;
            break;
        case TW_INC:
            Tw += incr_step;
            break;
        case STEP_DEC:
            if (incr_step > 0)
                incr_step--;
            break;
        case STEP_INC:
            incr_step++;
            break;
        default:
            break;
        }

        lcd_print_info();
        VWControl(Vv, Tv, Vw, Tw);


        KEYGetXY(&x, &y);
        key_code = KEY_Decode(x, y);


    }
    VWExit();
    return 0;
}
