#include "NXTDefs.h"

#define steer_counter 15

dseg segment
     temp1 sword 0
     temp2 sword 0
     temp3 sword 0
     
     init_ticks sword 0
     end_ticks sword 0
     my_ticks sword 0
     
     init_claw_rotations sword 0
     end_claw_rotations sword 0
     dif_claw_rotations sword 0
     
     distance sword 0
     light sword 0
     Switch sword 0
     
     move_forward_count sword 0
     countto0 sword 20

     notfound_ball_rotationsB sword 0
     notfound_ball_rotationsC sword 0
     found_ball_rotationsB sword 0
     found_ball_rotationsC sword 0
     got_ball_rotationsB sword 0
     got_ball_rotationsC sword 0
dseg ends


dseg segment
     //light sword 0
     steer sword 0
     steer_speed sword 0
     unsteer_speed sword 0
     follow_clockwise sword 0

     total_init_rotationsB sword 0
     total_init_rotationsC sword 0
     total_end_rotationsB sword 0
     total_end_rotationsC sword 0
     total_steer sword 0

     init_rotationsB sword 0
     end_rotationsB sword 0
     init_rotationsC sword 0
     end_rotationsC sword 0

    // move_forward_count sword 0
dseg ends


thread main
set follow_clockwise, 1
       //call init_claw
       wait 10000
       gettick init_ticks
       call search_ball
       call get_ball
       call find_line
       call follow_line
       call search_pad
       call goto_pad_leave_ball
       call get_out
       
       gettick end_ticks
       sub my_ticks, end_ticks, init_ticks
       NumOut(1,1, my_ticks)
       PlayTone(TONE_E5,2000)
       wait 1000000

       
       wait 10000
       wait 10000
       wait 10000
       exit
endt

//------------------------------------------------------------------------------

subroutine init_claw
stabilise:
       OnFwd(OUT_A, 70)
       wait 1000
       Off(OUT_A)
       
       return
ends


//------------------------------------------------------------------------------

subroutine search_ball
again:
       SetSensorUltrasonic(IN_4)
       set follow_clockwise, 1       //defines if it steers to left or right side
       set countto0, steer_counter   //defines the amount of time to steer to each side
       
       //check if ball is on sight
       ReadSensorUS(IN_4, distance)
       brcmp GT, go_on, distance, 50
       brcmp EQ, go_on, distance, 0
       jmp end
       
//start searching
go_on:

       OnRev(OUT_B, 100)
       wait 100
       sub countto0, countto0, 5
loop1:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check1, distance, 0
       brcmp LT, end, distance, 50
check1:
       brcmp GT, loop1, countto0, 0
       Off(OUT_BC)


       set countto0, steer_counter
       OnFwd(OUT_B, 100)
loop2:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check2, distance, 0
       brcmp LT, end, distance,50
check2:
       brcmp GT, loop2, countto0, 0
       Off(OUT_BC)


       set follow_clockwise, 0
       
//the other way now
       set countto0, steer_counter

       OnRev(OUT_C, 100)
       wait 100
       sub countto0, countto0, 5
loop3:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check3, distance, 0
       brcmp LT, end, distance, 50
check3:
       brcmp GT, loop3, countto0, 0
       Off(OUT_BC)
       

       set countto0, steer_counter
       OnFwd(OUT_C, 100)
loop4:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check4, distance, 0
       brcmp LT, end, distance,50
check4:
       brcmp GT, loop4, countto0, 0
       Off(OUT_BC)

       //if not in sight move forward and search again
       call move_forward
       jmp again
end:
       Off(OUT_BC)
       return
ends

//------------------------------------------------------------------------------

subroutine move_forward
       wait 100
       add move_forward_count, move_forward_count, 1    //count how many times moved forward
       brcmp EQ rotate180, move_forward_count, 4        //if too many reverse and search backwards
       OnFwdReg(OUT_BC, 100, OUT_REGMODE_SYNC)
       wait 900
       Off(OUT_BC)
       return
