#include <Arduino.h>
#include <SPI.h>
#include "mcp_can.h"
#include <U8g2lib.h>
#include <TaskScheduler.h>

#define debug
#define CAN_CS_PIN  49
#define CAN_SPEED   CAN_500KBPS
#define GLCD_CS_PIN 48

Scheduler task; 
MCP_CAN CAN(CAN_CS_PIN); 
U8G2_ST7920_128X64_1_8080 lcd(U8G2_R2, 5, 6, 7, 8, 9, 10, 11, 12, 4, U8X8_PIN_NONE, 3, 2);

#define Solarpanel_width 64
#define Solarpanel_height 56
static unsigned char Solarpanel_bits[] = {
   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07,
   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x88, 0x07, 0x00, 0x00, 0x00,
   0x00, 0x00, 0x1f, 0x7c, 0x08, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x90, 0x0f,
   0x08, 0x00, 0x00, 0x00, 0x00, 0x0f, 0x70, 0x10, 0x18, 0x00, 0x00, 0x00,
   0xf0, 0x18, 0x0e, 0x10, 0x1e, 0x00, 0x00, 0x00, 0x0e, 0xf0, 0x09, 0xe0,
   0x21, 0x00, 0x00, 0x00, 0x02, 0x1e, 0x10, 0x78, 0x40, 0x00, 0x00, 0x00,
   0x02, 0x02, 0x10, 0x4f, 0x40, 0x00, 0x00, 0x00, 0x04, 0x03, 0xf0, 0x80,
   0xf0, 0x00, 0x00, 0x00, 0xe4, 0x02, 0x38, 0x80, 0x0d, 0x01, 0x00, 0x00,
   0x18, 0x84, 0x47, 0x80, 0x03, 0x01, 0x00, 0x00, 0x08, 0x6c, 0x40, 0x60,
   0x01, 0xf2, 0x0f, 0x00, 0x10, 0x18, 0x80, 0x18, 0x82, 0x01, 0x10, 0x00,
   0x10, 0x08, 0x80, 0x07, 0x64, 0x00, 0x20, 0x00, 0x10, 0x1e, 0xc0, 0x01,
   0x1c, 0x00, 0x20, 0x00, 0xe0, 0x11, 0x30, 0x01, 0x06, 0x00, 0x20, 0x00,
   0x20, 0x20, 0x0c, 0x82, 0x01, 0x00, 0x20, 0x00, 0x40, 0xa0, 0x03, 0x62,
   0x00, 0x00, 0x20, 0x00, 0x40, 0xc0, 0x00, 0x1c, 0x00, 0xf0, 0xff, 0x3f,
   0x80, 0x70, 0x00, 0x07, 0x00, 0x10, 0x00, 0x20, 0x80, 0x8c, 0x80, 0x01,
   0x00, 0x10, 0x00, 0x20, 0x80, 0x83, 0x60, 0x00, 0x00, 0x90, 0x4b, 0x2f,
   0x00, 0x00, 0x1d, 0x00, 0x00, 0x50, 0x48, 0x21, 0x00, 0x00, 0x07, 0x00,
   0x00, 0x50, 0x48, 0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x50, 0x78, 0x2d,
   0x00, 0x00, 0x00, 0x00, 0x00, 0x50, 0x48, 0x29, 0x00, 0x00, 0x00, 0x00,
   0x00, 0x50, 0x48, 0x29, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x4b, 0x2f,
   0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x20, 0x00, 0xe0, 0x01, 0x78,
   0x00, 0x10, 0x00, 0x20, 0x00, 0x10, 0x02, 0x84, 0x00, 0x10, 0x00, 0x20,
   0x00, 0x10, 0x02, 0x84, 0x00, 0xf0, 0xff, 0x3f, 0x00, 0x00, 0x00, 0x00,
   0x00, 0x00, 0x20, 0x00, 0x00, 0xff, 0xff, 0xff, 0x0f, 0x00, 0x20, 0x00,
   0x80, 0x01, 0x00, 0x00, 0x18, 0x00, 0x20, 0x00, 0x80, 0x00, 0x00, 0x00,
   0x10, 0x00, 0x20, 0x00, 0x80, 0x00, 0x00, 0x00, 0x10, 0x00, 0x20, 0x00,
   0x80, 0x00, 0x00, 0x00, 0x10, 0x00, 0x20, 0x00, 0x80, 0x00, 0x00, 0x00,
   0x10, 0x00, 0x20, 0x00, 0x80, 0x00, 0x00, 0x00, 0x10, 0x08, 0x20, 0x00,
   0x80, 0x00, 0x00, 0x30, 0x10, 0x08, 0x20, 0x00, 0x80, 0x00, 0x00, 0x30,
   0x10, 0x0c, 0x10, 0x00, 0x80, 0xf8, 0x03, 0xfc, 0x10, 0xfe, 0x0f, 0x00,
   0x80, 0xf8, 0x03, 0xfc, 0x10, 0x0c, 0x00, 0x00, 0x80, 0x00, 0x00, 0x30,
   0x10, 0x08, 0x00, 0x00, 0x80, 0x00, 0x00, 0x30, 0x10, 0x08, 0x00, 0x00,
   0x80, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00,
   0x10, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00,
   0x80, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00,
   0x10, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00,
   0x00, 0xff, 0xff, 0xff, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
   0x00, 0x00, 0x00, 0x00 };

#define up_width 7
#define up_height 7
static unsigned char up_bits[] = {
   0x00, 0x08, 0x14, 0x22, 0x41, 0x00, 0x00 };

#define down_width 7
#define down_height 7
static unsigned char down_bits[] = {
   0x00, 0x41, 0x22, 0x14, 0x08, 0x00, 0x00 };

