#include "mbed.h"
#include <iostream>
#include <stdlib.h>
#include <time.h>
#include <stdio.h>

using namespace std;

InterruptIn P1Button(p5);
InterruptIn P2Button(p6);
InterruptIn slutt(p7);

DigitalOut myled(LED1);
DigitalOut bargraph[5] = {p26, p27, p28, p29, p30};
DigitalOut bsone[4] = {p21, p22, p23, p24};
DigitalOut led1(LED1), led2(LED2);
BusOut buss(p26, p27, p28, p29, p30);

const int P1 = 0, P2 = 1;

int P1Score = 0;
int P2Score = 0;
int vanskelighetsgrad;
int serveside = P2;

float ventetid;
bool P1Timer = false, P2Timer = false, P1Truffet = false, P2Truffet = false;
bool spillTapt = false;

Serial pc(USBTX, USBRX);

void loopFW ()
{
    for(int a = 0; a < 5; a++) {
        bargraph[a] = 1;
        wait_ms(ventetid);
        bargraph[a] = 0;
    }
}

void loopBW ()
{

    for(int a = 4; a >= 0; a--) {
        bargraph[a] = 1;
        wait_ms(ventetid);
        bargraph[a] = 0;
    }
}

void P1Avspark()
{
    bsone[0] = 0;
    bsone[1] = 1;
    bsone[2] = 1;
    bsone[3] = 1;
    loopBW ();
    bsone[0] = 1;
    bsone[1] = 0;
    loopBW ();
    bsone[1] = 1;
    bsone[2] = 0;
    loopBW ();
    bsone[2] = 1;
    bsone[3] = 0;
    loopBW ();
}

void P2Avspark()
{
    bsone[0] = 1;
    bsone[1] = 1;
    bsone[2] = 1;
    bsone[3] = 0;
    loopFW ();
    bsone[2] = 0;
    bsone[3] = 1;
    loopFW ();
    bsone[1] = 0;
    bsone[2] = 1;
    loopFW ();
    bsone[0] = 0;
    bsone[1] = 1;
    loopFW ();
}

void tap (int spiller)
{
    if (spiller == P1) {
        P2Score++;
        cout << "P2 scoret, stilling: " << P1Score << ":" << P2Score << endl;
        cout << "Spill tapt: " << spillTapt << endl;
        cout << "//" << endl;
    } else {
        P1Score++;
        cout << "P1 scoret, stilling: " << P1Score << ":" << P2Score << endl;
        cout << "Spill tapt: " << spillTapt << endl;
        cout << "//" << endl;
    }
}

void P1Skudd()
{
    if (P1Timer) {
        P1Timer = false;
        P1Truffet = true;
    } else {
        tap(P1);
        spillTapt = true;
    }
}

void P2Skudd()
{
    if (P2Timer) {
        P2Timer = false;
        P2Truffet = true;
    } else {
        tap(P2);
        spillTapt = true;
    }
}

void avslutt ()
{
//add senere
    exit (0);
}

int main()
{
    P1Button.rise(&P1Skudd);
    P2Button.rise(&P2Skudd);
    slutt.rise(&avslutt);

    cout << "1: 200ms " << endl;
    cout << "2: 250ms" << endl;
    cout << "3: 500ms" << endl;
start:
    cout << "Tast inn vanskelighetsgrad: ";
    cin >> vanskelighetsgrad;
    cout << vanskelighetsgrad << endl;

    if(vanskelighetsgrad == 1) {
        ventetid = 200;
    } else if (vanskelighetsgrad == 2) {
        ventetid = 250;
    } else if (vanskelighetsgrad == 3) {
        ventetid = 500;
    } else {
        cout << "Vennligst velg et tall mellom 1-3." << endl;
        goto start;
    }

    while(1) {
P1Serve:
        if(spillTapt) {
            break;
        } else {
            P1Avspark ();

            P2Timer = true;
            wait_ms(ventetid);
            P2Timer = false;
            if (!P2Truffet) {
                tap(P2);
                break;
            }
        }

P2Serve:
        if(spillTapt) {
            break;
        } else {
            P2Avspark ();

            P1Timer = true;
            wait_ms(ventetid);
            P1Timer = false;
            if (!P1Truffet) {
                tap(P1);
                break;
            }
        }
    }
    cout << "Ute av loop" << endl;
    spillTapt = false;
    cout << "Spill tapt: " << spillTapt << endl;

    if(serveside == P1) {
        serveside = P2;
        goto P1Serve;
    } else {
        serveside = P1;
        goto P2Serve;
    }
}