#include "ListE.h"
#include "TimerOne.h"
#include <string.h>

#include "REG.h"
#include "wit_c_sdk.h"

#define TIME_PER_LAYER_IN_US 800
#define BUS_START_PIN 22
#define CLOCK_START_PIN 30
#define SW_START_PIN 38

#define JOYSTICK_VRX A1
#define JOYSTICK_VRY A0
#define JOYSTICK_SWITCH 2

#define ACC_UPDATE 0x01
#define GYRO_UPDATE 0x02
#define ANGLE_UPDATE 0x04
#define MAG_UPDATE 0x08
#define READ_UPDATE 0x80

#define DEBUG false
#define TRACE false

// 3 width, 4 height

const uint16_t numbers[10] = {
    0xF6F, 0x592, 0xE57, 0xE8F, 0x979, 0xF8F, 0xF3F, 0xE54, 0xFEF, 0xFCF,
};

// 8 width, 7 height

const uint64_t letters[]{
    0x3C66667E666666, // A
    0x7C66667C66667C, // B
    0x3C66606060663C, // C
    0x7C66666666667C, // D
    0x7E60607C60607E, // E
    0x7E60607C606060, // F
    0x3C6660606E663C, // G
    0x6666667E666666, // H
    0x3C18181818183C, // I
    0x1E0C0C0C6C6C38, // J
    0x666C7870786C66, // K
    0x6060606060607E, // L
    0x63777F6B636363, // M
    0x63737B6F676363, // N
    0x3C66666666663C, // O
    0x7C6666667C6060, // P
    0x3C6666666E3C06, // Q
    0x7C66667C786C66, // R
    0x3C66603C06663C, // S
    0x7E5A1818181818, // T
    0x6666666666663C, // U
    0x66666666663C18, // V
    0x6363636B7F7763, // W
    0x6363361C366363, // X
    0x6666663C181818, // Y
    0x7E060C1830607E, // Z
    0x3C666E7666663C, // 0
    0x1818381818187E, // 1
    0x3C66060C30607E, // 2
    0x3C66061C06663C, // 3
    0x0C1C2C4C7E0C0C, // 4
    0x7E607C0606663C, // 5
    0x3C66607C66663C, // 6
    0x7E660C0C181818, // 7
    0x3C66663C66663C, // 8
    0x3C66663E06663C, // 9
};

struct Triple {
    float x;
    float y;
    float z;
};

class SensorReader {
private:
    static const uint32_t c_uiBaud[8];
    static volatile byte s_cDataUpdate;

    static void AutoScanSensor() {
        if (DEBUG) {
            Serial.println("Autoscan sensor");
        }

        int iRetry;

        for (int i = 0; i < sizeof(SensorReader::c_uiBaud) /
                                sizeof(SensorReader::c_uiBaud[0]);
             i++) {
            Serial1.begin(SensorReader::c_uiBaud[i]);
            Serial1.flush();
            iRetry = 2;
            SensorReader::s_cDataUpdate = 0;
            do {
                WitReadReg(AX, 3);
                delay(200);
                while (Serial1.available()) {
                    WitSerialDataIn(Serial1.read());
                }
                if (SensorReader::s_cDataUpdate != 0) {
                    Serial.print(SensorReader::c_uiBaud[i]);
                    Serial.println(" baud find sensor");
                    return;
                }
                iRetry--;
            } while (iRetry);
        }
        Serial.println("Can not find sensor, please check your connection");
    }

    static void SensorUartSend(uint8_t *p_data, uint32_t uiSize) {
        Serial1.write(p_data, uiSize);
        Serial1.flush();
    }

    static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {
        int i;
        for (i = 0; i < uiRegNum; i++) {
            switch (uiReg) {
            case AZ:
                SensorReader::s_cDataUpdate |= ACC_UPDATE;
                break;
            case GZ:
                SensorReader::s_cDataUpdate |= GYRO_UPDATE;
                break;
            case HZ:
                SensorReader::s_cDataUpdate |= MAG_UPDATE;
                break;
            case Yaw:
                SensorReader::s_cDataUpdate |= ANGLE_UPDATE;
                break;
            default:
                SensorReader::s_cDataUpdate |= READ_UPDATE;
                break;
            }
            uiReg++;
        }
    }

    static void Delayms(uint16_t ucMs) {
        delay(ucMs);
    }

public:
    static Triple angle, acceleration, gyro;

    static bool init() {
        WitInit(WIT_PROTOCOL_NORMAL, 0x50);
        WitSerialWriteRegister(SensorReader::SensorUartSend);
        WitRegisterCallBack(SensorReader::SensorDataUpdata);
        WitDelayMsRegister(SensorReader::Delayms);
        SensorReader::AutoScanSensor();

        if (WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK) {
            Serial.println("Set Baud Error");
            return false;
        }
        else {
            Serial1.begin(SensorReader::c_uiBaud[WIT_BAUD_9600]);
            if (DEBUG) {
                Serial.println("9600 Baud rate modified successfully");
            }
        }

        if (WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) {
            Serial.println("Set Bandwidth Error");
            return false;
        }

        if (WitSetContent(RSW_ANGLE | RSW_ACC | RSW_GYRO) != WIT_HAL_OK) {
            Serial.println("Set send content: angle, acc, and GYRO Error");
            return false;
        }
        else {
            if (DEBUG) {
                Serial.println("Set send content: angle, acc, and GYRO");
            }
        }

        if (WitSetOutputRate(RRATE_5HZ) != WIT_HAL_OK) {
            Serial.print("Set report rate failed");
            return false;
        }
        else {
            if (DEBUG) {
                Serial.println("Set report rate success");
            }
        }

        return true;
    }

