#include "PSP-Nx-lib.nxc"

const byte SensorPort  =  IN_2;
#define ADDR     0x02
int fwd;
int steer;
int left;
int right;

/*--------------------------------------
  Controller button layout:
----------------------------------------

      L1                R1
      L2                R2

      d                 triang
   a     c         square     circle
      b                  cross

     l_j_b              r_j_b
     l_j_x              r_j_x
     l_j_y              r_j_y

-------------------------------------- */
/*
  bits as follows:
   b1:   a b c d x r_j_b l_j_b x
   b2:   square cross circle triang R1 L1 R2 L2
*/

task main()
{
  string msg, msg0, msg1, msg2, msg3;
  psp currState;
  string x;
	int i;
  
  while (true ) {

    PSP_ReadButtonState(SensorPort, ADDR, currState);

    msg = "b1:               ";
    x = PSP_format_bin(currState.b1);
    msg = StrReplace(msg, 3, x);
    TextOut(0, LCD_LINE1, msg, false);

    msg = "b2:               ";
    x = PSP_format_bin(currState.b2);
    msg = StrReplace(msg, 3, x);
    TextOut(0, LCD_LINE2, msg, false);

    msg = "   . .       . .  ";
    x = NumToStr(currState.l1);
    msg = StrReplace(msg, 3, x);
    x = NumToStr(currState.l2);
    msg = StrReplace(msg, 5, x);
    x = NumToStr(currState.r1);
    msg = StrReplace(msg, 13, x);
    x = NumToStr(currState.r2);
    msg = StrReplace(msg, 15, x);
    TextOut(0, LCD_LINE3, msg, false);

    msg = "    .         .   ";
    x = NumToStr(currState.d);
    msg = StrReplace(msg, 4, x);
    x = NumToStr(currState.triang);
    msg = StrReplace(msg, 14, x);
    TextOut(0, LCD_LINE4, msg, false);

    msg = "   . .       . .  ";
    x = NumToStr(currState.a);
    msg = StrReplace(msg, 3, x);
    x = NumToStr(currState.c);
    msg = StrReplace(msg, 5, x);
    x = NumToStr(currState.square);
    msg = StrReplace(msg, 13, x);
    x = NumToStr(currState.circle);
    msg = StrReplace(msg, 15, x);
    TextOut(0, LCD_LINE5, msg, false);

    msg = "    .         .   ";
    x = NumToStr(currState.b);
    msg = StrReplace(msg, 4, x);
    x = NumToStr(currState.cross);
    msg = StrReplace(msg, 14, x);
    TextOut(0, LCD_LINE6, msg, false);


    msg = "l:               ";
    x = NumToStr(currState.l_j_x);
    steer =  currState.l_j_x;
    msg = StrReplace(msg, 2, x);
    x = NumToStr(currState.l_j_y);
    fwd =  - (currState.l_j_y);
    msg = StrReplace(msg, 7, x);
    TextOut(0, LCD_LINE7, msg, false);

    msg = "r:               ";
    x = NumToStr(currState.r_j_x);
    msg = StrReplace(msg, 2, x);
    x = NumToStr(currState.r_j_y);
    msg = StrReplace(msg, 7, x);
    TextOut(0, LCD_LINE8, msg, false);
    
    if (fwd > 0)
       {
        if (steer > 0)
          {
           left = ((fwd * fwd) + (steer * steer)) / (fwd + steer);
           right = fwd - steer;
          }
        else
          {
           left = fwd + steer;
           right = ((fwd * fwd) + (steer * steer)) / (fwd - steer);
          }
       }
    else
       {
       if (steer > 0)
          {
           left = steer + fwd;
           right = ((fwd * fwd) +(steer * steer)) / (fwd - steer);
          }
        else
          {
           left = ((fwd * fwd) +(steer * steer)) / (fwd + steer);
           right = fwd - steer;
          }
       }
      OnFwd(OUT_A,left);
      OnFwd(OUT_B,right);
//    Wait(10);
  }

}