rotate180:
       RotateMotor(OUT_B, 100, 360)
       RotateMotor(OUT_C, 100, -360)
       return
ends


//------------------------------------------------------------------------------

subroutine get_ball

       Off(OUT_BC)
       
       getout found_ball_rotationsB, OUT_B, RotationCount
       getout found_ball_rotationsC, OUT_C, RotationCount

       //anti-align
       brcmp EQ moveC, follow_clockwise, 0
       brcmp EQ moveB, follow_clockwise, 1

moveB:
       RotateMotor(OUT_B, 100, 20)
       jmp end_align

moveC:
       RotateMotor(OUT_C, 100, 20)
       jmp end_align

end_align:

       //move till touch sensor pressed but not for a long time
       OnFwdReg(OUT_BC, 100, OUT_REGMODE_SYNC)
       SetSensorTouch(IN_1)
       sub temp1, 5, move_forward_count
       mul temp2, temp1, 500
CheckSensor:
       getout temp3, OUT_B, RotationCount
       brcmp GT cannot_find, temp3, temp2
       ReadSensor(IN_1, Switch)
       brcmp EQ, CheckSensor, Switch, 0
       Off(OUT_BC)

       //grab the ball
       OnRev(OUT_A, 70)
       wait 1000
       Off(OUT_A)

       //calculate how much moved to determine with which method to aproach line
       getout got_ball_rotationsB, OUT_B, RotationCount
       getout got_ball_rotationsC, OUT_C, RotationCount

       sub got_ball_rotationsB, got_ball_rotationsB, found_ball_rotationsB
       sub got_ball_rotationsC, got_ball_rotationsC, found_ball_rotationsC

       brcmp GT Cis_bigger, got_ball_rotationsC, got_ball_rotationsB
       
Bis_bigger:
       brcmp GT, increase_move_forward_count, got_ball_rotationsB, 720
       jmp end
           
Cis_bigger:
       brcmp GT, increase_move_forward_count, got_ball_rotationsC, 720
       jmp end


increase_move_forward_count:
       add move_forward_count, move_forward_count, 1
end:

       return
       
//problem didn't find
cannot_find:
       call search_ball
       return
ends


//====================================Line Stuff==============================//


subroutine find_line
    //go a little backwards from pad

    
    //choose the approach method
    brcmp EQ, second_pos, move_forward_count, 1
    brcmp EQ, first_pos, move_forward_count, 0

//if it is far from initial position
second_pos:
    OnRevReg(OUT_BC, 100, OUT_REGMODE_SYNC)
    wait 300
    Off(OUT_BC)
    //determine the way to approach the line
    brcmp EQ, ballis_right, follow_clockwise, 1

ballis_left:
    RotateMotor(OUT_B, 100, 180)
    RotateMotor(OUT_C, 100, -180)
    jmp goto_line

ballis_right:
    RotateMotor(OUT_B, 100, -180)
    RotateMotor(OUT_C, 100, 180)
    jmp goto_line

//align to line now
goto_line:
    SetSensorLight(IN_3)
    OnFwd(OUT_BC, 100)
CheckSensor:
    ReadSensor(IN_3, light)
    brcmp GT, CheckSensor, light, 45          //branch if colour is white
    Off(OUT_BC)
    OnFwd(OUT_BC, 80)
    wait 100
    Off(OUT_BC)


    brcmp EQ, align_rightside, follow_clockwise, 1


align_leftside:
    OnFwd(OUT_B, 80)
align_left:
    ReadSensor(IN_3, light)
    brcmp GT, align_left, light, 50          //branch if colour is white
    Off(OUT_BC)
    RotateMotor(OUT_C, 100, 90)
    jmp end


align_rightside:
    OnFwd(OUT_C, 80)
align_right:
    ReadSensor(IN_3, light)
    brcmp GT, align_right, light, 50          //branch if colour is white
    Off(OUT_BC)
    RotateMotor(OUT_B, 100, 90)
    jmp end