    static void read_data_from_sensor_interrupt() {
        // Serial.println("Updated!");
        while (Serial1.available()) {
            WitSerialDataIn(Serial1.read());
        }
        if (SensorReader::s_cDataUpdate) {
            if (SensorReader::s_cDataUpdate & ANGLE_UPDATE) {
                SensorReader::angle.x = sReg[Roll] / 32768.0f * 180.0f;
                SensorReader::angle.y = sReg[Roll + 1] / 32768.0f * 180.0f;
                SensorReader::angle.z =
                    sReg[Roll + 2] / 32768.0f * 180.0f; // Unit: deg

                SensorReader::s_cDataUpdate &= ~ANGLE_UPDATE;
            }

            if (SensorReader::s_cDataUpdate & ACC_UPDATE) {
                SensorReader::acceleration.x =
                    sReg[AX] / 32768.0f * 16.0f * 9.8f;
                SensorReader::acceleration.y =
                    sReg[AX + 1] / 32768.0f * 16.0f * 9.8f;
                SensorReader::acceleration.z =
                    sReg[AX + 2] / 32768.0f * 16.0f * 9.8f;
                SensorReader::s_cDataUpdate &= ~ACC_UPDATE;
            }

            if (SensorReader::s_cDataUpdate & GYRO_UPDATE) {
                SensorReader::gyro.x = sReg[GX] / 32768.0f * 2000.0f;
                SensorReader::gyro.y = sReg[GX + 1] / 32768.0f * 2000.0f;
                SensorReader::gyro.z = sReg[GX + 2] / 32768.0f * 2000.0f;
                SensorReader::s_cDataUpdate &= GYRO_UPDATE;
            }

            SensorReader::s_cDataUpdate = 0;
        }
    }

    static float get_angle_z() {
        return SensorReader::angle.z;
    }
};

class Cube {
private:
    static int layer_count;
    static int brightness_count;
    static bool blinking_LED_status;

public:
    // Every int represents a row of 8 LED status.
    static uint16_t LED_status[8][8];
    static uint16_t LED_brightness[8][8];
    static uint8_t LED_blinking_status[8][8];

