/*
 * Copyright (C) 2023 Daniele Lacamera <root@danielinux.net>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdint.h>
#include <stddef.h>
#include "ui.h"
#include "button.h"
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include "ksp-serial.h"
#include "led.h"
#include "adc.h"
#include <math.h>
#include "arm_math.h"
#include "ui.h"
#include "trig.h"


#define ROLL_CALIB (30)
#define PITCH_CALIB (0)

#define M_PI_INT 3

const char main_deck_name[] = "Main Deck";
static void main_deck_draw(void);
static void main_deck_run(uint32_t ev, void *arg);

static char _soi_target[20];
static int vdata_valid = 0;

static int show_pitch_roll_yaw = 0;

static char *soi_target(uint8_t SOI)
{
    switch (SOI) {
        case 100:
            strcpy(_soi_target, "Kerbol");
            break;
        case 110:
            strcpy(_soi_target, "Moho");
            break;
        case 120:
            strcpy(_soi_target, "Eve");
            break;
        case 121:
            strcpy(_soi_target, "Gilly");
            break;
        case 130:
            strcpy(_soi_target, "Kerbin");
            break;
        case 131:
            strcpy(_soi_target, "Mun");
            break;
        case 132:
            strcpy(_soi_target, "Minmus");
            break;
        case 140:
            strcpy(_soi_target, "Duna");
            break;
        case 141:
            strcpy(_soi_target, "Ike");
            break;
        case 150:
            strcpy(_soi_target, "Dres");
            break;
        case 160:
            strcpy(_soi_target, "Jool");
            break;
        case 161:
            strcpy(_soi_target, "Laythe");
            break;
        case 162:
            strcpy(_soi_target, "Vall");
            break;
        case 163:
            strcpy(_soi_target, "Tylo");
            break;
        case 164:
            strcpy(_soi_target, "Bop");
            break;
        case 165:
            strcpy(_soi_target, "Pol");
            break;
        case 170:
            strcpy(_soi_target, "Eloo");
            break;
        default:
            strcpy(_soi_target, "Unknown");
    }
    return _soi_target;
}



static void main_deck_init(void)
{

}

struct screen main_deck_screen = {
    .draw = main_deck_draw,
    .name = main_deck_name
};

struct task main_deck_task = {
    .init = main_deck_init,
    .run = main_deck_run,
    .events = EV_BUTTON | EV_HEARTBEAT | EV_VESSELDATA,
    .screen = &main_deck_screen,
    .name =  main_deck_name
};

static char * _float_to_str(float x, char *p) {
    char *s = p + 20; // go to end of buffer
    uint16_t decimals;  // variable to store the decimals
    int units;  // variable to store the units (part to left of decimal place)
    memset(p, ' ', 20);
    if (x < 0.0) { // take care of negative numbers
        decimals = (int)(x * -100) % 100; // make 1000 for 3 decimals etc.
        units = (int)(-1 * x);
    } else { // positive numbers
        decimals = (int)(x * 100) % 100;
        units = (int)x;
    }

    *--s = (decimals % 10) + '0';
    decimals /= 10; // repeat for as many decimal places as you need
    *--s = (decimals % 10) + '0';
    *--s = '.';

    while (units > 0) {
        *--s = (units % 10) + '0';
        units /= 10;
    }
    if (x < 0) *--s = '-'; // unary minus sign for negative numbers
    return s;
}
float approx_sqrt(float x) {
    float guess = x / 2.0;
    float prev_guess;
    float error = 1e-3;  // set desired level of accuracy here
    
    do {
        prev_guess = guess;
        guess = (guess + x / guess) / 2.0;
    } while (fabs(guess - prev_guess) > error);

    return guess;
}
#ifndef PI
#define PI 3.14159265
#endif

void draw_circle(int x, int y, int radius, uint8_t color)
{

    int i, j;
    uint8_t *screen = ui_get_screen(0);
    int x0 = x - 10;
    for (i = x - radius; i <= x + radius; i++) {
        for (j = y - radius; j <= y + radius; j++) {
            if ((i - x) * (i - x) + (j - y) * (j - y) <= radius * radius) {
                ui_draw_h_segment(0, color, i, j, 1);
            }
        }
    }
}

void ui_draw_circle_slice(uint32_t x, uint32_t y, uint32_t r, int alpha, int beta, uint8_t color) {
    int i, j;
    uint32_t start, end;
    int cx = x;
    int cy = y;
    alpha += 90;
    beta += 90;

    if (alpha >= 360)
        alpha-=360;
    if (beta >= 360)
        beta -= 360;

    if (alpha < 0)
        alpha += 360;
    if (beta < 0)
        beta += 360;

    ui_draw_h_segment(0, color, x-1, y, 3);

    if (alpha < beta) {
        for (j = 0; j < r; j++) {
            for (i = alpha; i < beta; i++) {
                int px = cx + (int)(j * pcos(i));
                int py = cy + (int)(j * psin(i));
                if (px != cx || py != cy) {
                    ui_draw_h_segment(0, color, px, py, 1);
                }
            }
        }
    } else {
        for (j = 0; j < r; j++) {
            for (i = alpha; i < 360; i++) {
                int px = cx + (int)(j * pcos(i));
                int py = cy + (int)(j * psin(i));
                if (px != cx || py != cy) {
                    ui_draw_h_segment(0, color, px, py, 1);
                }
            }
            for (i = 0; i < beta; i++) {
                int px = cx + (int)(j * pcos(i));
                int py = cy + (int)(j * psin(i));
                if (px != cx || py != cy) {
                    ui_draw_h_segment(0, color, px, py, 1);
                }
            }
        }
    }
}


#define ptan(a) psin(a)/pcos(a)

void ui_draw_circle_sector(int x, int y, int r, int px, int py, int rho, int include_center) {
    int rho_norm = (0 - rho) + 90;
    int i, j;
    float m,q;
    uint8_t upc, downc;

    while (rho_norm < 0)
        rho_norm+=360;
    while (rho_norm >= 360)
        rho_norm -= 360;

    if ((rho == 0) || (rho == 180)) {
        upc = BROWN;
        downc = CYAN;
        if (rho == 180){
            upc = CYAN;
            downc = BROWN;
        }
        for (i = x - r; i <= x + r; i++) {
            for (j = y - r; j <= y + r; j++) {
                if ((i - x) * (i - x) + (j - y) * (j - y) <= r * r) {
                    if (i > px)
                        ui_draw_h_segment(0, upc, i, j, 1);
                    else
                        ui_draw_h_segment(0, downc, i, j, 1);
                }
            }
        }
        return;
    }

    if (rho > 0) {
        upc = CYAN;
        downc = BROWN;
    }else {
        upc = BROWN;
        downc = CYAN;
    }
    
    m = ptan(rho_norm);
    q = py - (m * px);
    for (i = x - r; i <= x + r; i++) {
        for (j = y - r; j <= y + r; j++) {
            if ((i - x) * (i - x) + (j - y) * (j - y) <= r * r) {
                if (j > (m * i + q))
                    ui_draw_h_segment(0, upc, i, j, 1);
                else
                    ui_draw_h_segment(0, downc, i, j, 1);
            }
        }
    }
}


void draw_navball(int x, int y, int pitch, int roll) {
    static int last_p_navball = -400;
    static int last_r_navball = - 400;
    
    if ((last_p_navball == pitch) && (last_r_navball == roll))
        return;
    ui_fill_area(0, BLACK, 150, 120, 320, 200);
    last_p_navball = pitch;
    last_r_navball = roll;
    if (pitch > 45 && pitch < 135) {
        draw_circle(x,y, 40, CYAN);
    } else if (pitch < -45 && pitch > -135) {
        draw_circle(x,y, 40, BROWN);
    } else if (pitch == 0) {
        int a = 0 - roll;
        int b = a + 180;

        ui_draw_circle_slice(x,y, 40, a, b, CYAN);
        ui_draw_circle_slice(x,y, 40, b, a, BROWN);
    } else {
        uint32_t px, py;
        px = x + 40 * psin(pitch) * pcos(roll);
        py = y - 40 * psin(pitch) * psin(roll);
        ui_draw_circle_sector(x, y, 40, px, py, roll, 1);
    }
    ui_draw_h_segment(0, ORANGE, x, y - 6,  5);
    ui_draw_h_segment(0, ORANGE, x, y + 1,  5);
    ui_draw_h_segment(0, ORANGE, x + 1, y - 2,  3);
    ui_draw_h_segment(0, ORANGE, x + 1, y + 1,  3);
    ui_draw_h_segment(0, ORANGE, x + 2, y, 1);

}

static void fuel_display(unsigned x, unsigned y, unsigned fuel, unsigned fuel_tot, char *label, int col)
{
    unsigned tx = x;
    unsigned ty = y;
    unsigned i;
    unsigned pct;
    if (!vdata_valid)
        return;

    pct = fuel * 100 / fuel_tot;
    
    ui_fill_area(0, BLACK, tx - 10, ty + 100, tx + 8, ty + 110);
    ui_fill_area(0, BLACK, tx, ty, tx + 8, ty + 100);
    ui_text_at(0, col, tx - 10, ty + 100, label);
    for (i = 0; i < 100; i++) {
        if (pct < 10)
            col = RED;
        if (pct > i) {
            ui_fill_area(0, col, tx, ty + 100 - i, tx + 8, ty + 101 - i);
        }
    }
}


void avionics_display(controlPacket_t *cp)
{

    uint32_t throttle, potr;
    int32_t pitch, yaw, roll;
    int i;
    int tx = 322;
    int ty = 150;
    /* Throttle display */
    pot_read(NULL, &potr);
    throttle = ((potr * 100) / 4096);
    
    ui_fill_area(0, BLACK, 245, 250, 332, 270);
    ui_fill_area(0, BLACK, tx, ty, tx + 10, ty + 100);
    ui_text_at(0, YELLOW, 245, 250, "Throttle:");
    ui_text_at(0, YELLOW, 265, 260, ui_printn((potr * 100) / 4096));
    for (i = 0; i < 100; i++) {
        int col = YELLOW;
        if (i > 90)
            col = RED;
        if (throttle > i) {
            ui_fill_area(0, col, tx, ty + 100 - i, tx + 10, ty + 101 - i);
        }
    }
    cp->Throttle = (potr * 1000) / 4096;

    if (show_pitch_roll_yaw) { 
        unsigned px, py, rx,ry, yx, yy;
        int col = GREEN;
        int labelcol = GREEN;
        char label[15];
        px = 55;
        py = 70;
        rx = 2;
        ry = 75;
        yx = 2;
        yy = 110;
        ui_fill_area(0, 238, 2, 52, 103, 138);


        /* Pitch display */
        pitch = 0 - (cp->Pitch / 50);
        snprintf(label, 15, "P %d", pitch);
        ui_text_at(0, labelcol, px + 10, py + 20, label);
        ui_fill_area(0, 235, px, py, px + 10, py + 40);
        for (i = -20; i < 0; i++) {
            if (pitch < i) {
                ui_fill_area(0, col, px, py + 20 + i, px + 10, py + 21 + i);
            }
        }
        for (i = 0; i < 20; i++) {
            if (pitch > i) {
                ui_fill_area(0, col, px, py + 20 + i, px + 10, py + 21 + i);
            }
        }
        ui_fill_area(0, RED, px - 2, py + 20, px + 12, py + 21);

        /* Roll display */
        roll = (cp->Roll / 50);
        snprintf(label, 15, "R %d", roll);
        ui_text_at(0, labelcol, rx, ry - 10, label);
        ui_fill_area(0, 235, rx, ry, rx + 40, ry + 10);
        for (i = -20; i < 0; i++) {
            if (roll < i) {
                ui_fill_area(0, col, rx + 20 + i, ry, rx + 21 + i, ry + 10);
            }
        }
        for (i = 0; i < 20; i++) {
            if (roll > i) {
                ui_fill_area(0, col, rx + 20 + i, ry, rx + 21 + i, ry + 10);
            }
        }
        ui_fill_area(0, RED, rx + 20, ry - 2, rx + 21, ry + 12);

        /* Yaw display */
        yaw = (cp->Yaw / 50);
        snprintf(label, 15, "Y %d", yaw);
        ui_text_at(0, labelcol, yx, yy - 10, label);
        ui_fill_area(0, 235, yx, yy, yx + 40, yy + 10);
        for (i = -20; i < 0; i++) {
            if (yaw < i) {
                ui_fill_area(0, col, yx + 20 + i, yy, yx + 21 + i, yy + 10);
            }
        }
        for (i = 0; i < 20; i++) {
            if (yaw > i) {
                ui_fill_area(0, col, yx + 20 + i, yy, yx + 21 + i, yy + 10);
            }
        }
        ui_fill_area(0, RED, yx + 20, yy - 2, yx + 21, yy + 12);
    } else
        ui_fill_area(0, 235, 1, 51, 104, 138);

}

