Robot.elm

module Main exposing (..)

import Exts.Float exposing (roundTo)
import Html exposing (..)
import Html.Attributes exposing (..)
import Html.Events exposing (on)
import Json.Decode as Decode
import Mouse exposing (Position)
import Keyboard exposing (..)
import Char


main =
    Html.program
        { init = init
        , view = view
        , update = update
        , subscriptions = subscriptions
        }



-- MODEL


type alias Model =
    { angle1 : Float
    , len1 : Float
    , angle2 : Float
    , len2 : Float
    }


init : ( Model, Cmd Msg )
init =
    ( Model 0 200 0 150, Cmd.none )



-- UPDATE


type Msg
    = Nop Char
    | Rotate1 Float
    | Rotate2 Float
    | Extend1 Float
    | Extend2 Float


update : Msg -> Model -> ( Model, Cmd Msg )
update msg model =
    ( updateHelp msg model, Cmd.none )


updateHelp : Msg -> Model -> Model
updateHelp msg model =
    case msg of
        Nop _ ->
            model

        Rotate1 delta ->
            { model | angle1 = model.angle1 + delta }

        Rotate2 delta ->
            { model | angle2 = model.angle2 + delta }

        Extend1 delta ->
            { model | len1 = model.len1 + delta }

        Extend2 delta ->
            { model | len2 = model.len2 + delta }



-- SUBSCRIPTIONS


subscriptions : Model -> Sub Msg
subscriptions model =
    Keyboard.downs kbDown


kbDown : KeyCode -> Msg
kbDown code =
    case Char.fromCode code of
        'A' ->
            Rotate1 -0.2

        'D' ->
            Rotate1 0.2

        'Q' ->
            Rotate2 -0.2

        'E' ->
            Rotate2 0.2

        -- 'W' ->
        --     Extend2 10
        -- 'S' ->
        --     Extend2 -10
        x ->
            Nop x



-- VIEW


(=>) =
    (,)


view : Model -> Html Msg
view model =
    let
        angle1 =
            model.angle1

        angle2 =
            model.angle1 + model.angle2

        ( x1, y1 ) =
            ( 400, 400 )

        ( x2, y2 ) =
            rotate ( x1, y1 ) angle1 model.len1

        ( x3, y3 ) =
            rotate ( x2, y2 ) angle2 model.len2

        ( a, b ) =
            calcAngles model.len1 model.len2 ( x3 - x1, y3 - y1 )

        ( px2, py2 ) =
            rotate ( x1, y1 ) a model.len1

        ( px3, py3 ) =
            rotate ( px2, py2 ) (a + b) model.len2
    in
        div
            [ style
                [ "background-color" => "#555"
                , "color" => "#fff"
                , "cursor" => "move"
                , "width" => "800px"
                , "height" => "800px"
                , "border-radius" => "4px"
                , "position" => "relative"
                , "display" => "flex"
                , "align-items" => "center"
                , "justify-content" => "center"
                ]
            ]
            [ pre
                [ style
                    [ "position" => "absolute"
                    , "left" => "0"
                    , "top" => "0"
                    ]
                ]
                [ text <| "A/D - rotate green arm (" ++ showFloat model.angle1 ++ " radians = " ++ showFloat (model.angle1 * 180 / pi) ++ "°)\n"
                , text <| "Q/E - rotate blue arm (" ++ showFloat model.angle2 ++ " radians = " ++ showFloat (model.angle2 * 180 / pi) ++ "°)\n"
                  -- , text <| "W/S - extend/retract blue arm (" ++ showFloat model.len2 ++ ")\n"
                , text <| "Elbow is at (" ++ showFloat x2 ++ ", " ++ showFloat y2 ++ ")\n"
                , text <| "Hand is at (" ++ showFloat x3 ++ ", " ++ showFloat y3 ++ ")\n"
                , text <| "Target (" ++ showFloat (x3 - x1) ++ ", " ++ showFloat (y3 - y1) ++ " => (" ++ showFloat (a * 180 / pi) ++ "°, " ++ showFloat (b * 180 / pi) ++ "°) -> (" ++ showFloat px3 ++ ", " ++ showFloat py3 ++ ")\n"
                ]
            , div
                [ style
                    [ "position" => "absolute"
                      -- , "left" => px x1
                      -- , "top" => px y1
                    , "left" => px x1
                    , "top" => px y1
                    , "width" => px model.len1
                    , "height" => "0px"
                    , "border-top" => "2px solid #3c8d2f"
                    , "border-bottom" => "2px solid #3c8d2f"
                    , "transform" => cssTransform [ cssRotate a ]
                    , "transform-origin" => "0 50%"
                    ]
                ]
                []
            , div
                [ style
                    [ "position" => "absolute"
                    , "left" => px px2
                    , "top" => px py2
                    , "width" => px model.len2
                    , "height" => "0px"
                    , "border-top" => "2px solid #2f6a8d"
                    , "border-bottom" => "2px solid #2f6a8d"
                    , "transform" => cssTransform [ cssRotate (a + b) ]
                    , "transform-origin" => "0 50%"
                    ]
                ]
                []
            , div
                [ style
                    [ "position" => "absolute"
                      -- , "left" => px x1
                      -- , "top" => px y1
                    , "left" => px x1
                    , "top" => px y1
                    , "width" => px model.len1
                    , "height" => "0px"
                    , "border-top" => "2px solid #3c8d2f"
                    , "border-bottom" => "2px solid #3c8d2f"
                    , "transform" => cssTransform [ cssRotate angle1 ]
                    , "transform-origin" => "0 50%"
                    ]
                ]
                []
            , div
                [ style
                    [ "position" => "absolute"
                    , "left" => px x2
                    , "top" => px y2
                    , "width" => px model.len2
                    , "height" => "0px"
                    , "border-top" => "2px solid #2f6a8d"
                    , "border-bottom" => "2px solid #2f6a8d"
                    , "transform" => cssTransform [ cssRotate angle2 ]
                    , "transform-origin" => "0 50%"
                    ]
                ]
                []
            , div
                [ style
                    [ "position" => "absolute"
                    , "left" => px (x3 - 10)
                    , "top" => px (y3 - 10)
                    , "width" => "20px"
                    , "height" => "20px"
                    , "border-radius" => "10px"
                    , "background-color" => "#8d2f2f"
                    , "transform" => cssTransform [ cssRotate angle2 ]
                    , "transform-origin" => "50% 50%"
                    ]
                ]
                []
            ]


