//#DOC Main, program entry and main loop
#include "main.h"
#include <otPort.h>
#include <otSerial.h>
#include <otTimer.h>
#include <otPrintf.h>
#include <otPS2Controller.h>

otPS2Controller ps2;

//#define FAST_PORT   // Un comment this define for FAST PORT test

#ifdef FAST_PORT // F A S T  P O R T Version ***********************************

void Clock(bool ena)
{
    PortSet(PP_F, 12, ena);
}

void Attention(bool ena)
{
    PortSet(PP_F, 13, ena);
}

void Command(bool ena)
{
    PortSet(PP_F, 14, ena);
}

bool Data()
{
    return PortGet(PP_F, 15);
}

void init()
{
    PortConfig(PP_F, 12, P_OUT);    // Clock
    PortConfig(PP_F, 13, P_OUT);    // Attention
    PortConfig(PP_F, 14, P_OUT);    // Command
    PortConfig(PP_F, 15, P_IN);     // Data
}

#else // ***********************************************************************
      // S P I Version *********************************************************

#include <otMCP23017.h>

otMCP23017  port;

void Clock(bool ena)
{
    port.pinWrite(0, ena);
}

void Attention(bool ena)
{
    port.pinWrite(1, ena);
}

void Command(bool ena)
{
    port.pinWrite(2, ena);
}

bool Data()
{
    return port.pinRead(3);
}

void init()
{
    port.init(0, true);
    port.pinMode(0, AS_OUTPUT); // Clock
    port.pinMode(1, AS_OUTPUT); // Attention
    port.pinMode(2, AS_OUTPUT); // Command
    port.pinMode(3, AS_INPUT);  // Data
}

#endif


void setup()
{
    // Setup the UART0 used by printf
    UartEnableSignals(UART0);
    UartSetBaud(UART0, 115200);
    
    StartTimer(TIMER0);
    
    DelayMs(1000);      // Wait terminal opening
    
    // Init printf
    setStdout(4096, UART0);         // printf output is now directed on UART0
    
    init();
    
    U8  rc = ps2.init(Attention, Command, Clock, Data, TIMER0);
    printf("PS2 ret code = %d\n", rc);
    if(rc == 0)
    {
        printf("Found Controller, configured successful\n");
        U8  type = ps2.readType();
        switch(type)
        {
        case 0:
              printf("Unknown Controller type found\n");
              break;
        case 1:
              printf("DualShock Controller found\n");
              break;
        case 2:
              printf("GuitarHero Controller found\n");
              break;
        case 3:
              printf("Wireless Sony DualShock Controller found\n");
              break;
        }
    }
    else
    {
        switch(rc)
        {
            case 1: printf("No controller found.\n");
                    break;
            case 2: printf("Controller found but not accepting commands.\n");
                    break;
            case 3: printf("Controller refusing to enter Pressures mode, may not support it.\n");
                    break;
        }
    }
}

void loop()
{
    U8  vibrate = 0;
    if(ps2.readType() == 1)    // DualShock Controller
    {
        ps2.readGamepad(false, vibrate);            // Read controller and set large motor to spin at 'vibrate' speed
        if(ps2.button(otPS2Controller::PSB_START))  // Will be TRUE as long as button is pressed
            printf("Start is being held\n");
        if(ps2.button(otPS2Controller::PSB_SELECT))
            printf("Select is being held\n");      

        if(ps2.button(otPS2Controller::PSB_PAD_UP)) // Will be TRUE as long as button is pressed
            printf("Up held this hard: %d\n", ps2.analog(otPS2Controller::PSAB_PAD_UP));
        if(ps2.button(otPS2Controller::PSB_PAD_RIGHT))
            printf("Right held this hard: %d\n", ps2.analog(otPS2Controller::PSAB_PAD_RIGHT));
        if(ps2.button(otPS2Controller::PSB_PAD_LEFT))
            printf("LEFT held this hard: %d\n", ps2.analog(otPS2Controller::PSAB_PAD_LEFT));
        if(ps2.button(otPS2Controller::PSB_PAD_DOWN))
            printf("DOWN held this hard: %d\n", ps2.analog(otPS2Controller::PSAB_PAD_DOWN));

        vibrate = ps2.analog(otPS2Controller::PSAB_CROSS);  // This will set the large motor vibrate speed based on how hard you press the blue (X) button
        if(ps2.buttonState())                       // Will be TRUE if any button changes state (on to off, or off to on)
        {        
            if(ps2.button(otPS2Controller::PSB_L3))
                printf("L3 pressed\n");
            if(ps2.button(otPS2Controller::PSB_R3))
                printf("R3 pressed\n");
            if(ps2.button(otPS2Controller::PSB_L2))
                printf("L2 pressed\n");
            if(ps2.button(otPS2Controller::PSB_R2))
                printf("R2 pressed\n");
            if(ps2.button(otPS2Controller::PSB_TRIANGLE))
                printf("Triangle pressed\n");        
        }

        if(ps2.buttonPressed(otPS2Controller::PSB_CIRCLE))  // Will be TRUE if button was JUST pressed
            printf("Circle just pressed\n");
        if(ps2.buttonState(otPS2Controller::PSB_CROSS))     // Will be TRUE if button was JUST pressed OR released
            printf("X just changed\n");
        if(ps2.buttonReleased(otPS2Controller::PSB_SQUARE)) // Will be TRUE if button was JUST released
            printf("Square just released\n");     

        if(ps2.button(otPS2Controller::PSB_L1) || ps2.button(otPS2Controller::PSB_R1))  // Print stick values if either is TRUE
            printf("Stick Values: %d, %d, %d, %d\n", ps2.analog(otPS2Controller::PSS_LY), ps2.analog(otPS2Controller::PSS_LX), ps2.analog(otPS2Controller::PSS_RY), ps2.analog(otPS2Controller::PSS_RX));
        DelayMs(50);  
    }
}