// **************************************************************************
//
//               Demo program for labs
//
// Subject:      Computer Architectures and Parallel systems
// Author:       Petr Olivka, petr.olivka@vsb.cz, 08/2016
// Organization: Department of Computer Science, FEECS,
//               VSB-Technical University of Ostrava, CZ
//
// File:         Main program for LCD module
//
// **************************************************************************

#include "mbed.h"
#include "lcd_lib.h"
#include <font12x20_lsb.h>
#include "FXOS8700Q.h"
I2C i2c(PTE25,PTE24);
//#include "lcd-simulator.hpp"
//#include "fonts/font10x16_lsb.hpp"

// Serial line for printf output
//Serial pc(USBTX, USBRX);

// two dimensional array with fixed size font
extern uint8_t g_font8x8[256][8];

#define width 10
#define height 16

struct Point2D
{
    int32_t x, y;
    Point2D(int32_t x, int32_t y) : x(x), y(y) {
    }
};

struct RGB
{
    uint8_t r, g, b;
    RGB(uint8_t r, uint8_t g, uint8_t b) : r(r), g(g), b(b) {
    }
};

class GraphElement
{
public:
    // foreground and background color
    RGB fg_color, bg_color;

    // constructor
    GraphElement(RGB t_fg_color, RGB t_bg_color) :
        fg_color(t_fg_color), bg_color(t_bg_color) {}

    // ONLY ONE INTERFACE WITH LCD HARDWARE!!!
    void drawPixel(int32_t t_x, int32_t t_y) { lcd_put_pixel(t_x, t_y, convert_RGB888_to_RGB565(fg_color)); }

    // Draw graphics element
    virtual void draw() = 0;

    // Hide graphics element
    virtual void hide() { swap_fg_bg_color(); draw(); swap_fg_bg_color(); }
    void setColor(RGB c) { fg_color = c; }
private:
    // swap foreground and backgroud colors
    void swap_fg_bg_color() { RGB l_tmp = fg_color; fg_color = bg_color; bg_color = l_tmp; }

    // conversion of 24-bit RGB color into 16-bit color format
    int convert_RGB888_to_RGB565(RGB t_color) {
        int red = (t_color.r >> 3) << 11;
        int green = (t_color.g >> 2) << 5;
        int blue = t_color.b >> 3;
        return red | green | blue;
    }
};

class Pixel : public GraphElement
{
public:
    // constructor
    Pixel(Point2D t_pos, RGB t_fg_color, RGB t_bg_color) : pos(t_pos), GraphElement(t_fg_color, t_bg_color) {}
    // Draw method implementation
    virtual void draw() { drawPixel(pos.x, pos.y); }
    // Position of Pixel
    Point2D pos;
};

class Character : public GraphElement
{
public:
    // position of character
    Point2D pos;
    // character
    char character;

    Character(Point2D t_pos, char t_char, RGB t_fg, RGB t_bg) :
        pos(t_pos), character(t_char), GraphElement(t_fg, t_bg) {};

    void draw() {
        for (int i = 0; i < height; i++) {
            for (int j = 0; j < width; j++) {
                if (font[character][i] & (1 << j))
                    drawPixel(j + pos.x, i + pos.y);
                //else
                    //lcd_put_pixel(j + pos.x, i + pos.y, 0x001F);
            }
        }
    }

};

class Circle : public GraphElement
{
public:
    // Center of circle
    Point2D center;
    // Radius of circle
    int32_t radius;

    Circle(Point2D t_center, int32_t t_radius, RGB t_fg, RGB t_bg) :
        center(t_center), radius(t_radius), GraphElement(t_fg, t_bg) {};

    void draw() {
        int f = 1 - radius;

        int x = 0;
        int y = radius;

        int ddF_x = 0;
        int ddF_y = -2 * radius;

        drawPixel(center.x, center.y + radius);
        drawPixel(center.x, center.y - radius);
        drawPixel(center.x + radius, center.y);
        drawPixel(center.x - radius, center.y);

        while (x < y) {
            if (f >= 0) {
                y--;
                ddF_y += 2;
                f += ddF_y;
            }

            x++;
            ddF_x += 2;
            f += ddF_x + 1;
            drawPixel(center.x + x, center.y + y);
            drawPixel(center.x - x, center.y + y);
            drawPixel(center.x + x, center.y - y);
            drawPixel(center.x - x, center.y - y);
            drawPixel(center.x + y, center.y + x);
            drawPixel(center.x - y, center.y + x);
            drawPixel(center.x + y, center.y - x);
            drawPixel(center.x - y, center.y - x);
        }
    }
};