    static void set_blinking(int x, int y, int z) {
        // 1 = enable blinking
        if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
            return;
        }
        LED_blinking_status[z][x] = LED_blinking_status[z][x] | (1 << y);
    }

    static void unset_blinking(int x, int y, int z) {
        if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
            return;
        }
        LED_blinking_status[z][x] = LED_blinking_status[z][x] & (~1 << y);

        LED_status[z][x] = (LED_status[z][x] & (~(3 << (y * 2)))) |
                           (LED_brightness[z][x] & (3 << (y * 2)));
    }

    static void do_blinking() {
        blinking_LED_status ^= 1;
        for (int i = 0; i < 8; i++) {
            for (int j = 0; j < 8; j++) {
                for (int k = 0; k < 8; k++) {
                    if (LED_blinking_status[i][j] >> k & 1) {
                        if (blinking_LED_status) {
                            LED_status[i][j] = LED_brightness[i][j];
                        }
                        else {
                            LED_status[i][j] =
                                LED_status[i][j] & (~(3 << (k * 2)));
                        }
                    }
                }
            }
        }
    }

    static void display() {
        // Serial.println("Here");
        if (brightness_count >= 2) {
            digitalWrite(SW_START_PIN + layer_count, LOW);
            layer_count = (layer_count + 1) % 8;
        }

        brightness_count = (brightness_count + 1) % 3;

        for (int i = 0; i < 8; i++) {
            for (int j = 0; j < 8; j++) {
                // In LED_status:
                // 0 = off
                // 4 = brightest
                digitalWrite(BUS_START_PIN + j,
                             ((LED_status[layer_count][i] >> (j * 2)) & 3) >=
                                 (3 - brightness_count));
            }
            digitalWrite(CLOCK_START_PIN + i, HIGH);
            digitalWrite(CLOCK_START_PIN + i, LOW);
        }

        digitalWrite(SW_START_PIN + layer_count, HIGH);
    }

    static void set_status(int x, int y, int z, int brightness) {
        if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
            return;
        }
        brightness %= 4;
        LED_brightness[z][x] = (LED_brightness[z][x] & (~(3 << (y * 2)))) |
                               (brightness << (y * 2));
        if (LED_blinking_status[z][x] >> y & 1) {
            return;
        }
        LED_status[z][x] =
            (LED_status[z][x] & (~(3 << (y * 2)))) | (brightness << (y * 2));
    }

    static int get_status(int x, int y, int z) {
        return LED_brightness[z][x] >> (y * 2) & 3;
    }

    static void clear() {
        for (int i = 0; i < 8; i++) {
            for (int j = 0; j < 8; j++) {
                LED_brightness[i][j] = 0;
                LED_blinking_status[i][j] = 0;
                LED_status[i][j] = 0;
            }
        }
    }

    static void draw_line(int x, int y, int z, int length, int direction,
                          int brightness) {
        if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
            return;
        }
        if (direction >= 3 || direction < 0 || length <= 0 || brightness >= 4 ||
            brightness < 0) {
            return;
        }
        // 0: x
        // 1: y
        // 2: z
        switch (direction) {
        case 0:
            for (int i = 0; i < length; i++) {
                set_status(x + i, y, z, brightness);
            }
            break;
        case 1:
            for (int i = 0; i < length; i++) {
                set_status(x, y + i, z, brightness);
            }
            break;
        case 2:
            for (int i = 0; i < length; i++) {
                set_status(x, y, z + i, brightness);
            }
            break;
        }
    }

    static void draw_num(int x, int y, int z, int num, int direction,
                         int brightness) {
        // 0 = look along x
        // 1 = look reverse x
        // 2 = look along y
        // 3 = look reverse y
        if (num > 9 || num < 0) {
            return;
        }
        switch (direction) {
        case 0: {
            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 4; j++) {
                    if ((numbers[num] >> (11 - (i + j * 3))) & 1) {
                        Cube::set_status(x, y - i, z - j, brightness);
                    }
                    else {
                        Cube::set_status(x, y - i, z - j, 0);
                    }
                }
            }
            break;
        }
        case 1: {
            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 4; j++) {
                    if ((numbers[num] >> (11 - (i + j * 3))) & 1) {
                        Cube::set_status(x, y + i, z - j, brightness);
                    }
                    else {
                        Cube::set_status(x, y + i, z - j, 0);
                    }
                }
            }
            break;
        }
        case 2: {
            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 4; j++) {
                    if ((numbers[num] >> (11 - (i + j * 3))) & 1) {
                        Cube::set_status(x + i, y, z - j, brightness);
                    }
                    else {
                        Cube::set_status(x + i, y, z - j, 0);
                    }
                }
            }
            break;
        }
        case 3: {
            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 4; j++) {
                    if ((numbers[num] >> (11 - (i + j * 3))) & 1) {
                        Cube::set_status(x - i, y, z - j, brightness);
                    }
                    else {
                        Cube::set_status(x - i, y, z - j, 0);
                    }
                }
            }
            break;
        }
        }
    }

    static void draw_letter(int x, int y, int z, int num, int direction,
                            int brightness) {
        // 0 = look along x
        // 1 = look reverse x
        // 2 = look along y
        // 3 = look reverse y
        if (num >= sizeof(letters) || num < 0) {
            return;
        }
        switch (direction) {
        case 0: {
            for (int i = 0; i < 8; i++) {
                for (int j = 0; j < 7; j++) {
                    if ((letters[num] >> (55 - (i + j * 8))) & 1) {
                        Cube::set_status(x, y - i, z - j, brightness);
                    }
                    else {
                        Cube::set_status(x, y - i, z - j, 0);
                    }
                }
            }
            break;
        }
        case 1: {
            for (int i = 0; i < 8; i++) {
                for (int j = 0; j < 7; j++) {
                    if ((letters[num] >> (55 - (i + j * 8))) & 1) {
                        Cube::set_status(x, y + i, z - j, brightness);
                    }
                    else {
                        Cube::set_status(x, y + i, z - j, 0);
                    }
                }
            }
            break;
        }
        case 2: {
            for (int i = 0; i < 8; i++) {
                for (int j = 0; j < 7; j++) {
                    if ((letters[num] >> (55 - (i + j * 8))) & 1) {
                        Cube::set_status(x + i, y, z - j, brightness);
                    }
                    else {
                        Cube::set_status(x + i, y, z - j, 0);
                    }
                }
            }
            break;
        }
        case 3: {
            for (int i = 0; i < 8; i++) {
                for (int j = 0; j < 7; j++) {
                    if ((letters[num] >> (55 - (i + j * 8))) & 1) {
                        Cube::set_status(x - i, y, z - j, brightness);
                    }
                    else {
                        Cube::set_status(x - i, y, z - j, 0);
                    }
                }
            }
            break;
        }
        }
    }
};

enum Operators {
    plus,
    minus,
    multiply,
    divide,
    power,
    left_parenthesis,
    right_parenthesis,
};

enum CompareResult {
    less,
    equal,
    greater,
};

class Symbol {
private:
    bool is_symbol;
    String name;
    double value;

public:
    static double *x_value_ptr, *y_value_ptr;
    Symbol(String symbol_name) {
        this->is_symbol = true;
        this->name = symbol_name;
    }
    Symbol(double new_value) {
        this->is_symbol = false;
        this->value = new_value;
    }

    double get_value() const {
        if (this->is_symbol) {
            if (this->name == "x") {
                return *x_value_ptr;
            }
            else if (this->name == "y") {
                return *y_value_ptr;
            }
        }

        return this->value;
    }
};

class Operator {
private:
    Operators name;

public:
    Operator(const char op) {
        if ('+' == op) {
            this->name = plus;
        }
        else if ('-' == op) {
            this->name = minus;
        }
        else if ('*' == op) {
            this->name = multiply;
        }
        else if ('/' == op) {
            this->name = divide;
        }
        else if ('^' == op) {
            this->name = power;
        }
        else if ('(' == op) {
            this->name = left_parenthesis;
        }
        else if (')' == op) {
            this->name = right_parenthesis;
        }
    }

    CompareResult compare(const Operator &other_operator) {
        int self_priority, other_priority;
        if (this->name == plus || this->name == minus) {
            self_priority = 2;
        }
        else if (this->name == multiply || this->name == divide) {
            self_priority = 4;
        }
        else if (this->name == power) {
            self_priority = 6;
        }
        else if (this->name == left_parenthesis) {
            self_priority = 8;
        }
        else if (this->name == right_parenthesis) {
            self_priority = 1;
        }

        if (other_operator.name == plus || other_operator.name == minus) {
            other_priority = 3;
        }
        else if (other_operator.name == multiply ||
                 other_operator.name == divide) {
            other_priority = 5;
        }
        else if (other_operator.name == power) {
            other_priority = 7;
        }
        else if (other_operator.name == left_parenthesis) {
            other_priority = 1;
        }
        else if (other_operator.name == right_parenthesis) {
            other_priority = 9;
        }

        if (self_priority < other_priority) {
            return less;
        }
        else if (self_priority == other_priority) {
            return equal;
        }
        else {
            return greater;
        }
    }

