#!/usr/bin/env python3
# coding: utf-8
'''
Siltanosturi: Perusdynamiikka toimii, mutta taakka ei reagoi esteisiin.
- Siirrä vaunua vasen ja oikea nuolinäppäimillä
- Nosta tai laske kuormaa ylä- ja alanuolella
- Kytke kuorman korkeuden säätö päälle ja pois s-näppäimellä
'''
import pygame
import json
from math import cos, sin
from dxdt import fdxdt, Flukko
from rk4 import rk4
from opt_params import par

g = 9.81
Musta = (0, 0, 0)  # Mustassa ei ole mitään valoa
Valk = (255, 255, 255)  # valkoisessa on kaikenväristä valoa
Sin = (0, 0, 255)  # vain sinistä
Pun = (255, 0, 0)
Vihr = (0, 255, 0)
Kelt = (255, 255, 0)
Seina = (200, 200, 200)
Vaunuvari = (0, 200, 200)
Puskurivari = (250, 0, 100)
Kiskovari = (50, 200, 100)


# Kuorman paikka xy-koordinaatistossa vaijerin
# L ja kulman funktiona
def rw_xy(L, kulma):
    return (L * sin(kulma), -L * cos(kulma))


# Voimanuolien piirtäminen
# p0: alkupiste;
def nuoli(p0, Voima):
    (ix0, iy0) = p0
    scale = 10.0
    (Fx, Fy) = Voima
    p1 = (ix0 + scale * Fx, iy0 - scale * Fy)
    pygame.draw.line(screen, Vihr, p0, p1, 4)


# referenssivoima, jolla kuorma pysyisi paikallaan
def nuolir(p0, Voima):
    (ix0, iy0) = p0
    scale = 10.0
    (Fx, Fy) = Voima
    p1 = (ix0 + scale * Fx, iy0 - scale * Fy)
    pygame.draw.line(screen, Pun, p0, p1, 4)


# Pelin ja simuloinnin ajoitus
class cl_Ajat:
    def __init__(self):
        self.Nayttotaajuus = 48
        self.Dt = 1.0 / self.Nayttotaajuus


# Pelimaailman kuvaaminen näytölle peli-ikkunaan.
# Luokka Kamera periytyy toisesta ohjelmasta, jossa kuvakulmaa
# saattoi vaihtaa.
class Cl_Kamera:
    def __init__(self, Wxnaytto, Wynaytto):
        self.Wxnaytto = Wxnaytto
        self.Wynaytto = Wynaytto
        kuvasuhde = self.Wynaytto/self.Wxnaytto
        self.Wx = 20.0
        self.Wy = kuvasuhde * self.Wx
        self.Wx0 = 0.0
        self.Wy0 = -0.9 * self.Wy


# Lasketaan mitä pikseliä näytöllä vastaa pelimaailman piste.
# Oivallinen geometrian harjoitus miettiä, mihin tämä perustuu

    def xy_naytolla(self, xy):
        scale = self.Wxnaytto / self.Wx
        (x, y) = xy
        xpikseli = int((x - self.Wx0) * scale)
        ypikseli = int(self.Wynaytto - (y - self.Wy0) * scale)
        return (xpikseli, ypikseli)

    def xy_skaalaus(self, w, h):
        scale = self.Wxnaytto / self.Wx
        xpikseli = int(w * scale)
        ypikseli = int(h * scale)
        return (xpikseli, ypikseli)

    def skaalaus(self, x):
        scale = self.Wxnaytto / self.Wx
        xpikseli = int(x * scale)
        return xpikseli


# piirretään muutama laatikko markkeeraamaan taloja, joiden katolle
# kuorma pitäisi laskea
# Ohjelmoimatta: Kuorman ja rakennusten vuorovaikutus
class Cl_tyomaa:
    def __init__(self, Talot):
        self.talot = Talot

    def piirra(self):
        for talo in self.talot:
            topleft = Kamera.xy_naytolla(talo)
            w = Kamera.skaalaus(1.0)
            h = Kamera.skaalaus(4.0)
            dim = (w, h)
            pygame.draw.rect(screen, Seina, (topleft, dim), 0)