cssTransform : List String -> String
cssTransform =
    String.join " " << List.reverse


cssTranslate : Float -> Float -> String
cssTranslate x y =
    "translate(" ++ toString x ++ "px, " ++ toString y ++ "px)"


cssRotate : Float -> String
cssRotate angle =
    "rotate(" ++ toString angle ++ "rad)"


px : Float -> String
px number =
    toString number ++ "px"


type alias Pos =
    ( Float, Float )


type alias Angle =
    Float


type alias Length =
    Float


rotate : Pos -> Angle -> Length -> Pos
rotate ( x0, y0 ) angle len =
    let
        dx =
            len * cos angle

        dy =
            len * sin angle
    in
        ( x0 + dx, y0 + dy )


showFloat : Float -> String
showFloat =
    toString << roundTo 2


calcAngles : Length -> Length -> Pos -> ( Angle, Angle )
calcAngles l1 l2 ( x, y ) =
    let
        aa =
            sqrt (2 * l1 * l1 * x * x + 2 * l2 * l2 * x * x + 2 * l1 * l1 * y * y + 2 * l2 * l2 * y * y - l1 * l1 * l1 * l1 - l2 * l2 * l2 * l2 + 2 * l1 * l1 * l2 * l2 - x * x * x * x - 2 * x * x * y * y - y * y * y * y)

        d1 =
            2 * l1 * x + l1 * l1 - l2 * l2 + x * x + y * y

        d2 =
            2 * l2 * x - l1 * l1 + l2 * l2 + x * x + y * y

        a =
            2 * atan ((aa + 2 * l1 * y) / d1)

        b =
            2 * atan ((2 * l2 * y - aa) / d2)
    in
        ( a, b - a )


target : Length -> Length -> Pos -> ( Angle, Angle )
target l1 l2 ( x, y ) =
    let
        a =
            acos (x / l1)

        b =
            acos ((x - l1 * cos a) / l2) - a

        untarget ( alpha, beta ) =
            let
                ( x1, y1 ) =
                    ( 0, 0 )

                ( x2, y2 ) =
                    rotate ( x1, y1 ) alpha l1

                ( x3, y3 ) =
                    rotate ( x2, y2 ) (alpha + beta) l2
            in
                ( x3, y3 )
    in
        minimumBy (\( alpha, beta ) -> squareDistance ( x, y ) (untarget ( alpha, beta ))) Nothing (Debug.log "choices" [ ( a, b ), ( a, -b ), ( -a, b ), ( -a, -b ) ])
            |> Maybe.withDefault ( a, b )


nearest : Pos -> List Pos -> Maybe Pos
nearest targetPoint points =
    minimumBy (squareDistance targetPoint) Nothing points


squareDistance : Pos -> Pos -> Float
squareDistance ( x1, y1 ) ( x2, y2 ) =
    let
        dx =
            x2 - x1

        dy =
            y2 - y1
    in
        dx * dx + dy * dy


minimumBy : (a -> Float) -> Maybe ( a, Float ) -> List a -> Maybe a
minimumBy f best xs =
    case xs of
        [] ->
            Maybe.map Tuple.first best

        a :: rest ->
            case best of
                Nothing ->
                    minimumBy f (Just ( a, f a )) rest

                Just ( b, fb ) ->
                    if f a < fb then
                        minimumBy f (Just ( a, f a )) rest
                    else
                        minimumBy f best rest