    Symbol calc(const Symbol first_pop, const Symbol second_pop) {
        if (this->name == plus) {
            return Symbol(first_pop.get_value() + second_pop.get_value());
        }
        else if (this->name == minus) {
            return Symbol(second_pop.get_value() - first_pop.get_value());
        }
        else if (this->name == multiply) {
            return Symbol(first_pop.get_value() * second_pop.get_value());
        }
        else if (this->name == divide) {
            if (abs(first_pop.get_value()) <= 1e-5) {
                return Symbol(atof("inf"));
            }
            return Symbol(second_pop.get_value() / first_pop.get_value());
        }
        else if (this->name == power) {
            return Symbol(pow(second_pop.get_value(), first_pop.get_value()));
        }

        return 0.0;
    }

    bool operator==(Operators op) {
        return this->name == op;
    }

    char display() {
        switch (this->name) {
        case plus:
            return '+';
        case minus:
            return '-';
        case multiply:
            return '*';
        case divide:
            return '/';
        case power:
            return '^';
        case left_parenthesis:
            return '(';
        case right_parenthesis:
            return ')';
        }
    }
};

class Calculator {
private:
    String expression;
    List<Symbol> number_stack;
    List<Operator> operator_stack;
    int left_parenthesis_count;
    void calculate() {
        Operator operation = this->operator_stack.pop();
        // this->operator_stack.pop();
        Symbol number1 = this->number_stack.pop();
        // this->number_stack.pop();
        Symbol number2 = this->number_stack.pop();
        // this->number_stack.pop();

        this->number_stack.append(operation.calc(number1, number2));
    }

public:
    Calculator(const String expression) {
        this->expression = expression + ')';
    }

    double evaluate() {
        this->number_stack.clear();

        this->operator_stack.clear();

        this->operator_stack.append(Operator('('));
        this->left_parenthesis_count = 1;

        char operator_chars[] = "+-*/^()";
        int operator_pos = -1;
        while (true) {
            if (TRACE) {
                Serial.println("A");
                Serial.print("Operator pos: ");
                Serial.println(operator_pos);
                Serial.println("Number stack: ");
                for (int i = 0; i < this->number_stack.length(); i++) {
                    Serial.println(this->number_stack[i].get_value());
                }
                Serial.println("Operator stack: ");
                for (int i = 0; i < this->operator_stack.length(); i++) {
                    Serial.println(this->operator_stack[i].display());
                }
            }

            bool found_next_operator = false;
            int first_char_of_num = operator_pos + 1;
            // operator_pos =
            //     this->expression.indexOf(operator_chars, operator_pos + 1);

            operator_pos = this->expression.length();

            for (int i = 0; i < sizeof(operator_chars); i++) {
                int current_operator_pos = this->expression.indexOf(
                    operator_chars[i], first_char_of_num);
                if (current_operator_pos < operator_pos &&
                    current_operator_pos >= 0) {
                    found_next_operator = true;
                    operator_pos = current_operator_pos;
                }
            }

            if (!found_next_operator) {
                break;
            }

            // Find the operator position
            if (TRACE) {
                Serial.println("B");
                Serial.print("Operator pos: ");
                Serial.println(operator_pos);
                Serial.println("Number stack: ");
                for (int i = 0; i < this->number_stack.length(); i++) {
                    Serial.println(this->number_stack[i].get_value());
                }
                Serial.println("Operator stack: ");
                for (int i = 0; i < this->operator_stack.length(); i++) {
                    Serial.println(this->operator_stack[i].display());
                }
            }

            if (operator_pos != first_char_of_num) {
                String number =
                    this->expression.substring(first_char_of_num, operator_pos);
                if (number == "x" || number == "y") {
                    this->number_stack.append(Symbol(number));
                }
                else {
                    this->number_stack.append(Symbol(atof(number.c_str())));
                }
            }
            else if (this->expression[operator_pos] == '-' &&
                     this->number_stack.length() ==
                         this->operator_stack.length() -
                             this->left_parenthesis_count) {
                this->number_stack.append(0.0);
            }

            // Understand the number and push to stack
            if (TRACE) {
                Serial.println("C");
                Serial.print("Operator pos: ");
                Serial.println(operator_pos);
                Serial.println("Number stack: ");
                for (int i = 0; i < this->number_stack.length(); i++) {
                    Serial.println(this->number_stack[i].get_value());
                }
                Serial.println("Operator stack: ");
                for (int i = 0; i < this->operator_stack.length(); i++) {
                    Serial.println(this->operator_stack[i].display());
                }
            }

            Operator current_operator(this->expression[operator_pos]);
            if (current_operator == left_parenthesis) {
                this->left_parenthesis_count += 1;
            }

            // Understand the operator
            if (TRACE) {
                Serial.println("D");
                Serial.print("Read operator: ");
                Serial.println(current_operator.display());
                Serial.print("Operator pos: ");
                Serial.println(operator_pos);
                Serial.println("Number stack: ");
                for (int i = 0; i < this->number_stack.length(); i++) {
                    Serial.println(this->number_stack[i].get_value());
                }
                Serial.println("Operator stack: ");
                for (int i = 0; i < this->operator_stack.length(); i++) {
                    Serial.println(this->operator_stack[i].display());
                }
            }

            switch (current_operator.compare(this->operator_stack.peek())) {
            case greater:
                // Serial.println("Choose greater");
                this->operator_stack.append(current_operator);
                break;
            case equal:
                // Serial.println("Choose equal");
                this->operator_stack.pop();
                this->left_parenthesis_count -= 1;
                break;
            case less:
                // Serial.println("Choose less");
                if (this->number_stack.length() < 2) {
                    // Error
                    return atof("nan");
                }
                // Serial.println("Into Calc");

                this->calculate();

                // Serial.println("Out Calc");
                // We only dealt with previous operators. Need to go through
                // this operator again.
                operator_pos--;
                break;
            }

            if (TRACE) {
                Serial.println("E");
                Serial.print("Operator pos: ");
                Serial.println(operator_pos);
                Serial.println("Number stack: ");
                for (int i = 0; i < this->number_stack.length(); i++) {
                    Serial.println(this->number_stack[i].get_value());
                }
                Serial.println("Operator stack: ");
                for (int i = 0; i < this->operator_stack.length(); i++) {
                    Serial.println(this->operator_stack[i].display());
                }
            }
        }

        if (this->left_parenthesis_count != 0) {
            return 0.0;
        }
        return this->number_stack.peek().get_value();
    }
};