//==========

//approach line if ball found close to init position
first_pos:

    brcmp EQ, align_rightside2, follow_clockwise, 1
     

align_leftside2:
    set follow_clockwise, 1
    SetSensorLight(IN_3)
    OnRev(OUT_C, 100)
    wait 200
align_left2:
    ReadSensor(IN_3, light)
    brcmp GT, align_left2, light, 50          //branch if colour is white

align_left2n:
   /* ReadSensor(IN_3, light)
    brcmp LT, align_left2n, light, 60          //branch if colour is black
    
    Off(OUT_BC)
    RotateMotor(OUT_C, 100, 160)
    wait 100*/
    Off(OUT_BC)
    jmp end
    
    

align_rightside2:
    set follow_clockwise, 0
    SetSensorLight(IN_3)
    OnRev(OUT_B, 100)
    wait 200
align_right2:
    ReadSensor(IN_3, light)
    brcmp GT, align_right2, light, 60          //branch if colour is white

align_right2n:
   /* ReadSensor(IN_3, light)
    brcmp LT, align_right2n, light, 60          //branch if colour is black

    Off(OUT_BC)
    RotateMotor(OUT_B, 100, 90)
    wait 100
    */Off(OUT_BC)
    jmp end


end:
   return
ends


//------------------------------------------------------------------------------
subroutine follow_line
       SetSensorLight(IN_3)

       getout total_init_rotationsB, OUT_B, RotationCount
       getout total_init_rotationsC, OUT_C, RotationCount
       
//here start to follow the line
CheckSensor:
       brcmp EQ, C_isBigger, follow_clockwise, 0
       brcmp EQ, B_isBigger, follow_clockwise, 1
       
B_isBigger:
       OnFwd(OUT_B, 100)
       OnFwd(OUT_C, 90)
       jmp found_initSpeed

C_isBigger:
       OnFwd(OUT_B, 90)
       OnFwd(OUT_C, 100)
       jmp found_initSpeed
       
found_initSpeed:

       ReadSensor(IN_3, light)
       brcmp LT, CheckSensor, light, 62          //branch if colour is black

       set steer, 0
       set steer_speed, 100         //60 25    //  80   25 in
       set unsteer_speed, 45      // 90 33            //100 37/34/48 in

       getout init_rotationsB, OUT_B, RotationCount
       getout init_rotationsC, OUT_C, RotationCount

//determine if steer is needed
steer:

       getout total_end_rotationsB, OUT_B, RotationCount
       getout total_end_rotationsC, OUT_C, RotationCount

       brcmp EQ, count_clockwise, follow_clockwise, 1

count_counterclockwise:
       sub total_steer, total_end_rotationsB, total_init_rotationsB
       sub total_steer, total_steer, total_end_rotationsC
       add total_steer, total_steer, total_init_rotationsC
       jmp check_2rounds


count_clockwise:
       sub total_steer, total_end_rotationsC, total_init_rotationsC
       sub total_steer, total_steer, total_end_rotationsB
       add total_steer, total_steer, total_init_rotationsB
       jmp check_2rounds

//check if two full circles have completed
check_2rounds:
       //NumOut(0,0,total_steer)
       brcmp GT, end, total_steer, 3300

/* Code made to uncover from serious mistakes but it is unuseful... :-)
so  removed to get results faster!!!
       getout end_rotationsB, OUT_B, RotationCount
       getout end_rotationsC, OUT_C, RotationCount

       brcmp EQ, find_clockwise, follow_clockwise, 1


find_counterclockwise:
       sub steer, end_rotationsB, init_rotationsB
       jmp check_steer

find_clockwise:
       sub steer, end_rotationsC, init_rotationsC
       jmp check_steer


//determine the way to steer
check_steer:
       brcmp EQ, sendsteer_otherside, follow_clockwise, 1


sendsteer_sameside:
       brcmp GT, steer_otherside, steer, 2200
       brcmp GT, steer_sameside, steer, 1500
       brcmp GT, steer_otherside, steer, 1000
       brcmp GT, steer_sameside, steer, 800
       brcmp GT, steer_otherside, steer, 500
       jmp steer_sameside


sendsteer_otherside:
       brcmp GT, steer_sameside, steer, 2200
       brcmp GT, steer_otherside, steer, 1500
       brcmp GT, steer_sameside, steer, 1000
       brcmp GT, steer_otherside, steer, 800
       brcmp GT, steer_sameside, steer, 500
       jmp steer_otherside
 */