static void navi_display(void)
{

   char Status[100] = "";
   char Points[100] = "";
   int ralt = (int)cur_vdata->RAlt;
   int i = 0;
   char hdg_bar[30] = "";
   int hdg_dec, hdg_ref;
   unsigned x_alt, y_alt;
   int vvi;
   uint8_t pointcolor;

   
   if (((int)cur_vdata->VVI == 0) && ((int)cur_vdata->RAlt < 10)) {
       strcpy(Status, "Landed at ");
   } else if ((int)cur_vdata->e != 0) {
       strcpy(Status, "Escaping from ");
   } else if ((int) cur_vdata->PE < 0) {
       strcpy(Status, "In flight over ");
   } else  {
       strcpy(Status, "Orbiting ");
   }
   strcat(Status, soi_target(cur_vdata->SOINumber));
   if ((int)(cur_vdata->PE < 0)){
       snprintf(Points, 100, "Apo: %d, Peri: Negative", (int)(cur_vdata->AP));
       pointcolor = PURPLE;
   } else {
       snprintf(Points, 100, "Apo: %d, Peri: %d", 
               (int)(cur_vdata->AP),
               (int)(cur_vdata->PE));
       pointcolor = CYAN;
   }

   ui_fill_area(0, BLACK, 140, 20, 380, 60);
   ui_text_at(0, WHITE, 150, 20, Status);
   ui_text_at(0, pointcolor, 150, 30, Points);
    
   /* Navball */ 
   draw_navball(160, 233, (int)cur_vdata->Pitch, (int)cur_vdata->Roll);

   /* Altimeter */
   x_alt = 105;
   y_alt = 99;
   ui_fill_area(0, DARKGREY, x_alt, y_alt, x_alt + 45, y_alt + 101);
   if (ralt < 100000) {
       ui_fill_area(0, BLACK, x_alt+45, y_alt, x_alt + 55, y_alt + 101);
       for (i = 0; i < 100; i+=10)
           ui_fill_area(0, DARKGREY, x_alt + 46, (y_alt + 99 - i), x_alt + 54, (y_alt + 101) - i);
       i = ralt / 1000;
       ui_fill_area(0, GREEN, x_alt + 45, (y_alt + 101) - i, x_alt + 55, (y_alt + 101));
       ui_fill_area(0, BLACK, x_alt, (y_alt + 90) - i, x_alt + 50, (y_alt + 100) - i);
       ui_text_at(0, WHITE, x_alt, (y_alt + 90) - i, ui_printn(ralt));
       ui_text_at(0, WHITE, x_alt + 40, (y_alt + 90) - i, "m");
       ui_fill_area(0, BRIGHT(GREEN), x_alt + 20, (y_alt + 101) - i, x_alt + 55, (y_alt + 102) - i);
   }
   ui_fill_area(0, WHITE, x_alt + 55, y_alt + 60, x_alt + 70, y_alt + 61);
   vvi = (int)(cur_vdata->VVI);
   if (vvi > 100)
       vvi = 100;
   if (vvi < -100)
       vvi = -100;

   if (vvi > 0) {
       ui_fill_area(0, BRIGHT(GREEN), x_alt+56, y_alt + 60 - (vvi / 4), x_alt +60, y_alt + 61);
   } else {
       ui_fill_area(0, BRIGHT(RED), x_alt+56, y_alt + 60, x_alt +60, (y_alt + 60) - (vvi / 4));
   }

   
   /* Flight Data */
   char pitch_s[6], roll_s[6], hdg_s[4] = "000";
   ui_fill_area(0, BLACK, 155, 100, 320, 120);

   ui_fill_area(0, BLACK, 222, 140, 246, 148);
   snprintf(hdg_s, 4, "%03d", (int)(cur_vdata->Heading));
   ui_text_at(0, WHITE, 222, 140, hdg_s);

   ui_text_at(0, GREEN, 205, 110, "SPD");
   ui_text_at(0, WHITE, 235, 110, ui_printn((int)(cur_vdata->IAS)));


   /* Heading bar */
   ui_fill_area(0, BLACK, 150, 200, 320, 220);
   hdg_ref = ((int)cur_vdata->Heading) - 10;
   if (hdg_ref < 0)
       hdg_ref += 360;
   for (i = 0; i < 20; i++) {
       if (((i + hdg_ref) % 10) == 9) {
           int hdg_pr = (hdg_ref + 1 + i) / 10 * 10;
           if (hdg_pr >= 360)
               hdg_pr -= 360;
           snprintf(hdg_bar + i, 4,"%03d", hdg_pr);
           i += 2;
           continue;
       }
       else if (((i + hdg_ref) % 5) == 0) {
          hdg_bar[i] = '|';
       } else {
          hdg_bar[i] = '.';
       }
   }
   hdg_bar[21] = '\0';
   ui_text_at(0, WHITE, 150, 200, "          |");
   ui_text_at(0, WHITE, 150, 210, hdg_bar);

}