enum Modes {
    CalculateAndDraw,
    Cube,
    Rain,
    Clock,
    Words,
};

enum ClockStatus {
    Normal,
    AdjustingHour,
    AdjustingMinute,
};

int Cube::layer_count = 0;
int Cube::brightness_count = 0;
uint16_t Cube::LED_status[8][8] = {0};
uint16_t Cube::LED_brightness[8][8] = {0};
uint8_t Cube::LED_blinking_status[8][8] = {0};
bool Cube::blinking_LED_status = false;

double *Symbol::x_value_ptr, *Symbol::y_value_ptr;

const uint32_t SensorReader::c_uiBaud[8] = {0,     4800,  9600,   19200,
                                            38400, 57600, 115200, 230400};
volatile byte SensorReader::s_cDataUpdate;
Triple SensorReader::angle, SensorReader::acceleration, SensorReader::gyro;

double symbol_x, symbol_y;
const String allowed_chars = "0123456789+-*/^()xy";
String input = "";
int x_offset = 0, y_offset = 0, z_offset = 0;
double zoom = 1.0;
float x_ref = 0.0, y_ref = 0.0, z_ref = 0.0;
bool waiting_for_command = true;

Modes current_mode = Modes::Rain;
bool can_change_mode = true;

byte cube_size = 1;
byte cube_step = 1;

bool rain_create_new = false;

String display_string = "MUELSYSE";
int letter_create_layer_count = 0;
// When count to 7, it's time to draw the new letter.
int next_char_ptr = 0;

int clock_hour = 0, clock_minute = 0, clock_sec = 0, clock_sec_flip = 0;
ClockStatus clock_status;
bool clock_waiting_for_new_command = true;

void setup() {
    for (int i = 22; i < 46; i++) {
        pinMode(i, OUTPUT);
    }

    pinMode(JOYSTICK_SWITCH, INPUT_PULLUP);
    pinMode(JOYSTICK_VRX, INPUT);
    pinMode(JOYSTICK_VRY, INPUT);

    Timer1.initialize();
    Timer1.setPeriod(TIME_PER_LAYER_IN_US);

    Serial.begin(9600);

    if (DEBUG) {
        Serial.println("Into setup");
    }

    Symbol::x_value_ptr = &symbol_x;
    Symbol::y_value_ptr = &symbol_y;

    if (!SensorReader::init()) {
        Serial.println("Initialize sensor failed.");
    }
    else {
        if (DEBUG) {
            Serial.println("Initialize sensor success.");
        }
    }

    Timer1.attachInterrupt(Cube::display);

    cli();
    // timer 4 is for blinking
    TCCR4A = 0; // set entire TCCR1A register to 0
    TCCR4B = 0; // same for TCCR1B
    TCNT4 = 0;  // initialize counter value to 0
    // set compare match register for 1hz increments
    OCR4A = 6500 / 1; // = (16*10^6) / (1*1024) - 1 (must be <65536)
    // 15625 = 1 sec
    // turn on CTC mode
    TCCR4B |= (1 << WGM12);
    // Set CS12 and CS10 bits for 1024 prescaler
    TCCR4B |= (1 << CS12) | (1 << CS10);
    // enable timer compare interrupt
    TIMSK4 |= (1 << OCIE4A);

    // timer 5 is for read sensor data
    TCCR5A = 0;
    TCCR5B = 0;
    TCNT5 = 0;
    OCR5A = 3125 / 1;
    TCCR5B |= (1 << WGM12);
    TCCR5B |= (1 << CS12) | (1 << CS10);
    TIMSK5 |= (1 << OCIE5A);
    sei();

    attachInterrupt(digitalPinToInterrupt(JOYSTICK_SWITCH), on_joystick_button,
                    FALLING);

    if (DEBUG) {
        Serial.println("ALL ON");
    }

    for (int i = 0; i < 8; i++) {
        for (int j = 0; j < 8; j++) {
            for (int k = 0; k < 8; k++) {
                Cube::set_status(i, j, k, 3);
            }
        }
    }

    delay(5000);
    Cube::clear();

    // Wait for z_ref to have value;
    while (SensorReader::angle.z == 0.0 || SensorReader::angle.y == 0.0 ||
           SensorReader::angle.x == 0.0) {
        Serial.print("");
        continue;
    }

    x_ref = SensorReader::angle.x;
    y_ref = SensorReader::angle.y;
    z_ref = SensorReader::angle.z;

    if (DEBUG) {
        Serial.print("X_ref: ");
        Serial.println(x_ref);
        Serial.print("Y_ref: ");
        Serial.println(y_ref);
        Serial.print("Z_ref: ");
        Serial.println(z_ref);
    }

    Serial.println("Setup complete.");
}