//now send steering commands
        brcmp EQ, steer_otherside, follow_clockwise, 1

//counter clockwise
steer_sameside:
       ReadSensor(IN_3, light)
       OnFwd(OUT_B, steer_speed)
       OnFwd(OUT_C, unsteer_speed)
       brcmp GT, steer, light, 62          //branch if colour is white
       jmp CheckSensor


//clockwise
steer_otherside:
       ReadSensor(IN_3, light)
       OnFwd(OUT_B, unsteer_speed)
       OnFwd(OUT_C, steer_speed)
       brcmp GT, steer, light, 62          //branch if colour is white
       jmp CheckSensor

end:
    Off(OUT_BC)
    return
 ends
 
//==================================Pad Stuff===================================

subroutine search_pad

      set move_forward_count, 0
       //return to view into the oval circle
       brcmp EQ rotate_right, follow_clockwise, 1
       
rotate_left:
       //RotateMotor(OUT_C, 100, -360)
       //RotateMotor(OUT_BC, 100, -180)
       OnRev(OUT_C, 100)
       OnRev(OUT_B, 30)
       wait 900
       Off(OUT_BC)

       OnRev(OUT_BC, 100)
       wait 100
       Off(OUT_BC)

       jmp now_find
       
       
rotate_right:
       //RotateMotor(OUT_B, 100, -360)
       //RotateMotor(OUT_BC, 100, -180)
       OnRev(OUT_B, 100)
       OnRev(OUT_C, 30)
       wait 900
       Off(OUT_BC)
       jmp now_find

       getout notfound_ball_rotationsB, OUT_B, RotationCount
       getout notfound_ball_rotationsC, OUT_C, RotationCount

       
//search for the pad now (almost same as search for ball)
now_find:
again:
       SetSensorUltrasonic(IN_4)
       set follow_clockwise, 1       //defines if it steers to left or right side
       set countto0, steer_counter   //defines the amount of time to steer to each side

       //check if ball is on sight
       ReadSensorUS(IN_4, distance)
       brcmp GT, go_on, distance, 50
       brcmp EQ, go_on, distance, 0
       jmp end

//start searching
go_on:

       OnRev(OUT_B, 100)
       //wait 100
       //sub countto0, countto0, 5
loop1:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check1, distance, 0
       brcmp LT, end, distance, 50
check1:
       brcmp GT, loop1, countto0, 0
       Off(OUT_BC)


       set countto0, steer_counter
       OnFwd(OUT_B, 100)
loop2:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check2, distance, 0
       brcmp LT, end, distance,50
check2:
       brcmp GT, loop2, countto0, 0
       Off(OUT_BC)


       set follow_clockwise, 0

//the other way now
       set countto0, steer_counter

       OnRev(OUT_C, 100)
       //wait 100
       //sub countto0, countto0, 5
loop3:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check3, distance, 0
       brcmp LT, end, distance, 50
check3:
       brcmp GT, loop3, countto0, 0
       Off(OUT_BC)


       set countto0, steer_counter
       OnFwd(OUT_C, 100)
loop4:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check4, distance, 0
       brcmp LT, end, distance,50
check4:
       brcmp GT, loop4, countto0, 0
       Off(OUT_BC)

       //if not in sight move forward and search again
       call move_forward
       jmp again
end:
       Off(OUT_BC)
       return
ends



subroutine goto_pad_leave_ball
goto_pad_leave_ball_again:

       Off(OUT_BC)

       getout found_ball_rotationsB, OUT_B, RotationCount
       getout found_ball_rotationsC, OUT_C, RotationCount