static int button = -1;
#define BUTTON_THRESHOLD 2

static void main_deck_run(uint32_t event, void *arg)
{
    int32_t x, y;
    uint32_t torque;
    struct sample *s;
    int ts;

    int throttle = 0;
    int pitch = 0, roll = 0, yaw = 0;

    int main_ctrl = 0;

    static int sas   = 0;
    static int rcs   = 0;
    static int brk   = 0;
    static int gear  = 0;
    static int light = 0;
    static uint8_t navmode = 0;

    int i;
    char btn_str[6];
    int staging = 0;
    controlPacket_t cp = {};

    if (screen_get_focus() != &main_deck_screen)
        return;
    if (ui_menu_is_on()) {
        if (event & EV_BUTTON)
            ui_redraw(EV_BUTTON);
        return;
    }
    ts = input_detect_touch();
    if (ts == TS_TOUCH_NONE) {
    } else {
        event |= EV_BUTTON;
        button = 12;
    }
    if (event & EV_VESSELDATA) {
        static int ev_data_ctr = 0;
        if ((++ev_data_ctr == 10) || (vdata_valid == 0)) {
            brk = (cur_vdata->ActionGroups & (1 << 4)) >> 4;
            gear = (cur_vdata->ActionGroups & (1 << 3)) >> 3;
            light = (cur_vdata->ActionGroups & (1 << 2)) >> 2;
            rcs = (cur_vdata->ActionGroups & (1 << 6)) >> 6;
            sas = (cur_vdata->ActionGroups & (1 << 7)) >> 7;
            navmode = cur_vdata->NavballSASMode;
            ev_data_ctr = 0;
        }
        vdata_valid = 1;
        navi_display();
        clear_event(EV_VESSELDATA);
    }
    if (event & EV_BUTTON) {
        if (button < 0)
            button = ui_process_button_pressed();
        switch(button) {
            case 12: /* TS */
                show_pitch_roll_yaw = !!!show_pitch_roll_yaw;
                break;

            case BUTTON_DPADU:
                cp.Pitch = -1000;
                break;
            case BUTTON_DPADD:
                cp.Pitch = 1000;
                break;
            case BUTTON_DPADL:
                cp.Yaw = -1000;
                break;
            case BUTTON_DPADR:
                cp.Yaw = 1000;
                break;
            case 7:
                ui_menu(1);
                ui_redraw(EV_NONE);
                break;
            case 10:
                if (sas)
                    navmode++;
                if ((navmode & 0xF) > 10) {
                    navmode &= 0xF0;
                    navmode |= 0x01;
                }
                break;
            case 8:
                if (vdata_valid) 
                    gear = !(((cur_vdata->ActionGroups & (1 << 3)) >> 3));
                break;
            case 9:
                if (vdata_valid) 
                    light = !light;
                break;
            case 5:
                if (vdata_valid) 
                    brk = !(((cur_vdata->ActionGroups & (1 << 4)) >> 4));
                break;
            case 4:
                staging++;
                break;
        }
        button = -1;
        clear_event(EV_BUTTON);
    }
    
    
    /* Sample joystick */
    joy_read(&x, &y);
    pot_read(&torque, NULL);

    x-=2048 + ROLL_CALIB;
    y-=2048 + PITCH_CALIB;
    x*=2000 * torque / 4096;
    y*=2000 * torque / 4096;
    x/=4096;
    y/=4096;
    if ((x > 20) || (x < -20))
        cp.Roll = x;
    else
        cp.Roll = 0;

    if ((y > 20) || (y < -20))
        cp.Pitch = y;
    else
        cp.Pitch = 0;

    avionics_display(&cp);

    if (vdata_valid) {
        fuel_display(370, 100, (int)(cur_vdata->LiquidFuel), (int)(cur_vdata->LiquidFuelTot), "LF", YELLOW );
        fuel_display(388, 100, (int)(cur_vdata->Oxidizer), (int)(cur_vdata->OxidizerTot), "Ox", CYAN);
        fuel_display(406, 100, (int)(cur_vdata->SolidFuel), (int)(cur_vdata->SolidFuelTot), "SF", GREY);
        fuel_display(424, 100, (int)(cur_vdata->ECharge), (int)(cur_vdata->EChargeTot), "el" , BRIGHT(GREEN));
        fuel_display(442, 100, (int)(cur_vdata->XenonGas), (int)(cur_vdata->XenonGasTot), "Xe" , PINK);

        if ((cur_vdata->ActionGroups & (1 << 1)) == (1 << 1))
            fuel_display(32, 150, (int)(cur_vdata->MonoProp), (int)(cur_vdata->MonoPropTot), "MP", BRIGHT(YELLOW));
        else
            ui_fill_area(0, DARKGREY, 20, 150, 50, 280);

        if ((cur_vdata->Density > 0)) {
            int dens = (int)(cur_vdata->Density * 100);
            if (dens > 100)
                dens = 100;
            fuel_display (50, 150, dens, 100, "At", 87);
            fuel_display (68, 150, (int)(cur_vdata->IntakeAir),(int)(cur_vdata->IntakeAirTot), "Ai", CYAN);
        }

    }

    
    /* Lamps */
    ui_fill_area(0, 242, 35, 0, 64, 27);
    ui_fill_area(0, light?58:233, 38, 0, 64, 24);
    ui_text_at (0, (light?BLACK:238), 38, 8, "LIG");
    
    ui_fill_area(0, 242, 65, 0, 94, 27);
    ui_fill_area(0, gear?28:233, 68, 0, 94, 24);
    ui_text_at (0, (gear?BLACK:238), 68, 8, "GEA");
    
    ui_fill_area(0, 242, 95, 0, 124, 27);
    ui_fill_area(0, brk?BLOOD:233, 98, 0, 124, 24);
    ui_text_at (0, (brk?WHITE:238), 98, 8, "(!)");

    /* Switches */
    sas = input_get_swr()?1:0;
    rcs = input_get_swl()?1:0;
    if (sas == 0) {
        navmode &= 0xF0;
    }
    ui_fill_area(0, 242, 447, 0, 474, 28);
    ui_fill_area(0, sas?122:233, 450, 0, 474, 24);
    ui_text_at (0, (sas?BLUE:238), 450, 8, "SAS");

    ui_fill_area(0, 242, 3, 0, 30, 28);
    ui_fill_area(0, rcs?GREEN:235, 6, 0, 30, 24);

    ui_text_at (0, rcs?BRIGHT(GREEN):238, 6, 8, "RCS");

    cp.id = 101;
    cp.NavBallSASMode = navmode;

    if (!rcs)
        staging = 0;

    if (vdata_valid) {
        cp.MainControls = ((sas << 7) | (rcs << 6) | (light << 5) | (gear << 4) | (brk << 3) | (staging << 0));
        ksp_serial_send(&cp, sizeof(cp));
    }

    if (sas)
        uled_on(4);
    else
        uled_off(4);
    if (rcs)
        uled_on(5);
    else
        uled_off(5);
    if(light)
        uled_on(0);
    else
        uled_off(0);
    if (gear)
        uled_on(1);
    else
        uled_off(1);
    if (brk)
        uled_on(2);
    else
        uled_off(2);

}

static void main_deck_draw(void)
{
    int i;
    char btn_str[4] = "";
    /* background to foreground */
    ui_fill_area(0, DARKGREY, 0, 0, xres, yres);
    ui_fill_area(0, WHITE, 0, 50, 105, 140); /* Pitch panel */
    /* Logo & appname */
    //image_at(0, logo, 5, 4, logox, logoy);
}

void main_deck_setup(void)
{
    register_task(&main_deck_task);
    register_screen(&main_deck_task, &main_deck_screen);
}