void loop() {
    if (can_change_mode && clock_status == ClockStatus::Normal) {
        if (analogRead(JOYSTICK_VRY) > 974) {
            current_mode = (enum Modes)(current_mode + 1) % 5;
            can_change_mode = false;
            Cube::clear();
            Serial.print("Changed to mode: ");
            Serial.println(display_mode_name(current_mode));
        }
        else if (analogRead(JOYSTICK_VRY) < 50) {
            current_mode = (enum Modes)(current_mode + 4) % 5;
            can_change_mode = false;
            Cube::clear();
            Serial.print("Changed to mode: ");
            Serial.println(display_mode_name(current_mode));
        }
    }
    else {
        if (analogRead(JOYSTICK_VRY) > 400 && analogRead(JOYSTICK_VRY) < 624) {
            can_change_mode = true;
        }
    }
    switch (current_mode) {
    case Modes::CalculateAndDraw:
        calculate_and_draw();
        break;
    case Modes::Clock:
        clock();
        break;
    case Modes::Cube:
        cube();
        break;
    case Modes::Rain:
        rain();
        break;
    case Modes::Words:
        words();
        break;
    default:
        current_mode = Modes::Rain;
    }
}

ISR(TIMER4_COMPA_vect) {
    Cube::do_blinking();
}

ISR(TIMER5_COMPA_vect) {
    SensorReader::read_data_from_sensor_interrupt();

    if (clock_status == ClockStatus::Normal) {
        clock_sec_flip++;
        clock_sec += clock_sec_flip / 4;
        clock_minute += clock_sec / 60;
        clock_hour += clock_minute / 60;

        clock_sec_flip %= 4;
        clock_sec %= 60;
        clock_minute %= 60;
        clock_hour %= 24;
    }
}

void cube() {
    Cube::clear();
    Cube::draw_line(3 - cube_size, 3 - cube_size, 3 - cube_size,
                    2 * cube_size + 2, 0, 3);
    Cube::draw_line(3 - cube_size, 4 + cube_size, 3 - cube_size,
                    2 * cube_size + 2, 0, 3);
    Cube::draw_line(3 - cube_size, 3 - cube_size, 4 + cube_size,
                    2 * cube_size + 2, 0, 3);
    Cube::draw_line(3 - cube_size, 4 + cube_size, 4 + cube_size,
                    2 * cube_size + 2, 0, 3);

    Cube::draw_line(3 - cube_size, 3 - cube_size, 3 - cube_size,
                    2 * cube_size + 2, 1, 3);
    Cube::draw_line(4 + cube_size, 3 - cube_size, 3 - cube_size,
                    2 * cube_size + 2, 1, 3);
    Cube::draw_line(3 - cube_size, 3 - cube_size, 4 + cube_size,
                    2 * cube_size + 2, 1, 3);
    Cube::draw_line(4 + cube_size, 3 - cube_size, 4 + cube_size,
                    2 * cube_size + 2, 1, 3);

    Cube::draw_line(3 - cube_size, 3 - cube_size, 3 - cube_size,
                    2 * cube_size + 2, 2, 3);
    Cube::draw_line(3 - cube_size, 4 + cube_size, 3 - cube_size,
                    2 * cube_size + 2, 2, 3);
    Cube::draw_line(4 + cube_size, 3 - cube_size, 3 - cube_size,
                    2 * cube_size + 2, 2, 3);
    Cube::draw_line(4 + cube_size, 4 + cube_size, 3 - cube_size,
                    2 * cube_size + 2, 2, 3);

    delay(75);
    cube_size += cube_step;
    if (cube_size >= 3) {
        cube_step = -1;
    }
    else if (cube_size <= 0) {
        cube_step = 1;
    }
}

void rain() {
    for (int z = 0; z < 7; z++) {
        for (int x = 0; x < 8; x++) {
            for (int y = 0; y < 8; y++) {
                int upper_state = Cube::get_status(x, y, z + 1);
                if (upper_state > 0) {
                    Cube::set_status(x, y, z, upper_state);
                    Cube::set_status(x, y, z + 1, upper_state - 1);
                }
                else {
                    Cube::set_status(x, y, z, 0);
                }
            }
        }
    }

    if (rain_create_new) {
        int num = random(64);
        Cube::set_status(num / 8, num % 8, 7, 3);
        rain_create_new = false;
    }
    else {
        rain_create_new = true;
    }
}