//anti align
       brcmp EQ moveC, follow_clockwise, 0
       brcmp EQ moveB, follow_clockwise, 1

moveB:
       RotateMotor(OUT_B, 100, 45)
       jmp end_align

moveC:
       RotateMotor(OUT_C, 100, 45)
       jmp end_align


end_align:
       //move till to find pad
       OnFwdReg(OUT_BC, 100, OUT_REGMODE_SYNC)
       SetSensorTouch(IN_1)
CheckSensor:
       ReadSensor(IN_1, Switch)
       brcmp EQ, CheckSensor, Switch, 0
       Off(OUT_BC)


       
//if it is at the right place move forward and leave the ball
       RotateMotor(OUT_BC, 100, -50)

       OnFwd(OUT_A, 10)
       wait 2000
       Off(OUT_A)
       
       return
      
       PlayTone(TONE_E5,5000)

       RotateMotor(OUT_BC, 100, -360)
       return
       
//this is if the pad isn't at the right place
align_again:

//go to initial position (after line_follow)
       call move_backward
       
       brcmp GT moveB_align, notfound_ball_rotationsB, notfound_ball_rotationsC
       brcmp GT moveC_align, notfound_ball_rotationsC, notfound_ball_rotationsB

moveB_align:
       OnRev(OUT_B, 100)
       getout notfound_ball_rotationsB, OUT_B, RotationCount
       brcmp GT moveB_align, notfound_ball_rotationsB, notfound_ball_rotationsC
       Off(OUT_BC)
       jmp now_align

moveC_align:
       OnRev(OUT_C, 100)
       getout notfound_ball_rotationsC, OUT_C, RotationCount
       brcmp GT moveC_align, notfound_ball_rotationsC, notfound_ball_rotationsB
       Off(OUT_BC)
       jmp now_align

       
now_align:

       getout notfound_ball_rotationsB, OUT_B, RotationCount
       getout notfound_ball_rotationsC, OUT_C, RotationCount



//----------------

       //search for pad as before
again:
       SetSensorUltrasonic(IN_4)
       set countto0, steer_counter

       ReadSensorUS(IN_4, distance)
       brcmp GT, go_on, distance, 50
       brcmp EQ, go_on, distance, 0
       jmp end


go_on:

       OnRev(OUT_B, 100)
       wait 20
loop1:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check1, distance, 0
       brcmp LT, end, distance, 50
check1:
       brcmp GT, loop1, countto0, 0
       Off(OUT_BC)
       wait 100


       set countto0, steer_counter
       OnFwd(OUT_B, 100)
loop2:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check2, distance, 0
       brcmp LT, end, distance,50
check2:
       brcmp GT, loop2, countto0, 0
       Off(OUT_BC)
       wait 100


       set countto0, steer_counter
       OnRev(OUT_C, 100)
loop3:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check3, distance, 0
       brcmp LT, end, distance,50
check3:
       brcmp GT, loop3, countto0, 0
       Off(OUT_BC)
       wait 100


       set countto0, steer_counter
       OnFwd(OUT_C, 100)
loop4:
       ReadSensorUS(IN_4, distance)
       sub countto0, countto0, 1

       brcmp EQ, check4, distance, 0
       brcmp LT, end, distance,50
check4:
       brcmp GT, loop4, countto0, 0
       Off(OUT_BC)
       wait 100

       call move_forward
       jmp again
 end:
       Off(OUT_BC)
       jmp goto_pad_leave_ball_again
       return

ends


//------------------------------------------------------------------------------

subroutine move_backward
       OnRevReg(OUT_BC, 100, OUT_REGMODE_SYNC)
       wait 400
       Off(OUT_BC)
       return
ends

subroutine get_out
       OnRevReg(OUT_BC, 100, OUT_REGMODE_SYNC)
       wait 2500
       Off(OUT_BC)
       return
ends