# Siltanosturin vaunu. Kulkee katonrajassa vaakasuoraa kiskoa pitkin
class Cl_vaunu:
    def __init__(self):
        self.M = 1.0  # Vaunun massa
        self.x = 15.0  # 0.1*Kamera.Wx  # vaunun paikka
        self.v = 0.0  # vaunun nopeus
        self.F = 0.0  # vaunua liikuttava voima
        self.Cf = par.cfr_car  # kitkakerroin

    def piirra(self):
        (xi, yi) = Kamera.xy_naytolla((self.x, 0.0))
        pygame.draw.line(screen, Vaunuvari, (xi - 20, yi), (xi + 20, yi), 20)
        nuoli((xi, yi), (self.F, 0.0))
        (xb0, yb0) = Kamera.xy_naytolla((0.1 * Kamera.Wx, 0.0))
        (xb1, yb1) = Kamera.xy_naytolla((0.9 * Kamera.Wx, 0.0))
        pygame.draw.line(screen, Puskurivari, (0, yi - 5),
                         (xb0 - 20, yi - 5), 40)
        pygame.draw.line(screen, Puskurivari, (xb1 + 20, yi - 5),
                         (Wxnaytto, yi - 5), 40)
        pygame.draw.line(screen, Kiskovari, (0, yi + 10),
                         (Wxnaytto, yi + 10), 10)

# Taakse/vasemmalle

    def taakse(self):
        self.F = -5.0
        # print(self.F)

    def eteen(self):
        self.F = 5.0
        # print(self.F)

# Kiskon päissä on joustavat puskurit,
# jotka työntävät vaunua poispäin puskurista
# Puskuritkin voisi piirtää

    def puskurit(self):
        dx1 = self.x - 19.0  # 0.9*Kamera.Wx
        dx2 = -self.x + 1.0  # 0.1*Kamera.Wx
        if dx1 > 0:
            self.F = -20.0 * dx1
        elif dx2 > 0:
            self.F = 20.0 * dx2


class Cl_kuorma:
    def __init__(self):
        self.M = 3.0  # Massa
        self.theta = 0.0  # vaijerin kulma
        self.w = 0.0  # vaijerin kulman muutosnopeus
        self.Lr = 4.0  # vaijerin pituuden asetusarvo
        self.L = self.Lr  # vaijerin pituus
        self.Lv = 0.0  # vaijerin pituuden muutosnopeus
        self.F0 = 9.81 * self.M
        self.F = self.F0  # vaijerin jännitys
        self.Cf = par.cfr_load  # ilmanvastuksen kerroin
        self.Ckela = par.cfr_kela
        self.lukittu = True

# Piirretään kuorma, vaijeri ja vaijerin pituuden asetusarvo

    def piirra(self):
        (x, y) = rw_xy(self.L, self.theta)
        (xi, yi) = Kamera.xy_naytolla((x + vaunu.x, y))
        pygame.draw.circle(screen, Kelt, (xi, yi), 10, 0)
        (rxi, ryi) = Kamera.xy_naytolla(
            (x + vaunu.x, -self.Lr * cos(self.theta)))
        if Saato:
            pygame.draw.line(screen, Vihr, (rxi - 50, ryi), (rxi + 50, ryi), 1)
        vp = Kamera.xy_naytolla((vaunu.x, 0.0))
        pygame.draw.line(screen, Sin, vp, (xi, yi), 4)
        F = rw_xy(-self.F, self.theta)
        F0 = rw_xy(-self.F0, self.theta)
        nuolir((xi + 2, yi), F0)
        nuoli((xi - 2, yi), F)

# lasketaan asetusarvoa

    def alas(self):
        self.lukittu = False
        if Saato:
            self.Lr += 0.02
        else:
            self.F -= 0.1

    def ylos(self):
        self.lukittu = False
        if Saato:
            self.Lr -= 0.02
            if self.Lr > 2.0:
                self.Lr -= 0.02
        else:
            self.F += 0.1

# Kuormaa oli hyvin vaikea pysäyttää tietylle korkeudelle voimaa säätelemällä
# Kun F = F0 ja vaunu täysin levossa, kuorma pysähtyy, mutta muuten se
# vaeltaa ylös tai alas.
# Tein yksinkertaisen takaisinkytketyn säädön pysäyttämään kuorma halutulle
# korkeudelle
# F0 on hyvä arvaus voimalle. Korjataan sitä vaijerin pituuden poikkeamaan
# verrannollisesti . Korjataan vielä nopeuteen verrannollisesti.
# Säätötekniikka on oma vaativa tieteenalansa, mutta kokeilemalla pääsee alkuun
# Ehkä kannattaisi tehdä säätimestä sellainen, että sitä voisi virittää
# on-line
# Säätimestä voisi tehdä sellaisenkin, että se pyrkii pitämään kuorman
# vakiokorkeudella.

    def saato(self):
        self.F = self.F0 - 5.0 * (self.Lr - self.L) + 2.0 * self.Lv


#        self.F = min(1.5*self.F0, max(0.5*self.F0, F))