void clock() {
    int VRX_status = analogRead(JOYSTICK_VRX);
    if (clock_waiting_for_new_command) {
        if (clock_status == ClockStatus::AdjustingHour) {
            if (VRX_status < 50) {
                clock_hour = (clock_hour + 23) % 24;
                clock_waiting_for_new_command = false;
            }
            else if (VRX_status > 974) {
                clock_hour = (clock_hour + 1) % 24;
                clock_waiting_for_new_command = false;
            }
        }
        else if (clock_status == ClockStatus::AdjustingMinute) {
            if (VRX_status < 50) {
                clock_minute = (clock_minute + 59) % 60;
                clock_waiting_for_new_command = false;
            }
            else if (VRX_status > 974) {
                clock_minute = (clock_minute + 1) % 60;
                clock_waiting_for_new_command = false;
            }
        }
    }
    else {
        if (VRX_status > 400 && VRX_status < 624) {
            clock_waiting_for_new_command = true;
        }
    }

    Cube::draw_num(0, 1, 7, clock_hour / 10, 2, 3);
    Cube::draw_num(4, 1, 7, clock_hour % 10, 2, 3);
    Cube::draw_num(0, 0, 3, clock_minute / 10, 2, 3);
    Cube::draw_num(4, 0, 3, clock_minute % 10, 2, 3);
}

void words() {
    if (Serial.available()) {
        byte input_char = Serial.read();
        if (input_char >= 'A' && input_char <= 'Z') {
            display_string.concat((char)input_char);
        }

        if (input_char >= 'a' && input_char <= 'z') {
            display_string.concat((char)(input_char - 'a' + 'A'));
        }
        if (input_char >= '0' && input_char <= '9') {
            display_string.concat((char)input_char);
        }

        if (input_char == '?') {
            Serial.print("Current string: ");
            Serial.println(display_string);
        }

        if (input_char == '/') {
            display_string = "";
            next_char_ptr = 0;
            Serial.println("Cleared input.");
        }
    }

    delay(50);

    // Move everything one light forward.
    for (int i = 0; i < 8; i++) {
        for (int j = 0; j < 8; j++) {
            Cube::LED_brightness[i][j] = Cube::LED_brightness[i][j] >> 2;
            Cube::LED_status[i][j] = Cube::LED_status[i][j] >> 2;
        }
    }

    letter_create_layer_count++;

    if (letter_create_layer_count < 7) {
        // The previous have not been moved out
        return;
    }

    letter_create_layer_count = 0;

    if (next_char_ptr == display_string.length()) {
        // Displayed all chars. 'Display' a space and reverse to the first char.
        next_char_ptr = 0;
        return;
    }

    if (display_string[next_char_ptr] >= 'A') {
        Cube::draw_letter(0, 7, 7, display_string[next_char_ptr] - 'A', 2, 3);
    }
    else {
        Serial.print(display_string[next_char_ptr]);
        Cube::draw_letter(0, 7, 7, display_string[next_char_ptr] - '0' + 26, 2,
                          3);
    }

    next_char_ptr++;
}