class Line : public GraphElement
{
public:
    // the first and the last point of line
    Point2D pos1, pos2;

    Line(Point2D t_pos1, Point2D t_pos2, RGB t_fg, RGB t_bg) :
        pos1(t_pos1), pos2(t_pos2), GraphElement(t_fg, t_bg) {}

    void draw() {
        drawLine(pos1.x,pos1.y, pos2.x, pos2.y);
    };

private:
   void drawLine(int x1, int y1, int x2, int y2) {
    int x,y,dx,dy,dx1,dy1,px,py,xe,ye,i;
    dx = x2-x1;
    dy = y2-y1;

    dx1 = fabs(dx);
    dy1 = fabs(dy);

    px=2 * dy1 - dx1;
    py=2 * dx1 - dy1;

    if(dy1<=dx1) {
        if(dx>=0) {
            x=x1;
            y=y1;
            xe=x2;
        } else {
            x=x2;
            y=y2;
            xe=x1;
        }
        drawPixel(x, y);
        for(i=0;x<xe;i++) {
            x=x+1;
            if(px<0) {
                px=px+2*dy1;
            } else {
                if((dx<0 && dy<0) || (dx>0 && dy>0)) {
                    y=y+1;
                } else {
                    y=y-1;
                }
                px=px+2*(dy1-dx1);
            }
            drawPixel(x, y);
        }
     } else {
         if(dy>=0) {
             x=x1;
             y=y1;
             ye=y2;
         } else {
             x=x2;
             y=y2;
             ye=y1;
         }
         drawPixel(x, y);
         for(i=0;y<ye;i++) {
             y=y+1;
             if(py<=0) {
                 py=py+2*dx1;
             } else {
                 if((dx<0 && dy<0) || (dx>0 && dy>0)) {
                     x=x+1;
                 } else {
                     x=x-1;
                 }
                 py=py+2*(dx1-dy1);
             }
             drawPixel(x, y);
         }
     }
}
};

#define LCD_WIDTH 320
#define LCD_HEIGHT 240

Serial pc(USBTX, USBRX);
int main(){
    lcd_init();
    lcd_clear();
    pc.baud(115200);
    FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1);

    motion_data_units_t acc_data;
    float faX, faY, faZ;
    acc.enable();

    struct Point2D p1(10, 10);
    struct Point2D p2(310, 10);

    struct Point2D p3(310, 230);
    struct Point2D p4(10, 230);

    struct Point2D p5(10, 120);
    struct Point2D p6(310, 120);

    struct RGB fgColor(0, 255, 0);
    struct RGB fgColor2(255, 0, 0);
    struct RGB fgColor3(0, 0, 255);

    struct RGB bgColor(0, 0, 0);

    Line l1(p1, p2, fgColor, bgColor);
    l1.draw();
    Line l2(p2, p3, fgColor, bgColor);
    l2.draw();

    Line l3(p4, p3, fgColor, bgColor);
    l3.draw();
    Line l4(p1, p4, fgColor, bgColor);
    l4.draw();

    Line l5(p5, p6, fgColor, bgColor);
    l5.draw();

    struct Point2D mid(10, 120);
    struct Point2D endOfx(10, 120);

    struct Point2D mid2(10, 120);
    struct Point2D endOfy(10, 120);

    struct Point2D mid3(10, 120);
    struct Point2D endOfz(10, 120);

    while (1) {
        Line lx(mid, endOfx, fgColor, bgColor);
        Line lx2(mid2, endOfy, fgColor2, bgColor);
        Line lx3(mid3, endOfz, fgColor3, bgColor);

        acc.getAxis(acc_data);
        acc.getX(faX);
        acc.getY(faY);
        acc.getZ(faZ);

        endOfx.y += 100 * faX;
        //endOfx.x += 10;

        endOfy.y += 100 * faY;
        //endOfy.x += 10;

        endOfz.y += 100 * faZ;
        //endOfz.x += 10;

        lx.draw();
        lx2.draw();
        //lx3.draw();

        mid = endOfx;
        mid2 = endOfy;
        mid3 = endOfz;

        endOfx.y += 100 * faX;
        endOfx.x += 10;

        endOfy.y += 100 * faY;
        endOfy.x += 10;

        endOfz.y += 100 * faZ;
        endOfz.x += 10;

        wait(1);
        //lx.hide();
        //pc.printf("acc: %5.2f,%5.2f,%5.2f          \r",faX,faY,faZ);


    }
}