class Cl_opt:
    def __init__(self):
        with open('uopt.dat', 'r') as fil:
            self.uu_opt = json.load(fil)
        self.otim = -50
        self.maxotim = len(self.uu_opt)
        self.opti = False
        vaunu.x = 3.0
        vaunu.v = 0.0
        vaunu.F = 0.0
        kuorma.theta = 0.0  # vaijerin kulma
        kuorma.w = 0.0  # vaijerin kulman muutosnopeus
        kuorma.Lr = 8.0  # vaijerin pituuden asetusarvo
        kuorma.L = kuorma.Lr  # vaijerin pituus
        kuorma.Lv = 0.0  # vaijerin pituuden muutosnopeus
        kuorma.F = kuorma.F0

    def optu(self):
        if (self.otim <= self.maxotim + 10):
            self.otim = self.otim + 1
        if (self.otim >= 0 and self.otim < self.maxotim):
            [F_vaunu, F_kuorma] = self.uu_opt[self.otim]
            vaunu.F = F_vaunu
            kuorma.F = F_kuorma
        if (self.otim >= self.maxotim):
            vaunu.F = 0.0
            kuorma.F = kuorma.F0
        if (self.otim > self.maxotim + 10):
            self.opti = False
            kuorma.Lr = kuorma.L


# Ohjelmoin yleispätevän Runge-Kutta algoritmin erilliseksi moduliksi rk4.py
# sille annetaan yhdeksi parametriksi järjestelmän tilamuuttujien derivaatat
# laskevan funktion nimi (dxdt). Systeemiyhtälöt johdetaan ja funktio dxdt
# generoidaan sympyllä.
def liike():
    yy = [vaunu.v, kuorma.w, kuorma.Lv, vaunu.x, kuorma.theta, kuorma.L]
    uu = [
        vaunu.F, kuorma.F, vaunu.M, kuorma.M,
        vaunu.Cf, kuorma.Cf, kuorma.Ckela
    ]
    if (kuorma.lukittu):
        uu[1] = Flukko(yy, uu)
        kuorma.F = uu[1]
        yy[2] = 0.9 * yy[2]
    yy = rk4(fdxdt, yy, uu, Aika.Dt)
    [vaunu.v, kuorma.w, kuorma.Lv, vaunu.x, kuorma.theta, kuorma.L] = yy


# ====================================================================
#
# Pääohjelma alkaa tästä
# ====================================================================

pygame.display.init()

# Selvitetään peli-ikkunan koko näytöllä pikseleinä
D_Info = pygame.display.Info()
Wxnaytto = D_Info.current_w
Wynaytto = D_Info.current_h

screen = pygame.display.set_mode((Wxnaytto, Wynaytto))
pygame.display.set_caption("Nosturi")
pygame.init()

clock = pygame.time.Clock()

Kamera = Cl_Kamera(Wxnaytto, Wynaytto)
Aika = cl_Ajat()
vaunu = Cl_vaunu()
kuorma = Cl_kuorma()
tyomaa = Cl_tyomaa([(2.5, -8.2), (14.5, -4.2)])
ohj = Cl_opt()
loppu = False
MuistaVanhat = False
Saato = False

font_ohj = pygame.font.SysFont('Arial', 22)

ohje = "nuolinäppäimet: liiku; s: korkeuden säätö; o: optimiohjaus; \
l: lukitse korkeus (aukeaa ylä/ala-nuolella); F1: näytä liikerata"
rend_ohje = font_ohj.render(ohje, True, (0, 255, 0), (0, 0, 0))
oix = int(0.2 * Wxnaytto)
oiy = int(0.9 * Wynaytto)

while not loppu:
    vaunu.F = 0.0
    if not MuistaVanhat:
        screen.fill(Musta)  # pyyhitään peli-ikkuna tyhjäksi
    screen.blit(rend_ohje, (oix, oiy))
    for event in pygame.event.get():
        # Lopetetaan peli sulkemalla ikkuna tai painamalla ESC
        if event.type == pygame.QUIT:
            loppu = True
        if event.type == pygame.KEYDOWN:
            if event.key == pygame.K_F1:
                MuistaVanhat = not MuistaVanhat
            if event.key == pygame.K_s:
                Saato = not Saato
            if event.key == pygame.K_o:
                Saato = False
                kuorma.lukittu = False
                ohj.opti = True
                # ohj.init_opt()
            if event.key == pygame.K_l:
                kuorma.lukittu = True
                Saato = False
            if event.key == pygame.K_ESCAPE:
                loppu = True

    pressed = pygame.key.get_pressed()
    if pressed[pygame.K_UP]:
        kuorma.ylos()
    if pressed[pygame.K_DOWN]:
        kuorma.alas()
    if pressed[pygame.K_LEFT]:
        vaunu.taakse()
    if pressed[pygame.K_RIGHT]:
        vaunu.eteen()

    vaunu.puskurit()
    if Saato:
        kuorma.lukittu = False
        kuorma.saato()
    if ohj.opti:
        ohj.optu()
    liike()
    vaunu.piirra()
    kuorma.piirra()
    tyomaa.piirra()

    pygame.display.flip()
    clock.tick(Aika.Nayttotaajuus)

pygame.quit()