void calculate_and_draw() {
    bool need_reevaluate = false;

    if (Serial.available()) {
        if (DEBUG) {
            Serial.println(input);
        }
        byte input_char = Serial.read();
        // Serial.print("Pos: ");
        // Serial.println(allowed_chars.indexOf(input_char));
        if (allowed_chars.indexOf(input_char) != -1) {
            input.concat((char)input_char);
            // Serial.print("String: ");
            // Serial.println(input);
        }

        if (input_char == '=') {
            need_reevaluate = true;
        }

        if (input_char == 'c') {
            input = "";
            Serial.println("Cleared input string.");
            return;
        }

        if (input_char == 'h') {
            Serial.print("Current expression: ");
            if (input.length() == 0) {
                Serial.println("(none)");
            }
            else {
                Serial.println(input);
            }
            Serial.println("Use c to clear input.");
        }
        // Prioritize user input.
    }

    if (waiting_for_command) {
        if (SensorReader::angle.x - x_ref > 40.0) {
            if (DEBUG) {
                Serial.println("a");
            }
            y_offset += 1;
            need_reevaluate = true;
            waiting_for_command = false;
        }
        else if (SensorReader::angle.x - x_ref < -40.0) {
            if (DEBUG) {
                Serial.println("b");
            }
            y_offset -= 1;
            need_reevaluate = true;
            waiting_for_command = false;
        }

        if (SensorReader::angle.y - y_ref > 40.0) {
            if (DEBUG) {
                Serial.println("c");
            }
            x_offset -= 1;
            need_reevaluate = true;
            waiting_for_command = false;
        }
        else if (SensorReader::angle.y - y_ref < -40.0) {
            if (DEBUG) {
                Serial.println("d");
            }
            x_offset += 1;
            need_reevaluate = true;
            waiting_for_command = false;
        }

        if (SensorReader::angle.z - z_ref > 40.0) {
            if (DEBUG) {
                Serial.println(SensorReader::angle.z);
            }
            Serial.println("Zoom out");
            zoom /= 2.0;
            need_reevaluate = true;
            waiting_for_command = false;
        }
        else if (SensorReader::angle.z - z_ref < -40.0) {
            if (DEBUG) {
                Serial.println(SensorReader::angle.z);
            }
            Serial.println("Zoom in");
            zoom *= 2.0;
            need_reevaluate = true;
            waiting_for_command = false;
        }

        if (analogRead(JOYSTICK_VRX) < 50) {
            z_offset -= 1;
            need_reevaluate = true;
            waiting_for_command = false;
        }
        else if (analogRead(JOYSTICK_VRX) > 974) {
            z_offset += 1;
            need_reevaluate = true;
            waiting_for_command = false;
        }

        if (abs(SensorReader::acceleration.x) +
                abs(SensorReader::acceleration.y) +
                abs(SensorReader::acceleration.z) >
            120.0) {
            if (DEBUG) {
                Serial.print("g");
            }
            x_offset = 0;
            y_offset = 0;
            z_offset = 0;
            zoom = 1;
            Serial.println("Reset zoom and translate.");
            need_reevaluate = true;
            waiting_for_command = false;
        }

        if (abs(SensorReader::gyro.x) + abs(SensorReader::gyro.y) +
                abs(SensorReader::gyro.z) >
            240) {
            if (DEBUG) {
                Serial.println("h");
                Serial.print(SensorReader::gyro.x);
                Serial.print(" ");
                Serial.print(SensorReader::gyro.y);
                Serial.print(" ");
                Serial.println(SensorReader::gyro.z);
            }
            x_offset = 0;
            y_offset = 0;
            z_offset = 0;
            zoom = 1;
            Serial.println("Reset zoom and translate.");
            need_reevaluate = true;
            waiting_for_command = false;
        }
    }
    else {
        if (DEBUG) {
            Serial.print(abs(SensorReader::angle.x - x_ref) < 20.0 ? "true"
                                                                   : "false");
            Serial.print(" ");
            Serial.print(abs(SensorReader::angle.y - y_ref) < 20.0 ? "true"
                                                                   : "false");
            Serial.print(" ");
            Serial.print(abs(SensorReader::angle.z - z_ref) < 20.0 ? "true"
                                                                   : "false");
            Serial.print(" ");
            Serial.println(analogRead(JOYSTICK_VRX));
        }

        if (abs(SensorReader::angle.x - x_ref) < 20.0 &&
            abs(SensorReader::angle.y - y_ref) < 20.0 &&
            abs(SensorReader::angle.z - z_ref) < 20.0 &&
            analogRead(JOYSTICK_VRX) > 400 && analogRead(JOYSTICK_VRX) < 648) {
            waiting_for_command = true;
            Serial.println("Waiting for new command");
        }
    }

    if (!need_reevaluate || input.length() == 0) {
        return;
    }

    Serial.print("Expression to be evaluated: ");
    Serial.println(input);

    if (DEBUG) {
        Serial.print("Offset: ");
        Serial.print(x_offset);
        Serial.print(" ");
        Serial.print(y_offset);
        Serial.print(" ");
        Serial.println(z_offset);
        Serial.print("Zoom: ");
        Serial.println(zoom);
    }

    Calculator calculator(input);

    Cube::clear();

    for (int i = 0; i < 8; i++) {
        for (int j = 0; j < 8; j++) {
            symbol_x = (i + x_offset) * zoom;
            symbol_y = (j + y_offset) * zoom;
            double result = calculator.evaluate();
            int z;
            if (!(isinf(result) || isnan(result))) {
                z = round((result + z_offset) * zoom);
                Cube::set_status(i, j, z, 3);
            }
            else {
                z = 0;
            }

            // Serial.print(x);
            // Serial.print(" ");
            // Serial.print(y);
            // Serial.print(" ");
            // Serial.println(z);

            // Display origin point
            Cube::set_status(round(-x_offset), round(-y_offset),
                             round(z_offset), 3);
            Cube::set_blinking(round(-x_offset), round(-y_offset),
                               round(z_offset));
        }
    }
}

void on_joystick_button() {
    if (current_mode == Modes::CalculateAndDraw) {
        x_ref = SensorReader::angle.x;
        y_ref = SensorReader::angle.y;
        z_ref = SensorReader::angle.z;
        Serial.println("Reset position.");
        return;
    }
    if (current_mode == Modes::Clock) {
        switch (clock_status) {
        case ClockStatus::Normal: {
            if (DEBUG) {
                Serial.println("Start adjusting hour");
            }
            for (int i = 0; i < 8; i++) {
                for (int j = 4; j < 8; j++) {
                    // Cube::set_blinking(i, 1, j);
                    Cube::LED_blinking_status[j][i] = 0xff;
                }
            }
            // Cube::LED_blinking_status[0][0] = 0x1;
            clock_status = ClockStatus::AdjustingHour;
            break;
        }
        case ClockStatus::AdjustingHour: {
            if (DEBUG) {
                Serial.println("Start adjucting minute");
            }
            for (int i = 0; i < 8; i++) {
                for (int j = 4; j < 8; j++) {
                    // Cube::unset_blinking(i, 1, j);
                    Cube::LED_blinking_status[j][i] = 0;
                }
                for (int j = 0; j < 4; j++) {
                    // Cube::set_blinking(i, 0, j);
                    Cube::LED_blinking_status[j][i] = 0xff;
                }
            }
            clock_status = ClockStatus::AdjustingMinute;
            break;
        }
        case ClockStatus::AdjustingMinute: {
            if (DEBUG) {
                Serial.println("Normal mode");
            }
            for (int i = 0; i < 8; i++) {
                for (int j = 0; j < 4; j++) {
                    Cube::LED_blinking_status[j][i] = 0;
                }
            }
            clock_sec = 0;
            clock_sec_flip = 0;
            clock_status = ClockStatus::Normal;
            break;
        }
        default:
            clock_status = ClockStatus::Normal;
        }
    }
}

String display_mode_name(Modes mode_name) {
    switch (mode_name) {
    case Modes::CalculateAndDraw:
        return String("Draw function graph");
    case Modes::Cube:
        return String("Cube");
    case Modes::Rain:
        return String("Rain");
    case Modes::Clock:
        return String("Clock");
    case Modes::Words:
        return String("Display Words");
    default:
        return String("Unknown");
    }
}