unsigned char len = 0;
unsigned char buf[8];
bool CANtimeout = false; bool CANtimeout_blink = false;
unsigned long previousMillis = 0;
unsigned long currentMillis = 0;
//char str[20];
int lcd_categeory = 0;
int lcd_screen = 0;
int lcd_zahl, lcd_comma;
int c100_charger_cell_voltage = 0;
int c101_charger_cell_current = 0;
int c102_charger_cell_power = 0;
int c103_charger_battery_voltage = 0;
int c104_charger_battery_current = 0;
int c105_charger_cell_peakpower = 0;
int c106_charger_work_today = 0;
int c107_charger_work_yesterday = 0;
int c108_charger_work_total = 0;
int c109_charger_co2 = 0;
bool c10a_charger_error = 0;


void lcd_format_int(int a){
    lcd_zahl = a/10;
    lcd_comma = a%10;
}
void lcd_category_0() {
    lcd.setFont(u8g2_font_6x10_tr);
    lcd.drawStr(2,7,"PV-Module");
    lcd.drawLine(0, 8, 128, 8);
    lcd.drawLine(100, 0, 100, 8);
    lcd.drawXBM(106,0, down_width, down_height, down_bits);
    lcd.drawXBM(116,0, up_width, up_height, up_bits);
}

void lcd_print() {
    //Print LCD Variables
    if (CANtimeout == true && CANtimeout_blink == true) {
            lcd.firstPage();
        do {
            lcd.setFont(u8g2_font_10x20_tr);
            lcd.drawStr(0,20,"CAN Test");
            lcd.drawStr(48,35,"Project");
            lcd.setFont(u8g2_font_6x10_tr);
            lcd.drawStr(0,60,"CAN: NO DATA received");
            CANtimeout_blink = false;
    } while ( lcd.nextPage() );
    }
    else if (CANtimeout == true && CANtimeout_blink == false) {
        //CAN Timeout - Error
        lcd.firstPage();
        do {
            lcd.setFont(u8g2_font_10x20_tr);
            lcd.drawStr(0,20,"CAN Test");
            lcd.drawStr(48,35,"Project");
            lcd.drawBox(0, 50, 126, 14);
            lcd.setColorIndex(0);
            lcd.setFont(u8g2_font_6x10_tr);
            lcd.drawStr(0,60,"CAN: NO DATA received");
            lcd.setColorIndex(1);
            CANtimeout_blink = true;
    } while ( lcd.nextPage() );
    }
    if (CANtimeout == false && lcd_categeory == 0 && lcd_screen == 0) {
        //Solarzellen
        lcd.firstPage();
        do {
            lcd_category_0();
            lcd.drawXBM(0,9, Solarpanel_width, Solarpanel_height, Solarpanel_bits);
            //lcd_format_int(wert1);
            lcd.setCursor(80,18); lcd.print(lcd_zahl); lcd.print("."); lcd.print(lcd_comma); lcd.print("V");
            //lcd_format_int(wert2);
            lcd.setCursor(80,28); lcd.print(lcd_zahl); lcd.print("."); lcd.print(lcd_comma); lcd.print("W");
            } while ( lcd.nextPage() );
    }
}

Task t1(1000, TASK_FOREVER, &lcd_print);

void setup()
{
    #ifdef debug
    Serial.begin(115200);
    Serial.println("CAN Test Project - Display Unit");
    delay(1000);
    Serial.println("Init MCP2515 CAN Controller...");
    #endif

START_INIT:

    if(CAN_OK == CAN.begin(CAN_SPEED))
    {
        #ifdef debug
        Serial.println("Sucessfully initialized.");
        #endif
    }
    else
    {
        #ifdef debug
        Serial.println("Initialization failed, try again.");
        #endif
        delay(100);
        goto START_INIT;
    }
    #ifdef debug
    Serial.println("Init lcd...");
    #endif
    lcd.begin();
    lcd.firstPage();
    do {
        lcd.setFont(u8g2_font_10x20_tr);
        lcd.drawStr(0,20,"CAN Test");
        lcd.drawStr(48,35,"Project");
        lcd.setFont(u8g2_font_6x10_tr);
        lcd.drawStr(0,52,"by Carsten Schmiemann");
        lcd.drawStr(30,60,"2021, for Kaubi");
    } while ( lcd.nextPage() );
    delay(2000);
    //lcd.clear();
    #ifdef debug
    Serial.println("Init threads...");
    #endif
    task.init();
    task.addTask(t1);
    t1.enable();
}

void loop()
{
    task.execute();
    currentMillis = millis();
    //Read CAN Messages
    if(CAN_MSGAVAIL == CAN.checkReceive())
    {
        CAN.readMsgBuf(&len, buf);
        int canid = CAN.getCanId();

        #ifdef debug
        Serial.print("CAN packet received with ID "); Serial.println(canid);
        #endif

        currentMillis = millis();
        previousMillis = currentMillis; //Reset Timeout Counter
        CANtimeout = false; //Reset Timeout Counter

        switch (canid)
        {
        case 1:
            //wert1 = buf[0] + (buf[1] << 8);
            #ifdef debug
            Serial.print("Known CAN-ID: "); Serial.print(canid); Serial.print(", Value: "); //Serial.println(wert1);
            #endif
            break;
        
        case 2:
            //wert2 = buf[0] + (buf[1] << 8);
            #ifdef debug
            Serial.print("Known CAN-ID: "); Serial.print(canid); Serial.print(", Value: "); //Serial.println(wert2);
            #endif
            break;
        }
    }
    if (currentMillis - previousMillis >  8000) {
        CANtimeout = true;
    }
}